Quote:
Originally Posted by GuyM142
Correct me if I'm wrong:
The code generates an array of velocities which the robot has to reach at specific times of the auto (each side independently) and with encoders on each side and a PID loop I need to make the wheels actually get to that velocity.
Did I get it right?
If so, what is the best way to use PID for velocity control?
|
As Art said there are many ways to implement this. The easiest velocity loops (IMO) heavily utilize feedforward terms.
Code:
// For each side of the drive...
while (following) {
// desired_position, desired_velocity, and desired_acceleration are all generated by your profile.
position_error = desired_position[i] - actual_position;
command[i] = Kv * desired_velocity[i] + Ka * desired_acceleration[i] + Kp * position_error;
}
You will find that it is very easy to tune with the following procedure. Before doing this, it is REALLY helpful to have graphs of position vs. time and/or velocity vs. time in Smart Dashboard (or an equivalent plotting tool).
1) Generate an impulse response. From a stop, slam the sticks forward until you max out at your full robot speed. You may require a long distance to get up to speed, so plan accordingly. Stop the robot once it hits full speed (...or the back wall of your shop).
2) You can now analyze your charts to obtain a couple useful quantities:
max_speed: The maximum speed (maximum slope) of the position vs. time plot.
accel_time: How long from when you started until you hit the maximum speed (it is a bit of a judgement call since the curve is smooth, but try to get it in the right ballpark). max_accel ~= max_speed / accel_time
4) Set Kp = Ka = 0, and Kv = 1/max_speed.
5) Generate a motion profile with a maximum velocity of about 80% of your max speed and a maximum acceleration of about 80% of your max acceleration. The 80% is to be conservative and robust to batteries, etc.
6) Attempt to follow the profile with the gains from step 4. Watch your plots vs. the profile (yep...also plot the commanded positions/velocities). It is likely that you lag the profile initially, and lead it (or overshoot) at the end due to the robot's inertia.
7) Next, we will tune Ka to have the robot compensate for this lag/lead. The idea is that when you are accelerating heavily, you can add or subtract from your command to compensate for the robot's inertia. Tune Ka by adjusting and repeating the experiment in 6 until you are following the profile as close as possible. The right value of Ka will be between 0 and 1/max_accel. 1/(2*max_accel) is a reasonable first guess.
8) At this point, you should be following profiles reasonably well, but it is not perfectly repeatable, probably doesn't drive straight, etc. Let's add some feedback. The units of Kp are in (% of power per unit of error). If you have tuned Kv and Ka pretty well, you can get away with a Kp on the order of 1 or 2 if your units are meters (1/3 to 2/3 if using feet, etc).
9) You should follow the trajectories really, really well now. They should be somewhat straighter and quite repeatable.
At this point, things are probably working decently well. Here are a few more options if you want to keep tweaking:
1) Add an integral term based on the sum of position error to get even more precision in final distance. We found this totally unnecessary last year, but YMMV.
2) Add a gyro to help stay straight. Each side of the drive is totally independent, so you can add a simple gyro controller that adds or subtracts from the velocity command being fed to each in order to compensate for accumulated errors.
3) Add trajectory replanning so that if you get bumped to the side, you instantly regenerate a new trajectory to smoothly get you back on track. If necessary for our strategy, 254 might do this in 2015
4) Better characterize your drive train. The calculated left and right velocities are actually assuming a two wheeled vehicle with no slip...4, 6, and 8 wheel drives always have some scrub that results in the vehicle turning less than commanded. You can attempt to measure this and adjust your trajectories accordingly.