Our team is trying a swerve system for the first time ever this year. We have it mostly done, but we have a bit of a hiccup with the programming portion of it. The optimize function does not seem to be working correctly and we do not know the reason of it. Whenever turning and strafing, our wheels shake back and forth. We looked at the diagnostic, and we discovered our wheels keep trying to rotate 180 degrees, and then rotate back during the first rotation, very quickly, causing them to shake uncontrollably.
We think this is because the optimize function is not working correctly. We think it keeps trying to flip the direction of the wheel over and over again.
For example, if we try to strafe left, the desired state angle it keeps flipping from 90 degrees to -90 repeatedly.
You have both turningPID.enableContinuousInput(-Math.PI, Math.PI); // minimize rotations to 180 and the optimize doing somewhat similar functions. Make sure they aren’t in conflict with each other.
Yes they both minimize turns but the optimize function will also invert the drive motor too. U need both. the continuous just means that u can cross the boundaries.
U appear to be using to different methods to read ur current angle. One method u use in the optimize method, but a different method for input into ur PID control. Can’t say that’s the problem but I feel it can’t help.
We changed the method used in the PID to the other one once. But it made it worse. All modules started shaking uncontrollably whenever we drive, no matter the direction. However, we can change it back and test it again if you think that might be the problem
The PID controller that’s controling the voltage to the turn motor, turningPID = new PIDController(0.05, 0, 0); it doesn’t seem too high but the 0.05 is multiplied by the position error which if the max error could be 2π, then ur max voltage applied would be 0.3, so idk that’s the problem.
Ohh okay. That might be the problem. We have never done this before so we didn’t know what to put for the values. We just used one that we saw was being used by another team. We will try changing and testing it tomorrow.
If u log the desired state, is that changing as it oscillates? Or does it stay constant? If the desired state stays constant, then it’s something with the control to get to the desired state. If the desired state is oscillating then u have a control issue higher up in the stack of how ur setting/calculating the desired state.
Yes. Everything seems to work fine with the chassis speeds. Thank you for the link. We will look at it and compare with ours. If you can come up with anything else that might be the problem, we would really appreciate it
We had this exact problem a few weeks before, turns out WPILIB’s optimize’s PID continuous input requirement means that motor.set(<rot position>) needs to be able to continuously turn when the angle output from optimize changes from 360 to 0; (or others similar large angle jumps where on a circle the angles are near each other, but the actual numbers are far apart). ~~
CTRE’s apis take in motor rotations raw so the motors will jump a full 180 or 360 some times to reach the target angle given by optimize. There might be someway to get it to accept the outputs given by optimize correctly, but we didn’t find any info.
We solved this by reimplementating optimize ourselves, so it will add the output it wants to the motor encoder positions correctly and output that. Here’s the github repo for it: https://github.com/Alvin-He/swerve_optimize. The code explains what’s happening better than I could here, so just take a look at Optimize.java. The function can be plugged in place of WPILIb’s optimize function without any parameter changes and they accept the same things.
Edit: not the actual problem if you have continuousWarp enabled anywhere
