We’ve been trying to get our drivetrain code properly working for a little while now but have been running into issues. Our drivetrain is a 3 wheel kiwi drive. We have a gyro on the robot plugged into the RoboRIO’s SPI port, pretty much centered on the robot. The robot currently compensates for errors in the target angle and actual angle as it drives (by rotating), but it has a couple of issues:
Sometimes the robot will just start continuously rotating when when the driver stops providing input. It seems to be when the target angle is around 180, the gyro angle never reaches it for some reason and the robot just spins in place. The driver can fix it by moving the rotation joystick, but it shouldn’t happen in the first place and we’d like to fix it.
It overshoots and oscillates sometimes. Many times when strafing or even driving forward it will keep wobbling back and forth, making it look unstable. We’d like to get it to drive smoother (after all, that’s the whole point of the gyro).
Here is our drivetrain code: https://gist.github.com/nathan815/156f6c8c669e712a0fdfd47e7558de98
The gyro angle is returned by getHeading, which uses another method called getCorrectedAngle that takes the raw gyro reading and fixes it so it is between -180 and 180 (the raw value keeps increasing well beyond 360).
The drive method is where most of the fun happens. It takes in an x, y, and rotation value from two joysticks. In the method, we create a diff/error variable which is the absolute value of the difference between the gyro angle and the target angle. If this error is larger than 3, we compensate: If the target angle is greater than the current angle we rotate right; if it’s less, then we rotate left.
The compensation value is calculated by taking the diff/error and multiplying it by a small value to scale it down. Since motor controllers take values between 1 and -1, we don’t want it adding large numbers to the speeds. (We also limit the compensation value to min +/-0.1 and max +/-0.3.)
At the end of the drive method we calculate the speed for each wheel. The compensation value is simply added to each wheel speed so the robot rotates to compensate.
The robot drives alright overall, besides the issues listed above. We’ve been able to get it driving a bit smoother by adding a time delay when rotating and such. The robot also corrects back to its current angle when it’s kicked.
Earlier today, we tried switching to using a PIDSubsystem, but we could not get it working properly. It just kept oscillating no matter what values we tried for the P, I, and D constants and it got much worse. It would just constantly oscillate in place without even touching the driver controller. Most likely we were doing something wrong.
We know a PID loop is the proper way to go, but we don’t know how to do go about it. Maybe there is a way to integrate PID into our current code? Technically our current code is already a proportional “P” controller, if I understand correctly.