// reduce to 12 bit
word value12Bit = (register_value & data_mask)/4*4;
// return register_value & data_mask; // Return the data, stripping the non data (e.g parity) bits
return value12Bit; // Return the data, stripping the non data (e.g parity) bits
// return register_value & data_mask; // Return the data, stripping the non data (e.g parity) bits
The stepper still seem to be working fine in my test. I was still able to achieve fast movements in close loop position: 45 degrees (stop to stop) in less than 66 ms. YouTube video link - 6s.
So the resolution doesn’t seem to be a problem.
Regarding the linearity of the encoder - I suppose it would be possible to determine the non-linearity with the stepper in open loop and correct for this? I’m thinking that something like a 2 parameter fit will probably do wonders.
And simply applied that within the StepperMotor class.
Works great.
Here is the main code that does the work:
// zero electric angle not known
if(!_isset(zero_electric_angle)){
// Move 1 shaft rotation open loop (pole_pairs * 2 pi electric angle)
// create a look up table with a offset for each polepair
float sum[pole_pairs] = {};
int count[pole_pairs] = {};
for (int pp_i = 0; pp_i < pole_pairs; pp_i++) {
// for every pole we want ~ 20 measurements;
for ( int measure_i = 0; measure_i < 20; measure_i++) {
// for every measurement we want to take 20 steps to get to the next one
float angle = 0;
for (int i = 0; i <=20; i++ ) {
angle = _3PI_2 + _2PI * (measure_i * 20 + i) / 400.0f;
setPhaseVoltage(voltage_sensor_align, 0, angle);
_delay(1);
}
_delay(2);
sensor->update();
float mechAngle = sensor->getMechanicalAngle();
float openLoopAngle = _normalizeAngle(angle);
float encoderElectricAngle = electricalAngle(); // from encoder)
float angleOffset = _normalizeAngle(encoderElectricAngle - openLoopAngle);
monitor_port->println(angleOffset);
int bin = mechAngle * pole_pairs / _2PI;
sum[bin] += angleOffset;
count[bin] += 1;
}
}
for (int pp_i = 0; pp_i < pole_pairs; pp_i++) {
zero_offset_nonlin[pp_i] = sum[pp_i]/count[pp_i];
}
if(monitor_port) {
for (int pp_i = 0; pp_i < pole_pairs; pp_i++) {
monitor_port->print(F("bin: "));
monitor_port->print(pp_i);
monitor_port->print(F(" sum: "));
monitor_port->print(sum[pp_i]);
monitor_port->print(F(" count: "));
monitor_port->print(count[pp_i]);
monitor_port->print(F(" mean: "));
monitor_port->println(zero_offset_nonlin[pp_i]);
}
}
zero_electric_angle = 0;
// stop everything
setPhaseVoltage(0, 0, 0);
_delay(200);
}else if(monitor_port) monitor_port->println(F("MOT: Skip offset calib."));
and within LoopFOC
// encoder offset
float nonLinOffset = zero_offset_nonlin[(int) (sensor->getMechanicalAngle() * pole_pairs / _2PI)];
// Needs the update() to be called first
// This function will not have numerical issues because it uses Sensor::getMechanicalAngle()
// which is in range 0-2PI
electrical_angle = electricalAngle() - nonLinOffset;
Ideally the offset should values should be transferred to the Sensor and not stored in the Stepper as it’s really the Sensor that’s wrong and not the electric poles…
I’ll see if I can find some time to try an implementation based on your code… I have to think about the full rotation counting, and if there aren’t some tricky edge cases around the 0-crossing.
I have to think about the full rotation counting, and if there aren’t some tricky edge cases around the 0-crossing.
If you always apply the correction just after reading the sensor, then I can’t see it should make a difference (I might of cause have overlooked something).
They way I organised the bins is such that the measured shaft angle determines the bin / correction to apply.
One could imagine that jumping to the next bin would change the offset such that in fact the corrected measurement would jump backward. (especially for a high resolution encoder).
This is equivalent to sensor noise and shouldn’t throw off the rotation counting algorithm.
The maximum difference in the bins are (with my encoder) is approximately 0.05 shaft degree (0.05 rad *180/pi/ 50 poles) equivalent to a resolution of 1/0.05 = 7200. So the backwards jump will happen with a 2^14 encoder (16384).
Still it should not be a problem.
Also I decided that it would be best to have a number of bins equal to the number of poles as the offset angle varies a lot with the electric angle… Using the same amount of poles made it easier to mean the results.
Still using the calibration method mentioned above. I don’t think you can get anymore sensored torque for your buck:
0.4 Nm = 4$ driver, $3 encoder, $12 stepper
// zero electric angle not known
if(!_isset(zero_electric_angle)){
// Move 1 shaft rotation open loop (pole_pairs * 2 pi electric angle)
// create a look up table with a offset for each polepair
float sum[pole_pairs] = {};
int count[pole_pairs] = {};
for (int pp_i = 0; pp_i < pole_pairs; pp_i++) {
// for every pole we want ~ 20 measurements;
for ( int measure_i = 0; measure_i < 20; measure_i++) {
// for every measurement we want to take 20 steps to get to the next one
float angle = 0;
for (int i = 0; i <=20; i++ ) {
angle = _2PI * (measure_i * 20 + i) / 400.0f;
setPhaseVoltage(voltage_sensor_align, 0, angle);
_delay(1);
}
_delay(2);
sensor->update();
float sensorAngle = sensor->getAngle() * pole_pairs * sensor_direction;
float openLoopAngle = pp_i * _2PI + angle;
float angleOffset = sensorAngle - openLoopAngle;
monitor_port->println(angleOffset);
int bin = sensor->getMechanicalAngle() * pole_pairs / _2PI;
sum[bin] += angleOffset;
count[bin] += 1;
}
}
for (int pp_i = 0; pp_i < pole_pairs; pp_i++) {
zero_offset_nonlin[pp_i] = _normalizeAngle(sum[pp_i]/count[pp_i] - _PI_2);
}
if(monitor_port) {
for (int pp_i = 0; pp_i < pole_pairs; pp_i++) {
monitor_port->print(F("bin: "));
monitor_port->print(pp_i);
monitor_port->print(F(" sum: "));
monitor_port->print(sum[pp_i]);
monitor_port->print(F(" count: "));
monitor_port->print(count[pp_i]);
monitor_port->print(F(" mean: "));
monitor_port->println(zero_offset_nonlin[pp_i]);
}
}
zero_electric_angle = 0;
// stop everything
setPhaseVoltage(0, 0, 0);
_delay(200);
}else if(monitor_port) monitor_port->println(F("MOT: Skip offset calib."));
return exit_flag;
There is a small update to the algorithm. The calibration didn’t work before when the offset would wrap around / cross 2PI. Also improved the calibration in cases when the sensor direction is anticlockwise.