Hi,
I’ve been playing around about with SimpleFOC and I’ve somewhat run into a bit of a wall in getting any super useful closed loop control happening outside of torque voltage mode.
The above screenshot is with the motor sitting at rest not spinning. There is no change in the measured angle, so I’ve been looking at it as a problem of calculating velocity from position.
I’m running an optical encoder with the STM32 Hardware Encoder driver.
I’ve tried running the foc and move loops inside the main loop, from a timer trigged by the PWM timer, and from the ADC end of conversion interrupt. None of these approaches have had a better result than the other.
Any help or ideas on troubleshooting and formulating a path to a solution would be greatly appreciated.
Cheers,
Pat
Are you sure the encoder is giving good signal? Velocity is calculated from the position, so for it to be spiky means the position must also be. But it could be true if the motor is vibrating. In that case it may be your PID values are too high.
I’ve had the encoder output on the oscilloscope and it looks good.
I’ll do another trace grabbing the encoder timer count rather than the motor shaft angle to ensure there is no motor vibration causing it, the encoder is giving 20480 cpr so it wouldn’t need much vibration for the timer count to change I guess.
I can’t hear or feel any motor vibration. I’m using voltage mode in torque control with a phase resistance value, so there shouldn’t be many PID terms used, and I would expect with the motor at an actual standstill, it should converge towards zero.
Should I look at removing the phase resistance so the loop runs purely on voltage at the target?
Would it be worth remove the dt calculation from the velocity calculation since the loop is running at a fixed period?
It sounds like something weird is going on, so I’d start at the bottom and work your way up. STM32HWEncoder inherits from the base Sensor class and does not override getVelocity, so it should be using the base class version. Try printing some stuff there, like this:
float Sensor::getVelocity() {
// calculate sample time
float Ts = (angle_prev_ts - vel_angle_prev_ts)*1e-6f;
if (Ts < 0.0f) { // handle micros() overflow - we need to reset vel_angle_prev_ts
vel_angle_prev = angle_prev;
vel_full_rotations = full_rotations;
vel_angle_prev_ts = angle_prev_ts;
return velocity;
}
if (Ts < min_elapsed_time) return velocity; // don't update velocity if deltaT is too small
velocity = ( (float)(full_rotations - vel_full_rotations)*_2PI + (angle_prev - vel_angle_prev) ) / Ts;
static unsigned long lastPrintTime = millis();
if(abs(velocity) > 1 && millis() - lastPrintTime > 1000) {
lastPrintTime = millis();
Serial.print("full_rotations "); Serial.println(full_rotations);
Serial.print("vel_full_rotations "); Serial.println(vel_full_rotations);
Serial.print("angle_prev "); Serial.println(angle_prev);
Serial.print("vel_angle_prev "); Serial.print(vel_angle_prev);
Serial.print("Ts "); Serial.println(Ts);
Serial.println();
}
vel_angle_prev = angle_prev;
vel_full_rotations = full_rotations;
vel_angle_prev_ts = angle_prev_ts;
return velocity;
}
That might give you a clue as to the nature of the values that result in bad velocity calculations.
Well, it definitely falls into the category of something weird I think.
I can see no correlation between the velocity jumping all over the place, and the numbers it uses in the math for it.
Also is a link to the CSV log if anyone is better at making graphs and analysing numbers than I am to see if anything jumps out. https://drive.google.com/file/d/1jLRRx2KtT1nL1TOe9ZdUJUgdKRIX6gss/view?usp=sharing
I may even revisit my hardware timer based velocity to see if there’s a difference.
These graphs look nonsensical. If I’m understanding the log file correctly, it may be a problem with the data reporting. For example at 176 microseconds, column D (velocity?) jumps all over the place while the other values don’t change at all. And since it looks like your loop is running at 25KHz, and velocity is only updated once per frame, there’s no way it should be able to change multiple times within a single microsecond like that.
That’s the exact thought I’m having.
I ended up using SWO and MCUViewer trace because when I tried the serial output code it spends all CPU time in the serial interrupt while not outputting anything, and also not working. Interesting you said the loop looks like it’s 25kHz, at that point the PWM frequency was 10kHz, and the loop should have been running once per period. Perhaps those two are related.
I might go back to running the loop from a timer clocked from the PWM time update rather than the ADC complete callback, possibly I’ve picked the wrong callback, it was meant to be the callback for when the entire set injected conversions was complete, I may have stuck it in the callback for each individual conversion.
The concept of running the foc loop as soon as the ADC read was complete to make sure the current values recorded are close to what was happening was sound. But it looks like the concept is easier to say than implement.
As you said though, the fact the output value changes when the input values don’t means something’s wrong somewhere. Even using the variable viewer mode in MCUViewer where it just grabs the value at memory locations has the speed jumping around everywhere, so I don’t think it’s related to me possibly doing the SWO trace wrong.
The other possibility is somehow the velocity value memory location is being changed by something that shouldn’t be changing it.
If it helps I’m running this on G474 Nucleo board. One of the reasons for running the loop from a timer or interrupt is because the loop speed is just too fast in the main program loop.
This is my second attempt at an implementation, first time round I ran up against the noisy velocity problem, so I decided to go back to simple first principles and get the baseline stuff sorted before adding stuff.
Yeah, serial might not work if you’re running the whole thing from inside an interrupt handler. It’s best to keep interrupts as short as possible. For example you could have the ADC interrupt set a variable, and main loop wait until it’s set before running loopFOC. Declare the variable as volatile so it’s sure to reload from memory for every check.
Shit sticks. You’re spot on, I do recall reading that exact advice somewhere, then I promptly forgot it.
And now you mention it I remember discovering printing to serial doesn’t work in an interrupt handler. Something about reentrant interrupts and most likely getting such in a loop, sounds familiar.
I’ll give the serial print another go, the trace output gives me a bit of data overload sometimes.
I’ll let you know the outcome. Thanks for the advice.
