Programming in LabVIEW. We have all of the inverse kinematics working. The Gains are all properly tuned. But we don’t have the correct algorithm to set the angle wrapping process. We are re-reading the cancorder at angles larger than 360 or -360 degrees and resetting motor encoder position.
However we still have having the situation where every rotation of the pod it will want to reverse direction 360 degrees.
Is there a write up on how best to implement the wrapping process?
Or should we just allow the angle to continuously wind up.
I believe the solution we implemented years ago (and still use today, it’s just been a while since I looked at it) was to let it wind up and send the nearest angle multiple as the set point. We chose to do it this way as we wanted to run the PID loops on the Talons directly, and therefore we had little control over wrapping behavior. Resetting doesn’t work in this case, as the Talon loop will run asynchronously from the RIO; the reset therefore can’t be precisely timed with the wrap around.
If you’re running the PID loops on the RIO, the problem you’re running into is that even if you reset the position upon crossing the origin, the error a standard PID loop calculates doesn’t understand that 1 is two degrees up from 359; it instead calculates -358, and negative means going the wrong way in this case. A simple hack you can do is to manually calculate the error yourself (I’ll leave this as a mathematical exercise for your team* - in part because I’m not confident our method is at all optimal :p), feed the error as your setpoint for the PID loop, and constantly feed 0 as the sensor position. The PID loop will then calculate an error identical to what you already calculated yourself (since the error minus zero is just the error), and everything should work as expected from there.
*LabVIEW does have a built in function for calculating absolute angle differences, but absolute in this case means it’s always positive - so it still doesn’t get you all the way there.
Edit: May I ask exactly how you’re resetting the CANcoders? Simply setting the position to 0 upon crossing the wraparound point is a bad idea.
Don’t you have a buffer overrun problem if the motor encoder is allowed to wrap infinitely?
Our reset happens when we reach one full turn in either direction with respect to the motor encoder. We then ask the cancorder it’s value and set motor position back to what that position.
In theory, sure. In practice, we’ve run swerve this way since 2018 and it’s never cropped up, even in extended practice sessions. The modules simply don’t do full rotations nearly often enough for that to matter; the battery will die before you overrun the motor encoder position.
Gotcha - I misread a bit then, and that makes sense. We also run our PID off the motor encoder directly for latency reasons. However, if you’re running PID on the motor controller, this still doesn’t work. In order to get the steering to rotate positively from, say 358 degrees to 45 degrees, you’d need to feed the motor controller a setpoint corresponding to larger than 360 degrees - and AFAIK there’s no way to reset the position and set the setpoint to 45 degrees at the same time, as I believe they’re not part of the same CAN packets.
If you really want to do this, I’d advise trying to detect and reset during no-motion events, and seeing how that works for you.
Since those videos we’ve also implemented the goal angle unwrapping so that we can run the PID on the Falcons this year. You can find our current code here: https://github.com/secret-city-wildbots/2022-Robot
And you would be looking for the SwerveUnwrapGoalAngle VI
Thanks all. Just to make sure I understand, @lscime , @Tom_Line@thatnameistaken, none of you reset your motor encoder position using Cancoder in periodic once the pod runs around a certain number of revolutions. Is that correct?
Yes, I can confirm that we don’t reset the absolute encoder position in normal operation. One nuance that might be worth mentioning is that we are not using a Cancoder, we are only using the encoders built into the Falcons.
Correct. In theory we could even close the CANcoders in begin after setting the Falcon position (which might save on CAN bus bandwidth?), but we leave them open in case a calibration goes awry.
The drivers essentially have to set the wheels to the correct angle during setup. This of course adds some time and risk, but packaging an absolute encoder was extremely challenging due to the placement of the pancake piston and the ball shifter in our swerve modules. So the trade-off was worth it to us. So far we’ve done an off-season and two regionals with this configuration without anyone forgetting to set the modules before the match… There are some additional code “fixes” to handle situations where the Falcons or the roboRIO might brownout/reboot during a match.
We also did this for a bit after losing an absolute encoder in 2018. We weren’t attempting precise autos back then, but I can attest that it works well enough for teleop.
I am resetting motor encoder at 360 degrees and -360 degrees of the Cancoder. I was also able to develop the short spin algorithm after reviewing everyone else’s code. It is a little different but I think technically the same.