Because this thread is so long, I haven't had a chance to read it all, but I see some common themes coming up... some people fighting for using PID... some people fighting to understand the math behind it... some people just looking for examples. I do a great deal of linear and non-linear control applications when I'm not doing FIRST, so I was thrilled this year to have some time to help the team build in some simple feedback controls systems to control not only our drive train but a two-jointed articulated arm.
And so I want to comment on a few things... But first, as a reference, if you would like some examples of PID (actually PIID, notice the second I) feedback loops as well as some other important modifications, take a look at our code for this year's Pittsburgh regional:
http://www.osufirst.org/twiki/bin/vi...04RegionalCode
To give some background, the robot is driven by one joystick out of the box, a number of buttons and switches, and a mock-up of our actual two-jointed arm. This mock-up has potentiometers built into each joint just like our big arm. Our big arm tracks the position of the small arm. Additionally, our drive train wheels have Banner sensors encoding every 36 degrees of their rotations into "ticks" that can be handled by interrupt handlers tied to digital inputs 1 and 2. If we had our ticks spaced correctly, we could run the Banner "normally closed" input into two more digital inputs to get double the resolution. There are some other software tricks to increase resolution, but they need speed first... and we need speed first anyway...
Use Speed Control for Actual Motor Control Whenever Possible
So my first comment is on speed control. Because motors inherently output speed and not position, even though you have position sensors (effectively), it's much easier to control speed. However, it's not trivial to calculate speed on such a robot with all of these quantization issues. For example, it's tempting to calculate speed by counting the number of ticks that pass by your encoder in a particular time. If you have lots and lots of ticks and are not sampling that fast, this works fine. However, if you're sampling fast with respect to the tick changes and you don't have that much resolution so the ticks take a long time to get noticed, you need a new way to calculate speed.
How to Calculate Speed in Software Quickly
On our robot, we have a clock running that keeps track of the number of 100ths of a second (centiseconds) that have passed. This is an "elapsedTime" variable. This is setup using interrupts, as is explained on IFI as well as many places here. Now, the interesting part is that our interrupt handler that catches the tick transitions of each wheel doesn't just increment a counter; it also calculates the time difference between the current tick and the last tick and also stores the current time. This time difference allows for the current speed to be easily calculated (in our code, in inches per second), except the problem is that if the robot stops turning, it will never register that we've slowed down to 0. So in our fast code, we calculate the difference between the current time and the previous tick time, and if that grows too large for our slowest speed (slowest speed is found by ramping up input until the robot moves and recording that speed), then we assume we've stopped.
Calculate Acceleration Too: This is Important Later
At each tick, calculate a new acceleration based on the change in speed over the time it took to move from tick to tick. This is much simpler than the speed calculation, but you should do it here rather than anywhere else so that it stays constant from tick to tick. Again, you should have some way of bringing the acceleration to 0 if the robot is stopped.
Use Constant Time Sampling
It doesn't always pay to do things as fast as you can. In these applications, sampling your actual data at 100 times a second is probably enough. So even though you are calculating speed very quickly from those interrupt handlers, actually create "speed samples" that will be known to be constant over the whole sample. This will become more important later when implementing the PID controller.
System Identification: Keep It Linear!
While a great deal in engineering is random, unknown, and often unsolvable,
nothing in engineering is black magic. Once you can capture speed being outputted each 100th of a second to the program port, it is easy to characterize your system (or a linear approximation of it). This can be done by software encoding an input which sits at 0 for a small time and then immediately jumps up to some value (otherwise known as a "step" input). The input speed from the wheel turning due to such an input can actually give you the PID coefficients almost directly. (REMEMBER TO USE CONSTANT TIME SAMPLING!!) If you are not familiar with this compensation process or have no one around you who is familiar with root locus and/or Ziegler-Nichols methods, then you can bypass this section entirely.
However, if you do do system identification on your robot,
be sure that your step is not too large!! Having your robot jump into a speed that is approaching its saturation point will unfortunately cause your response to characterize a particular point on a very non-linear side of the motor. If you want to approximate your motor as a simple second-order linear time-invariant system, you probably should do it in the region where the change in output over change in input is still fairly constant.
Note that in the 1014 code, we actually took a step and implemented it, but unfortunately our drive train changed substantially after the step was taken and we weren't able to use our optimized coefficients in time for competition. We resulted to setting these coefficients heuristically, and things worked very well during competition.
Before Implementing: Condition Inputs and Outputs
Create functions that will take in a value from -128 to 127 and output a PWM signal that has had a dead-zone applied (and possibly a hold-over zone) that goes from 0 to 255. Keeping your code in terms of -128 to 127 will simplify things GREATLY. This makes things all "linear" rather than "affine."
The Basic PID Controller
PID is simple to implement. The basic pseudocode goes something like this. Keep in mind the last note about conditioning inputs and outputs to be centered around 0. Also remember that both actual_seed and actual_accel are available as they have been calculated already. NOTICE THE USE OF Ts. This is the time between samples. If you sample 100 times a second, this is thus 0.01. THIS IS VERY IMPORTANT. It will help make your Ki gains much more reasonable. It is needed to approximate an integrator. It is the "width" of each rectangle that is being integrated.
error = desired_speed - actual_seed;
output = Kp * error + Ki * Ts * sum_error + Kd * actual_accel;
sum_error += error; // Don't actually do this!!! SEE NEXT NOTE!!!
The goal is then to set the Kp, Ki, and Kd coefficients properly. If you have identified your system, this is easy to do. If you have not, keep this in mind, and BE CAREFUL as you can EASILY CREATE OSCILLATIONS:
Kp: Proportional Gain - Sets the shape of the output to generally match the input
Ki: Creates a lag in the output that helps smooth transitions. This also causes overshoot due to this lag. However, a great thing it does is that it gaurantees no steady-state error. After some sufficiently long period of time, if enough of this term is added in, the output will match the input exactly.
Kd: Speeds up the response. This reduces the lag caused by the Ki control. However, not only does it couple in lots of noise into the system, but it can actually cause steady-state error problems (because derivatives of constants are 0).
Dealing with Integer Overflow: SATURATE
If that sum_error term just keeps getting added to over and over again, it will very quickly overflow. This will turn a VERY LARGE POSITIVE NUMBER quickly into a VERY LARGE NEGATIVE NUMBER. Integral control can on its own cause nasty oscillations just due to its delay/lag. However, adding this fun roll-over effect creates even worse ones.
To fix this, wrap something around the addition that prevents it from ever rolling over. Something like:
sum_error = safe_add( sum_error, error );
Where safe_add makes sure that if a positive is added to a positive, a positive comes out, and if a negative is added to a negative, a negative comes out.
You can see our PIID code in user_routines_fast.c. There is an additional integrator to allow us the freedom to tune our second order (ramp) input response. There are reasons for this additional I, but I am placing them outside the scope of this message. This controller does show all of the points above, including some more.
Be Careful: Literals are BYTES by Default
In this process, if you use ANY literals, be sure you explicitly cast them as, for example, int if they are negative or over 127. Otherwise your 255 looks like a -1 to the PIC.
In general, be careful about your signed/unsigned conversions. Things may not be as you think they seem.
Use State Machines in Autonomous Mode Once Good Controllers are Built
Once good speed and position controllers are built (position controllers can be built on top of speed controllers), use state machines to move from one state (motors driving forward) to another state (motors turning robot left) to another state (motors driving forward again) to...... The only time you should have to look at your feedback in autonomous mode is thus when to CHANGE STATES. Otherwise, you let the speed controller do all the hard work for you.
See the 1014 code for an example of this.
Additionally, it is possible to build "steering controllers" that are PID based in themselves. These can take angles and forward velocities and turn them into left and right motor CONTROLS which then get fed into the left and right motor PID controllers which then turn them into motor commands.
Some Simple Benefits (and Cons) of Feedback Control
Being able to count on a controller to deliver a very simple task is so important. This allows the driver to simply "suggest" a speed with his or her joystick and the robot will handle all the rest. The driver can then get up the step at a slow speed, for example. Rather than running straight into the step, the driver holds the joystick constant and the feedback controller figures out how to get up the step to maintain its own speed.
Additionally, for that same reason, a real feedback controller actually makes the torque-speed curve of a motor almost entirely horizontal (within a certain range). This actually can be dangerous because the robot will perform CONSISTENTLY under ANY BATTERY POWER and it may be easy to forget to monitor it's battery life.
Plus, these controllers are extremely robust. People spend hours dealing with open-loop filters on joystick inputs and often come up with odd non-linear controllers which have no easy way to optimize. The linear feedback control system uses the motors own natural low-pass effects to smooth out its own input. The linear feedback control system needs very little tweaking to be operational, and will do its hardest to perform as close to optimally as possible even on a low battery or when facing an odd load.
However, there is a much stronger dependence on hardware. There is a tremendous increase in complexity. There simply are costs for all the things that the feedback gains you.
Anyway, I hope that code might help some people. I'm afraid my post might be too long to help pepole directly, but hopefully some will read it. The system works great on our robot; I hope it can help yours.