I’m not sure what I had seen before, but my linear halls are not as accurate as I thought, getting ±6 units of random variation on my 1/6400 scale (similar to milli-radians). The measured error when holding with nocascade PID is within a couple units, but that’s because it’s following the noise, which is why it vibrates a bit.
I experimented with deadband to make it relax when holding still, and was able to get it working pretty well. To reduce the effect of random noise, I only switch between idle and active if the position has been consistently in or out of the deadband zone for a certain amount of time (less than a millisecond). And only enter idle mode if the PID output has been below an adjustable threshold during that time, so it will hold against a continuously applied torque rather than repeatedly relaxing and then jolting back. It reliably comes to rest with the deadband zone set to ±0.005 radians, which works out to ±3 micrometers with SFU1204 ballscrews. But that feels surprisingly large when turning the shaft by hand, so I think I’ll set it up so deadband is only enabled if the spindle is turned off, since I’m only really concerned with the vibration if I leave the machine sitting idle for hours.
float error = shaft_angle_sp - shaft_angle;
// History keeps track of whether the shaft angle was inside or outside the deadband zone.
// If it's consistently inside (and PID is not exerting significant effort to hold position), the motor enters idle mode.
// If it's consistently outside, the motor exits idle mode. Otherwise it continues what it was doing.
uint32_t history_mask = (1<<angle_deadband_history_length)-1;
angle_deadband_history = (angle_deadband_history << 1) & history_mask; // Discard oldest entry to make room for new
if(fabs(error) >= angle_deadband)
angle_deadband_history |= 1;
if(angle_deadband_history == 0)
angle_deadband_active = true;
else if(angle_deadband_history == history_mask)
angle_deadband_active = false;
if(angle_deadband_active) {
current_sp = 0;
P_angle.timestamp_prev = _micros(); // Trick PID into thinking no time has passed while idle
}
else {
current_sp = P_angle(error);
if (fabs(current_sp) >= angle_deadband_output_threshold)
angle_deadband_history |= 1; // If it's taking some effort to hold position, don't idle
}
A little bit of angle lowpass (Tf=0.0005 or 0.001) smooths out the noise a lot and mostly eliminates the need for the deadband history, but I’m not sure if that would introduce any discernible discrepancies with synchronized axis motion. I suppose if all of them are delayed equally it would probably be ok.
This would be a usable setup, giving me about ±5 micrometer precision. But I remembered I bought two AS5048A years ago and never used them, so I decided to try that out to see if it’s significantly better, and indeed it is. The holding vibration is barely perceptible, and deadband can be set to ±0.001. But I don’t have a proper diametrically magnetized disc for it, and it has significant nonlinearity with the magnet shown in my earlier post. The diagnostic info doesn’t say the field strength is out of range, so I think it’s mostly due to not being perfectly centered on the shaft.
I tried using CalibratedSensor on it, but it still didn’t run well, so I decided to write my own CalibratedAS5048A that works with the raw integer angle, and that works much better. I had a heck of a time figuring out the proper math to get all the angle wrapping right, and the sensor gives occasional bad readings so I had to filter that out too.
#ifndef CALIBRATED_AS5048A_H
#define CALIBRATED_AS5048A_H
class CalibratedAS5048A: public MagneticSensorAS5048A {
public:
CalibratedAS5048A(int cs) : MagneticSensorAS5048A(cs) {}
void init(int _lut_resolution, const uint16_t *_lut, SPIClass* _spi = &SPI)
{
this->MagneticSensorAS5048A::init(_spi);
lut_resolution = _lut_resolution;
lut = _lut;
}
// Perform full calibration. The table passed in here will be filled with data and its pointer retained.
// It should have space for lut_resolution+1 entries (the extra 1 simplifies interpolation).
void init(FOCMotor *motor, int _lut_resolution, uint16_t *_lut, SPIClass* _spi = &SPI)
{
int i, j;
uint16_t table[_lut_resolution*4];
this->MagneticSensorAS5048A::init(_spi);
lut_resolution = _lut_resolution;
lut = _lut;
Serial.println("CalibratedAS5048A: Begin calibration movement");
// Rotate motor forward and back, recording the sensor values at each step.
// Move slightly negative, so first loop iteration moves forward like subsequent iterations
motor->setPhaseVoltage(motor->voltage_sensor_align, 0, _electricalAngle(-1 * _2PI / lut_resolution, motor->pole_pairs));
delay(2000/lut_resolution);
for (j = 0; j < 2; j++)
for (i = 0; i < lut_resolution; i++) {
uint32_t time = 3000000 / lut_resolution, start_time = _micros();
while(_micros() < start_time + time) {
float a = (i - 1) + (float)(_micros() - start_time) / time;
motor->setPhaseVoltage(motor->voltage_sensor_align, 0, _electricalAngle(a * _2PI / lut_resolution, motor->pole_pairs));
}
uint16_t raw = readRawAngle();
// Scale so overall range is 0-65535
table[j*lut_resolution+i] = (raw << 2) + (raw >> 12);
}
// Complete previous revolution, so first loop iteration moves backward like subsequent iterations
motor->setPhaseVoltage(motor->voltage_sensor_align, 0, 0);
delay(2000/lut_resolution);
for (j = 0; j < 2; j++)
for (i = lut_resolution - 1; i >= 0; i--) {
uint32_t time = 3000000 / lut_resolution, start_time = _micros();
while(_micros() < start_time + time) {
float a = (i + 1) - (float)(_micros() - start_time) / time;
motor->setPhaseVoltage(motor->voltage_sensor_align, 0, _electricalAngle(a * _2PI / lut_resolution, motor->pole_pairs));
}
uint16_t raw = readRawAngle();
table[(2+j)*lut_resolution+i] = (raw << 2) + (raw >> 12);
}
// Average the samples by adding half the signed difference so it behaves correctly near wraparound
for (i = 0; i < lut_resolution; i++) {
uint16_t temp1 = table[lut_resolution*0+i] + (((int16_t)(table[lut_resolution*1+i] - table[lut_resolution*0+i])) >> 1);
uint16_t temp2 = table[lut_resolution*2+i] + (((int16_t)(table[lut_resolution*3+i] - table[lut_resolution*2+i])) >> 1);
table[i] = (uint16_t)(temp1 + (((int16_t)(temp2 - temp1)) >> 1));
}
table[lut_resolution] = table[0]; // Extra for interpolation
//Serial.print("const uint16_t table[] = {"); delayMicroseconds(200);
//printTable(table, lut_resolution);
//Serial.print("\n};\n\n"); delayMicroseconds(200);
for (i = 0; i < lut_resolution; i++) {
uint16_t a = (int16_t)(((int32_t)i << 16) / lut_resolution);
for (j = 0; j < lut_resolution; j++) {
int32_t smaller, larger;
if(table[j] < table[j+1]) smaller = table[j], larger = table[j+1];
else larger = table[j], smaller = table[j+1];
// If entries are on opposite sides of the wrap point, wrap one or the other
if (smaller < 1000 && larger > 60000) {
if (a < 32768) larger -= 65536; else smaller += 65536;
larger += smaller; smaller = larger - smaller; larger -= smaller; // Swap
}
if (smaller <= a && a <= larger) {
int32_t angle = (int32_t)j << 16; // Integer portion of shaft position for this sensor value
angle += (((int32_t)((int16_t)(a - table[j]))) << 16) / (int16_t)(table[j + 1] - table[j]); // Fractional portion
angle /= lut_resolution; // Range is now 0-65535 for one revolution
_lut[i] = (uint16_t)angle;
}
}
}
// Set extra entry at end of table, used for interpolation
_lut[lut_resolution] = _lut[0];
Serial.println("CalibratedAS5048A: Calibration complete");
printCalibration();
}
// Helper function to look up shaft angle corresponding to sensor value
uint16_t InterpolatedLookup(const uint16_t *lut, uint16_t val, uint16_t range) const {
int idx = (uint32_t)val * lut_resolution / range;
int frac = (uint32_t)val * lut_resolution % range;
return (uint16_t)((int32_t)lut[idx] + ((int32_t)((int16_t)(lut[idx+1] - lut[idx])) * frac / range));
}
float getSensorAngle() override {
uint16_t oldRaw = beforeLastRaw;
beforeLastRaw = lastRaw;
lastRaw = readRawAngle();
// If the new reading is significantly different from the last two, it's probably bad.
if (abs((int16_t)((lastRaw - beforeLastRaw)<<2)) > 500 && abs((int16_t)((lastRaw - oldRaw)<<2)) > 500)
return (float)lastCal * (_2PI/65536.0f);
int32_t temp = (uint32_t)lastRaw * lut_resolution;
const uint16_t *t = lut + (temp >> 14);
lastCal = (uint16_t)((int32_t)t[0] + ((int32_t)((int16_t)(t[1] - t[0])) * (temp & 0x3fff) >> 14));
return (float)lastCal * (_2PI/65536.0f);
}
static void printTable(const uint16_t *table, int num) {
for (int i = 0; i < num; i++) {
if(!(i & 7)) { Serial.print("\n\t"); delayMicroseconds(200); }
Serial.print(table[i]); delayMicroseconds(200);
if(i != num - 1) { Serial.print(", "); delayMicroseconds(200); }
}
}
// Print data to serial so it can be copy/pasted into the code to avoid re-calibrating every startup.
void printCalibration() const {
Serial.print("const uint16_t lutAS5048A["); delayMicroseconds(200);
Serial.print(lut_resolution); delayMicroseconds(200);
Serial.print("+1] = { // Extra entry at end for interpolation"); delayMicroseconds(200);
printTable(lut, lut_resolution+1);
Serial.print("\n};\n"); delayMicroseconds(200);
}
const uint16_t *lut=NULL; // Table has one extra entry on the end to simplify interpolation code
int lut_resolution=0; // Number of entries in lookup table, excluding the extra for interpolation
uint16_t lastRaw=0, beforeLastRaw=0, lastCal=0; // Last sensor reading
};
#endif
Now I have to decide, is it worth $25 for a few more micrometers of precision on X and Y, or should I stick with linear halls and keep these fancy sensors in my arsenal for future projects?
I also did a little testing with foc_current mode, but voltage mode seems to work much better so I’ll stick with that. I’ll see if I can add some code to decrease the voltage if measured current goes above the limit, to reduce the chance of damaging anything (electrical or mechanical) in a crash.
