I have a rotation sensor that gives me values from 0…359.9.
My Arduino code has a PID loop. I set the target angle and the PID loop moves a motor until the sensor value matches the target.
If I’m at 350 and I say “move +20” the Arduino wants to reach 370… but the moment it passes 359.9 the sensor says 0. The PID loop goes bonkers.
I cannot physically limit the system to less than 360 degrees rotation and I’m wracking my brain trying to come up with a good solution. It should just be a software problem, right?
I faced a similar issue years ago with GPS location coordinate values which
use circular minutes and seconds and go from 59 to 0 etc.
It’s the math that screws up with the zeros so as you are simply using the
match to find the target why not add 1.00 to every encoder value.
You could perhaps also change to use an incremental encoder with quadrature
output and a ‘zero’ position output
But with PID control you presumably are interested in the distance to the
target position and not the precise value of where the shaft encoder is at
any one time.
Is this simply an absolute positioning problem, or do you need to always rotate forward (or always rotate forward for +inc inputs, perhaps)?
If it’s just a matter of reaching the absolute position, I would handle this by treating all arithmetic on the target angle as mod 360. This will cause your +20 input to roll over to 10 degrees. With naive error magnitude code, this will cause backwards rotation through the 340 degrees to reach the target position. This is easy to deal with in the error signal though; if the error is > 180 degrees, use (error - 360) instead.
I’m interested in both the absolute position and the relative movements. At any given moment I use the absolute position to build a 1:1 simulation in my computer. I then drive the simulation with relative movements. There is potential to move both up past 359 and down past 0.
Although potentially complicated, it might be possible to create a PID loop that works on a 360 degree domain. I don’t know, though, so here’s a simpler solution.
Instead of taking the direct sensor reading, perhaps you could maintain a variable that tracks the total number of degrees rotated as well.
float angle = 0;
void setup() {
//This line isn't really necessary
angle = ReadAngleSensor(); // returns value in range [0, 360)
}
void PID_Loop() {
angle = UpdateAngle(angle, ReadAngleSensor());
// Use variable 'angle' in your PID calculations
// ...
}
/* sensorValue is mod 360, prevAngle is continuous.
Returns the angle closest to prevAngle (rotates
to the nearest 180deg), not mod 360. */
float UpdateAngle(float prevAngle, float sensorValue) {
float modDiff = prevAngle % 360 - sensorValue;
if (modDiff <= -180)
modDiff += 360;
else if (modDiff > 180)
modDiff -= 360;
return prevAngle - modDiff;
}
I think this covers all cases, but you’d better verify yourself because I’m sort of out of it today.
For any of your joints that can rotate indefinitely, you then have to consider whether the angle variable might overflow and what to do to prevent that. Perhaps you could check if angle exceeds a bounds, and if it does, you may have to re-scale a number of variables (not only angle) in your PID loop.
One thing to keep in mind here is that a ‘float’ is only 32 bits, and if your robot rotates in the same direction for a long time such that you end up with more than six significant digits, you’ll start to lose precision. It’s easy to say “well this is just a toy” but The Prototype Becomes The Product.
If you use a ‘double’ instead you’re much less likely to ever see problems… Or just figure out how to keep the value within a reasonable range.
