Drivetrain PID tuning

[ollien]

Originally Posted by ahartnet

Robot.drivetrain.arcadeDrive(DriveForwardSpeed, (Robot.drivetrain.getGyroAngle().getAngle()));

You might want/need to multiply the gyro angle by a Kp or -Kp. We actually ended up with the following:

Robot.drivetrain.arcadeDrive(-DriveForwardSpeed, (-(Robot.drivetrain.getGyroAngle().getAngle()) * Kp+.1));

How are you allowing the driver to adjust the robot's angle?

[D.Allred]

Originally Posted by ollien

How do you accomplish this? Do you just set your setpoint proportional to the position of the stick?

We did not use it to execute a turn or follow a path while driving. It was only used to make a stationary turn or maintain a heading for a "drive straight" mode similar to the example in your original post.

For tele-op drive straight, a button on the xBox controller sets the current heading as the desired heading value then replaces the driver's rotation joystick value with the output of the heading proportional controller. Essentially the driver is only controlling thrust at that point and the loop maintains heading.

For our use, a proportional only loop worked but you could set up PID and experiment with the tuning.

David

[sspoldi]

In 2015 we used Mecanum wheels and one thing we really noticed was that the robot would rotate and drive forward well, but generated a lot more random slipping when moving sideways. We ended up using encoders for forward and lateral positioning and the gyro for heading. We could have used the encoders (one on each wheel) to determine heading, but the slip was giving us problems.

It was worse when the wheels got worn out. we were using the 6" Vexpro wheels, very cost effective but had a very noticeable life span for the rollers. Replacement wheels altered our autonomous timing required for the 3 tote auto.

Cheers,
Steve.

[Jared Russell]

Originally Posted by sspoldi

In 2015 we used Mecanum wheels and one thing we really noticed was that the robot would rotate and drive forward well, but generated a lot more random slipping when moving sideways. We ended up using encoders for forward and lateral positioning and the gyro for heading. We could have used the encoders (one on each wheel) to determine heading, but the slip was giving us problems.

It was worse when the wheels got worn out. we were using the 6" Vexpro wheels, very cost effective but had a very noticeable life span for the rollers. Replacement wheels altered our autonomous timing required for the 3 tote auto.

Cheers,
Steve.

This is very common for mecanum drives. The rollers don't need to spin for forward movement or turns in place (depending on your drivebase geometry), but do need to spin for sideways translation. The less your rollers spin freely, the more your strafing performance suffers.

[SamcFuchs]

Originally Posted by Jared Russell

You can definitely do motion profiling with a gyro.

Of course I'm going to defer to you on this, but how reliable is displacement with a gyro? I'm skeptical.

[Jared Russell]

Originally Posted by SamcFuchs

Of course I'm going to defer to you on this, but how reliable is displacement with a gyro? I'm skeptical.

Do you mean angular displacement? (Gyros measure angular rate, which then must be integrated to obtain angular displacement). With commonly available FRC gyros based on an Invensense MPU-9250 (NavX, CTRE Pigeon) or Analog Devices ADXRS-450/451 (FIRST Choice gyro, WCP Spartan board) you can measure angular displacement *very* accurately...you will only drift a handful of degrees over an entire match under most conditions (e.g. level field). You can generate a turn-in-place motion profile just as you would using encoders.

A gyro can't help you measure translational motion, of course. If you use a multi-DOF IMU with gyros AND accelerometers, you can measure linear acceleration...but trying to obtain precise translational displacement from these is not going to work well (maybe this is what you were referring to). But you can combine a gyro with encoders to give you the "best of both worlds" and follow a profiled path while correcting for yaw errors. As long as the profile contains yaw information, you can use a PID controller (often a P controller is enough) to add a bit of velocity to one side and subtract it from the other to keep you on track.

For more details, see: https://github.com/Team254/FRC-2016-...rive.java#L385 (our "drive straight" mode engaged by the driver by holding down a button)
and:
https://github.com/Team254/FRC-2016-...rive.java#L396 (our path following mode used for autonomous mode)

[SamcFuchs]

Originally Posted by Jared Russell

Do you mean angular displacement? (Gyros measure angular rate, which then must be integrated to obtain angular displacement). With commonly available FRC gyros based on an Invensense MPU-9250 (NavX, CTRE Pigeon) or Analog Devices ADXRS-450/451 (FIRST Choice gyro, WCP Spartan board) you can measure angular displacement *very* accurately...you will only drift a handful of degrees over an entire match under most conditions (e.g. level field). You can generate a turn-in-place motion profile just as you would using encoders.

A gyro can't help you measure translational motion, of course. If you use a multi-DOF IMU with gyros AND accelerometers, you can measure linear acceleration...but trying to obtain precise translational displacement from these is not going to work well (maybe this is what you were referring to). But you can combine a gyro with encoders to give you the "best of both worlds" and follow a profiled path while correcting for yaw errors. As long as the profile contains yaw information, you can use a PID controller (often a P controller is enough) to add a bit of velocity to one side and subtract it from the other to keep you on track.

For more details, see: https://github.com/Team254/FRC-2016-...rive.java#L385 (our "drive straight" mode engaged by the driver by holding down a button)
and:
https://github.com/Team254/FRC-2016-...rive.java#L396 (our path following mode used for autonomous mode)

No, I mean linear displacement, which you would need for motion profiling with a gyro, no?

[ollien]

Originally Posted by SamcFuchs

No, I mean linear displacement, which you would need for motion profiling with a gyro, no?

Gyros don't track linear displacement. As Jared said, they track the rate at which the angular displacement is changing.

[SamcFuchs]

Originally Posted by Jared Russell

Do you mean angular displacement? (Gyros measure angular rate, which then must be integrated to obtain angular displacement). With commonly available FRC gyros based on an Invensense MPU-9250 (NavX, CTRE Pigeon) or Analog Devices ADXRS-450/451 (FIRST Choice gyro, WCP Spartan board) you can measure angular displacement *very* accurately...you will only drift a handful of degrees over an entire match under most conditions (e.g. level field). You can generate a turn-in-place motion profile just as you would using encoders.

A gyro can't help you measure translational motion, of course. If you use a multi-DOF IMU with gyros AND accelerometers, you can measure linear acceleration...but trying to obtain precise translational displacement from these is not going to work well (maybe this is what you were referring to). But you can combine a gyro with encoders to give you the "best of both worlds" and follow a profiled path while correcting for yaw errors. As long as the profile contains yaw information, you can use a PID controller (often a P controller is enough) to add a bit of velocity to one side and subtract it from the other to keep you on track.

I think there was a misunderstanding here. I was talking about using a gyro/accelerometer (the navX in particular) to measure translational displacement, which we both agree is too inaccurate for motion profiling. However, I do have one concern with the alternative you suggest, using the PID loop together with your profile follower. Isn't there a concern that the PID loop could correct in such a way that you end up over/undershooting your goal for the profile?

Our system is to run an independent profile on each side of the robot, where each wheel follows the profile and corrects itself based on the error from the end point. This way, at the end of the loop, the setpoint for each side is the endpoint of the profile, so it ends up correcting itself to the right place.

I guess what I'm saying is that when you use the gyro to go straight (based on yaw) you're adding a new variable (yaw) instead of interpreting yaw as a difference of distances. How do you ensure that that doesn't become a problem?

Hope this makes some sense.

[Jared Russell]

Originally Posted by SamcFuchs

However, I do have one concern with the alternative you suggest, using the PID loop together with your profile follower. Isn't there a concern that the PID loop could correct in such a way that you end up over/undershooting your goal for the profile?

Our system is to run an independent profile on each side of the robot, where each wheel follows the profile and corrects itself based on the error from the end point. This way, at the end of the loop, the setpoint for each side is the endpoint of the profile, so it ends up correcting itself to the right place.

I guess what I'm saying is that when you use the gyro to go straight (based on yaw) you're adding a new variable (yaw) instead of interpreting yaw as a difference of distances. How do you ensure that that doesn't become a problem?

Hope this makes some sense.

Yep this makes sense and is a problem if you have large errors that the gyro needs to correct (which would also be a problem if you didn't have a gyro present). Usually, though, your yaw errors will be fairly small and stem from the fact that your left and right profiles are executed independently and can encounter independent disturbances.

For example: Say we want to go in a straight line for 10 feet. You would generate left and right profiles that in this case would be identical. You begin following each profile. At some point, maybe there's a bunch in the carpet on the left side, so your left controller begins lagging a bit because of the disturbance...but the right controller keeps tracking accurately. If this happens, your robot is now veering off to the left.

The gyro's role in all of this being to coordinate the execution of your independent left and right profiles. If you are using a gyro to track your actual yaw angle compared to your profile, you'll see that you are now veering left, so your left side should try *extra* hard to get back on track, and the right side should ease up just a bit. With enough gain on the gyro term, the left side should recover very quickly and get synchronized with the right side before you've drifted too far from your intended path.

[SamcFuchs]

Originally Posted by Jared Russell

Yep this makes sense and is a problem if you have large errors that the gyro needs to correct (which would also be a problem if you didn't have a gyro present). Usually, though, your yaw errors will be fairly small and stem from the fact that your left and right profiles are executed independently and can encounter independent disturbances.

For example: Say we want to go in a straight line for 10 feet. You would generate left and right profiles that in this case would be identical. You begin following each profile. At some point, maybe there's a bunch in the carpet on the left side, so your left controller begins lagging a bit because of the disturbance...but the right controller keeps tracking accurately. If this happens, your robot is now veering off to the left.

The gyro's role in all of this being to coordinate the execution of your independent left and right profiles. If you are using a gyro to track your actual yaw angle compared to your profile, you'll see that you are now veering left, so your left side should try *extra* hard to get back on track, and the right side should ease up just a bit. With enough gain on the gyro term, the left side should recover very quickly and get synchronized with the right side before you've drifted too far from your intended path.

Ok, I can see how that would be effective...how does it compare to using encoders for correction?

[AustinSchuh]

Originally Posted by SamcFuchs

Ok, I can see how that would be effective...how does it compare to using encoders for correction?

Jared probably has a slightly different way of looking at this, but I think there's a more fundamental change needed first to make sure you go straight. The encoder vs gyro problem becomes much easier to try. Instead of left, right profiles, I rewrite it as a distance, angle profile, and then use a distance PID loop and an angle PID loop.

angle = (right_distance - left_distance) / width
distance = (left_distance + right_distance) / 2

You can do a similar transform for your powers (pardon any sign errors) to get back to actual outputs you physically have.

left_voltage = distance_voltage - turn_voltage
right_voltage = distance_voltage + turn_voltage

This then lets you control what you care about.

In the presence of tire slip and tire wear, you won't always go straight with just encoders. A gyro has a different set of issues (noise), but isn't affected by those problems. So, we use the gyro for the angle of the robot instead of computing the angle as above.

Just re-phrasing your loops to be an angle, distance pair of loops has huge benefit. Unless your robot has all it's mass perfectly centered over the two drive wheels, your left, right PID loop won't be able to do good distance control and turn control without compromises. I tend to find that good distance control results in a small amount of turn chatter and oscillation. (You can work out the physics to show that the two sides are coupled in left, right, but not in angle, distance) A angle, distance loop doesn't make that assumption.

If you really want to do left, right with a gyro, you can reverse the equations above.

[Jaci]

Originally Posted by SamcFuchs

Ok, I can see how that would be effective...how does it compare to using encoders for correction?

I think this question mostly stems from a misunderstanding.

The Motion Profiling you're describing, like most motion profiling, is based upon distance, velocity and acceleration. Something to note is that these all function in a single dimension. For devices like actuation arms or conveyors, this makes sense, as there is only one direction of motion. (there can still be issues here in tracking, but that is not related to a difference in distances)

If you want to start using this in a 2D sense (i.e. a drive base), these distances can be misread. For example, if a wheel (or track) on one side of your drive base slips (assuming 'tank' drive), it will mistakenly read a distance, even though it hadn't moved that far. To the code, your drive base is tracking properly, but in the real world it isn't. This is a common problem with drive bases. The most common way to fix this is to add an extra proportional gain based on the error between what you want your heading to be, and what your heading is reported to be by the gyroscope (per Austin's example). Let's say you have the following code:

float left_power = calc_power_left(time, profile_l, encoder_l);
float right_power = calc_power_right(time, profile_r, encoder_r);

The profile will have already generated the desired distance for each side of the drive base (the arc length along the curve of motion for each side), so in a perfect world, this would track just fine. However, we don't live in a perfect world, so we add something like the following:

float gyro_heading = ... your gyro code ...
float desired_heading = get_heading(time, profile);
float delta_angle = desired_heading - gyro_heading; // This is our error in alignment
float turn_power = 0.8 * (-1/80) * delta_angle;   // Change these coefficients as required

setLeftMotors(left_power + turn_power);
setRightMotors(right_power - turn_power);

In the above example, get_heading returns the heading of the path at the given time (i.e. the 'tangent' if you were to plot the curve). This accounts for errors in the heading by 'nudging' the robot in the right direction.

The main point I'm trying to clear up here is that it isn't entirely possible to use encoders for correction, as they rely on continuous, perfect connection to the field floor. Adding in a gyroscope will account for any errors in heading that the encoders, wheels or field introduce.

I hope this clears up any misconceptions you have.

[thatprogrammer]

Originally Posted by AustinSchuh

Jared probably has a slightly different way of looking at this, but I think there's a more fundamental change needed first to make sure you go straight. The encoder vs gyro problem becomes much easier to try. Instead of left, right profiles, I rewrite it as a distance, angle profile, and then use a distance PID loop and an angle PID loop.

angle = (right_distance - left_distance) / width
distance = (left_distance + right_distance) / 2

You can do a similar transform for your powers (pardon any sign errors) to get back to actual outputs you physically have.

left_voltage = distance_voltage - turn_voltage
right_voltage = distance_voltage + turn_voltage

This then lets you control what you care about.

In the presence of tire slip and tire wear, you won't always go straight with just encoders. A gyro has a different set of issues (noise), but isn't affected by those problems. So, we use the gyro for the angle of the robot instead of computing the angle as above.

Just re-phrasing your loops to be an angle, distance pair of loops has huge benefit. Unless your robot has all it's mass perfectly centered over the two drive wheels, your left, right PID loop won't be able to do good distance control and turn control without compromises. I tend to find that good distance control results in a small amount of turn chatter and oscillation. (You can work out the physics to show that the two sides are coupled in left, right, but not in angle, distance) A angle, distance loop doesn't make that assumption.

If you really want to do left, right with a gyro, you can reverse the equations above.

This post is very useful, but I had a few questions based on it.

1. Assuming that you use your gyro to calculate your angular offset rather than "angle = (right_distance - left_distance) / width." How would you calculate the turn voltage part of the bottom equation?

"left_voltage = distance_voltage - turn_voltage"

Would you just use a full PID loop to calculate a voltage (and tune it) and then just add the result of that to your distance PID loop?

2. Do you have any tips for tuning PID loops in cases with lots of friction like turning PID loops? Jared (very kindly!) sent me some tips on tuning PID loops before, but I'm curious to see if there are other viewpoints on this in situations with lots of friction.

[AustinSchuh]

Originally Posted by thatprogrammer

This post is very useful, but I had a few questions based on it.

1. Assuming that you use your gyro to calculate your angular offset rather than "angle = (right_distance - left_distance) / width." How would you calculate the turn voltage part of the bottom equation?
Would you just use a full PID loop to calculate a voltage (and tune it) and then just add the result of that to your distance PID loop?

Pick your favorite controller. P, PD, PID, LQR, Lead/Lag, MPC, YMMV I try to start with what boils down to a PD loop before I pull out I or any other terms. The input to your controller is the angle, and the output is the voltage.

Originally Posted by thatprogrammer

2. Do you have any tips for tuning PID loops in cases with lots of friction like turning PID loops? Jared (very kindly!) sent me some tips on tuning PID loops before, but I'm curious to see if there are other viewpoints on this in situations with lots of friction.

I've got a bit of a random pile of advice, but hopefully some of it is useful when the robot isn't working right. Sometimes you just need to try different things to figure out what's going wrong.

If you've got a bunch of friction, you end up needing to slow things down a bit. Friction is a pain to deal with. You also need to ask yourself really how accurate you need to be. If you can tolerate moderate steady state error, leave it as a PD loop. As the robot goes faster, it takes less energy to get it to steer, which works in your favor. If you really need good tracking, you are going to have to work at tuning I correctly.

Consider running in low gear if you have a transmission so friction is a smaller portion of your overall torque. The most annoying steering loop I tuned was 254's 2011 robot, geared for crazy speeds. We needed to run auto in high gear too, which meant we were close to saturation all the time.

The most annoying issue I've had tuning heading PID loops was where there was lag in the gyro angle reading. That phase lag meant I couldn't push the bandwidth of the loop up to anything useful. I had to fix that before I could get it to stabilize well. I debugged that by plotting the gyro and encoder headings.

Run your loops at 100 - 200 hz. You want to run your loops at 10x the frequency of the highest frequency you want to control. So, if you want to control at 10 hz, you need a 100 hz loop. The more reliable you can get your loop timing, the better. We go to great lengths to hit a 5 ms +- 5% loop timing, and it helps a lot.

The most important part here is to plot everything. Plot your error vs time, and watch it evolve. Plot the power due to P, I, D, FF. Plot the encoder based heading and the gyro based heading. It's possible but hard to tune these things by eye. Honestly, sometimes I think it's easier to rough tune them by listening to them and listening for the overshoot, and then reaching for the plots when I'm stuck. I also like to grab it and feel the loop, though you have to be very careful since robots can cause lots of damage fast.

We use more complicated controllers, which means I have less recent relevant robot experience than I'd like here. I should probably go grab a robot and tune a PID heading loop again just to have some more guidance.