Experimenting with zero_electric_angle computation

For those interested in possible extensions to SimpleFOC, here is a function that uses several methods to compute the zero_electric_angle. I developed it as a learning tool, so there might be bugs.

#include <vector>
#include <sstream>
#include <string>

//------------------------------------------------------------------------------

// Class to average a value and display it
class Avg {
 private:
  float _sum = 0;
  float _min = MAXFLOAT;
  float _max = -MAXFLOAT;
  int _count = 0;

 public:
  void push(float value) {
    _min = std::min(_min, value);
    _max = std::max(_max, value);
    _sum += value;
    ++_count;
  }

  void display(const char* name) {
    const float avg = _sum / _count;
    std::stringstream s;
    s.precision(3);
    s << "zero_electric_angle avg" << name << "=" << avg << " (min=" << _min
      << ", max=" << _max << ")";
    SIMPLEFOC_DEBUG(s.str().c_str());
  }
};

//------------------------------------------------------------------------------

// Trapezoidal map from the SimpleFOC library
// Each step is 30 electrical degrees with values for 3 phases of 1, -1, 0
extern int trap_150_map[12][3];

//------------------------------------------------------------------------------

// Compute zero_electric_angle with various methods
void testZeroAngles(BLDCMotor& m) {
  // Number of angular steps we will use for each pole pair
  static constexpr int STEP_COUNT_PER_PP = 12;

  // Number of sensor readings we'll average for each step
  static constexpr int SENSOR_AVG_COUNT = 100;

  // Przpare an array to store results
  std::vector<float> zeroElectricalAngles;

  // Pre-compute the center phase voltage
  const float center = m.modulation_centered ? (m.driver->voltage_limit) / 2
                                             : m.voltage_sensor_align;

  // Iterate over pole pairs and electrical steps. This performs a full
  // mechanical revolution.
  for (int pp = 0; pp < m.pole_pairs; ++pp) {
    for (int elecStep = 0; elecStep < STEP_COUNT_PER_PP; ++elecStep) {
      // Compute the electrical angle corresponding to this step
      const float currentElectricalAngle =
          elecStep * m.sensor_direction * _2PI / STEP_COUNT_PER_PP;

      // Set the phases to the right value
      m.driver->setPwm(
          center + (float)trap_150_map[elecStep][0] * m.voltage_sensor_align,
          center + (float)trap_150_map[elecStep][1] * m.voltage_sensor_align,
          center + (float)trap_150_map[elecStep][2] * m.voltage_sensor_align);
      _delay(700);

      // Get the sensor value
      float sensorAngleRad = 0;
      for (int i = 0; i < SENSOR_AVG_COUNT; ++i) {
        m.sensor->update();
        sensorAngleRad += m.sensor->getMechanicalAngle();
      }
      sensorAngleRad /= SENSOR_AVG_COUNT;

      // Compute the same value in electrical unit
      float sensorAngleInElectricalUnit = _normalizeAngle(
          (float)(m.sensor_direction * m.pole_pairs) * sensorAngleRad);

      // Compute and store the zero electrical angle
      const float zeroElectricalAngle = _normalizeAngle(
          sensorAngleInElectricalUnit - currentElectricalAngle + _3PI_2);
      zeroElectricalAngles.push_back(zeroElectricalAngle);

      // Display something, so that we know it's alive
      SIMPLEFOC_DEBUG("zero_electric_angle=", zeroElectricalAngle);
    }
  }

  // Stop the motor
  m.setPhaseVoltage(0, 0, 0);

  // We'll display a lot of stuff
  SIMPLEFOC_DEBUG("\n****** Test Zero Angle ******");

  // Display the zero_electric_angle as computed by SimpleFOC
  SIMPLEFOC_DEBUG("\n1. Zero electrical angle as computed by SimpleFOC");
  SIMPLEFOC_DEBUG("zero_electric_angle = ", m.zero_electric_angle);

  // Display the average 6 steps zero_electric_angle over a single electrical
  // revolution
  SIMPLEFOC_DEBUG(
      "\n2. Zero electrical angle using 6 steps average (single electrical "
      "revolution)");
  Avg avg;
  for (int i = 0; i < STEP_COUNT_PER_PP / 2; ++i) {
    avg.push(zeroElectricalAngles[i * 2]);
  }
  avg.display("6");

  // Display the average 6 intermediate steps zero_electric_angle over a single
  // electrical revolution
  SIMPLEFOC_DEBUG(
      "\n3. Zero electrical angle using 6 intermediate steps average (single "
      "electrical revolution)");
  avg = Avg();
  for (int i = 0; i < STEP_COUNT_PER_PP / 2; ++i) {
    avg.push(zeroElectricalAngles[1 + i * 2]);
  }
  avg.display("6-i");

  // Display the average 12 steps zero_electric_angle over a single electrical
  // revolution
  SIMPLEFOC_DEBUG(
      "\n4. Zero electrical angle using 12 steps average (single electrical "
      "revolution)");
  avg = Avg();
  for (int i = 0; i < STEP_COUNT_PER_PP; ++i) {
    avg.push(zeroElectricalAngles[i]);
  }
  avg.display("12");

  // Display the average 6 steps zero_electric_angle over a full mechanical
  // revolution
  SIMPLEFOC_DEBUG(
      "\n5. Zero electrical angle using 6 steps average (full mechanical "
      "revolution)");
  avg = Avg();
  for (int i = 0; i < zeroElectricalAngles.size() / 2; ++i) {
    avg.push(zeroElectricalAngles[i * 2]);
  }
  avg.display("6xPP");

  // Display the average 12 steps zero_electric_angle over a full mechanical
  // revolution
  SIMPLEFOC_DEBUG(
      "\n6. Zero electrical angle using 12 steps average (full mechanical "
      "revolution)");
  avg = Avg();
  for (auto value : zeroElectricalAngles) {
    avg.push(value);
  }
  avg.display("12xPP");
}

//------------------------------------------------------------------------------

The testZeroAngles(motor) function should be used after SimpleFOC initialization in closed loop mode, for example at the end of your setup() function. It will make your motor rotate a full revolution, computing the zero_electric_angle for each of the [12 x pole_pairs] steps, then display some statistics.

Here is the output with my motor:

****** Test Zero Angle ******

1. Zero electrical angle as computed by SimpleFOC
zero_electric_angle = 3.84

2. Zero electrical angle using 6 steps average (single electrical revolution)
zero_electric_angle avg6=3.66 (min=3.62, max=3.7)

3. Zero electrical angle using 6 intermediate steps average (single electrical revolution)
zero_electric_angle avg6-i=3.66 (min=3.61, max=3.73)

4. Zero electrical angle using 12 steps average (single electrical revolution)
zero_electric_angle avg12=3.66 (min=3.61, max=3.73)

5. Zero electrical angle using 6 steps average (full mechanical revolution)
zero_electric_angle avg6xPP=3.69 (min=3.57, max=3.82)

6. Zero electrical angle using 12 steps average (full mechanical revolution)
zero_electric_angle avg12xPP=3.68 (min=3.56, max=3.83)

Key takeways:

  • It is imprecise to compute zero_electric_angle on a single step as done by SimpleFOC, as values vary across steps (see for example the above output 2, where results vary from 3.62 to 3.7).
  • It is imprecise to compute zero_electric_angle on a single electrical revolution as done by SimpleFOC, as values vary across pole pairs (see for example the above output 5, where results vary from 3.57 to 3.82).
  • Intermediate PI/12 steps looks a bit less precise that full PI/6 steps (see for example the min-max range difference between outputs 5 and 6). This confirms on my motor what @dekutree64 said here and here, although in a less dramatic way. If this is confirmed, then SimpleFOC shouldn’t use 3PI/2 as a single reference step, as 3PI/2 is an intermediate step.

Please let me know your thoughts.

3 Likes