Getting drivetrain to drive smoothly with gyro

Technical Programming
1

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:

  1. 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.

  2. 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.

2

Bump

3

Try this instead:

Don’t coerce the gyro angle to -180…180. Instead, subtract the (uncoerced) gyro angle from the desired angle and then coerce that difference to the range -180 to +180. The result will be the shortest angle path from the present gyro angle to the desired angle.

4

Back at ya.*
*

5

Ah, that makes sense. I tested Math.IEEEremainder with some mock gyro data and it seems to work good.

We will test it on the robot when we un-bag it.

Thank you!

I have a feeling we still may need to implement a full PID loop to make it as smooth as possible though.

6

Possibly, possibly not? We have had good success for two years now using a gyro for a similar purpose during autonomous (keep the robot pointed in the same direction while not intentionally rotating). This has been with tank drive and mechanum setups.

If you do go full PID, here’s a couple thoughts on making it better given what you’ve said:

-Reducing P can help with oscillations.
–Increasing I will allow the system to still get to the setpoint with a lower P
–Setting a limit so the accumulators for the I term only run below certain errors allows the I gain to be large without compromising stability. I’ve used this with great success on drivetrains where stiction at low speeds is noticeable.

-Increasing D can (to a point) help reduce oscillations produced by P gain alone.

7

Thanks for the tips. If we end up going full PID (first figure out how to) we will keep this in mind.