Part 1 covered the theory behind PID control. Part 2 discussed the practical issues of odometry and using PID for precise mobile robot navigation. Now let's look at some more code and other features of practical rover design.
A high-level program, in my case written in FORTHdsPIC, a dialect of the Forth language for embedded control. This code performs the higher 'brain functions', for example, monitoring and responding to sensor inputs by driving motors according to a master plan. The master plan could be to explore and map an area of terrain while avoiding falling into holes or crashing into walls. The basic demonstration program shown in Listing 1 doesn't do much except wait for an update from the wheel tacho system and then calculate a new control value for the motor drive system.
Low-level bare-metal code which determines the rotational speed of a wheel using pulse data provided by a slotted-disc tachometer (See Part 2.). This code forms part of an Interrupt Service Routine (ISR) which measures the time interval between tacho pulses using the dsPIC's Input Capture hardware. There is no need to convert a time interval to speed – it just means the Set Point value used to calculate the error is specified as a time interval too. The ISR also calculates new raw (no K factors applied yet) Error, I and D values for the motor control loop, saving these values in RAM for the high-level program to read. The ISR code only runs when the rising edge of a tacho pulse triggers the interrupt. That means the raw data values in RAM are updated after every tacho pulse.
The tachometer ISR does all the heavy-lifting for the PID algorithm, leaving the Forth code to handle the application of the K factors and the scaling of the numbers to create a control value for the PWM servomotor driver. It's split this way to allow maximum program flexibility. The ISR forms part of the embedded FORTHdsPIC environment programmed into Flash memory on the target system. The high-level Forth code is provided by the user, usually downloaded into RAM as a text file from a host PC. The user code can thus specify which combination of P, I and D is to be used, the K factors and the Set Point value.
The first five lines in Listing 1 are 'Colon-Definitions' of new words to be used later in the program. LIMIT is used to restrict the numeric range of the top stack item. PROP, INTL and DERV each perform the combined scaling and multiplication by K factors of the raw P, I and D variables, leaving the processed result on the stack. PID performs the summation of these three components.
Next comes the definition for the top-level program containing the control loop which will run continuously once MAIN <return> is typed at the command prompt. The first few lines of MAIN only execute once, initialising some variables and starting the motor running for 60ms before the loop 'closes'. See 'Practical Problems' below.
Finally, we have the PID control code contained within the BEGIN …..AGAIN loop structure. Remember, a new control value is only calculated when the tachometer ISR returns with updated values. Hence the BEGIN ….UNTIL loop exits when the PULSE flag has been set by the ISR. It is then cleared and the word PID performs the calculation to create a new control output. Just as a precaution against negative or oversize control values, LIMIT executes before the new value is sent to the PWM continuous-rotation servomotor controller routine CSERVO. And that's it.
The relatively imprecise measurement using wheel-mounted tachometers can be drastically improved when a reduction gearbox is used between motor and wheel. If the tacho sensor is on the motor output shaft, instead of the wheel axle, then it will rotate n times for every rotation of the wheel, where n is the gear ratio. So, if the tacho disc has 20 slots and the gear ratio is 100 to 1 then there will be 2000 pulses generated for each wheel rotation. Naturally, this assumes that the processor can cope with the frequency of pulse interrupts!
The effect of invalid tacho data for the first few pulses after starting from rest can be reduced by allowing the motor to run open-loop for a short interval. I found 60ms was long enough before using the tacho data in this case. The motor control value used for starting should be that produced by the PID loop when the motor is running at the set point speed. This can be found by running a small program which runs the motor open-loop with a particular setting and reads back the corresponding speed from the tachometer to the PC. A 'calibration curve' can then be drawn for a range of control inputs. In the demonstration program, a control value of 30 approximately corresponds to the set point speed of 1800. In order to reduce startup hiatus further, the Integral value can be pre-set to its target value. Now when running at the target speed, ERROR = 0, so the only non-zero component amongst P, I and D is the Integral. That means in this case, INTL yields a value of 30. Given that a Ki factor of 0.01 is being used, the raw data variable INTEG should be initialised to 3000.
The numbers used in my demo program apply to a robot' jacked-up' with its wheels clear of the ground. In other words, the motor is running with little or no load. On the ground, the motor will have to work a lot harder: in order to reach the target speed, a larger control input will be required. This should be no problem provided you've selected motors powerful enough to suit the application with some margin to spare. Matching motors and drive train to the likely load is a vital first design task and it has to be got right (See 'Building the Powertrain' below). The PID controller must be able to crank up the motor power to deal with the worst obstacle likely to be encountered if it's to maintain the maximum set point speed selectable.
Years ago, someone discovered that by replacing the position feedback potentiometer with a manual pot, and after breaking off a plastic stop that prevents the output shaft from rotating fully, you created a continuous rotation 'servo'. The standard PWM signal now controls the speed of shaft rotation from maximum 'reverse' with 1ms pulses down to zero at 1.5ms and back up to full forward speed at 2ms. But, and it's a big but, the built-in feedback control has been lost. The manual pot is a 'zero-set trimmer' to ensure no rotation when 1.5ms pulses are applied to the control input. The up-side is that you have a motor that can be connected directly to a microcontroller GPIO pin and controlled 'digitally' via the chip's onboard PWM hardware. Most microcontrollers offer multiple channels of PWM, only requiring small amounts of code to get them up and running. Even better, continuous rotation servomotors are widely available 'ready-made'.
When a more powerful PMDC motor is used, a special driver interface will be needed between it and the microcontroller. Brushed DC motors with stall currents of about 1 to 2A are widely available and cheap. This size of motor is ideal for educational purposes and for prototyping concepts before scaling up in power and cost. PMDC motors are easy to work with because their characteristics are so linear (Fig 1).
Notice how the no-load motor speed is proportional to the applied voltage. The Torque or turning force of the motor is inversely proportional to the speed. This is an excellent characteristic because it means that maximum torque is available just as power is applied to the motor. The motor current follows the same line and falls to a minimum (not quite zero) when the motor reaches its maximum speed with no load.
A lot of people are tempted to connect the motor directly to the wheel and achieve low speed by using a fixed low motor voltage and no feedback control. The torque curves show what happens when you do this. There is, in fact, an infinite number of those blue torque lines, one for each possible voltage. If you choose to operate at No-Load Speed S1 corresponding to Voltage V1 then the maximum Torque you can get is T1 – a fraction of the motor's capability at full speed. The robot probably won't start to move because even the stall torque is not enough to overcome 'stiction'. Better to operate at a higher no-load speed S2 and use a reduction gearbox to achieve the desired road speed. Note that the motor will now be turning at S1 rpm because it's running with a load. It's a win-win situation here: the gearbox reduces the speed but it also multiplies the torque available at the wheel by the same ratio.
If no sort of feedback speed control is applied, the motor voltage V is kept constant. When the robot starts to climb a hill, the amount of torque needed at the wheels rises and the operating point moves up the torque line for voltage V. As a result, the motor speed falls. If the slope steepens, the speed will continue to fall until the motor 'stalls' when the maximum torque is achieved, for that voltage. Clearly, if we increased V we would get more torque, provided the rated maximum is not exceeded. This load-dependent variation in speed is nevertheless, undesirable.
PID control provides a means of automatically changing V in response to varying loads, by sensing the speed with a tachometer and attempting to keep it constant. Now when the load increase is detected by measuring a drop in the speed, the control loop increases V to compensate. In terms of the graph, the operating point moves onto the next curve for the increased value of V. This new curve to the right of the old, has higher torque values for the same speed. Hence feedback control allows the full performance of a particular motor to be exploited.
The principle of PWM as described in the above video is that power is applied to the motor during the pulse and disconnected for the rest of the cycle. Turn off the PWM altogether and the motor freewheels continuously until frictional and other losses bring it to a stop.
Now it's time to look at the drivetrain. A planetary rover is not going to travel very fast and it can't have huge, heavy motors either. The powertrain will need to supply a great deal of torque to the wheels, however, so that the rover can climb slopes and over obstacles. Exploration of unknown terrain requires slow but accurate movement.
For example, say we propose to use a brushed DC motor with a no-load speed of around 6000rpm. It will be selected to have enough torque at a minimum-load speed of around 1500rpm when the rover is running on the flat. The wheel speed needs to be brought down to a more sedate pace, perhaps 10rpm requiring a gearbox ratio of 150:1. This should allow the PWM signal to have a minimum-load duty cycle of about 50%. We have built in an 'overhead' for the PID control loop to cope with climbing slopes and over obstacles without slowing down. It also ensures at least half the maximum stall torque is available to overcome friction (stiction) when starting from rest. Naturally, the torque required at the wheels of the finished rover will have to be estimated before motor selection takes place to ensure these numbers work.
When it comes to gearboxes there are three types to consider: the familiar parallel-axle format, epicyclic (also called planetary) and Harmonic Drive (also called strain wave gear). If you really are building a Mars rover then the last option, perhaps supplemented with epicyclic gears will be your first choice. The Harmonic Drive is a relatively recent invention and offers great performance and small size, but you may need to be working for a Space agency to justify the cost. For a small lab robot, there are plenty of motor-gearbox sets on the market for a more reasonable price.
What about brakes? In general, unless the rover is likely to be descending very steep slopes, then simply turning off the PWM should be enough to bring it to a stop. If gravity starts to have too much influence and the robot starts to roll away out of control, then Dynamic Braking can be used with a PMDC motor. Note I say 'roll away', not 'slide away'. If the latter happens, just hope for a soft landing. Some H-Bridge motor control chips, like the L298, have a dynamic brake function. When selected, the PWM is disconnected and the motor terminals are shorted together. The motor becomes a generator and a very large current flows in the reverse direction through the windings. The suddenly reversed magnetic flux brings the armature to a very rapid stop. It can't act as a parking brake, but while the short circuit remains it will to some extent resist further motion. If that's no good, then a mechanical brake will be needed – as on the Curiosity rover.
There is a further option: use a Worm Drive in your reduction gearbox. The basic worm/pinion gear set is non-reversible and with the worm on the motor shaft, should the motor stop, the drive will lock up.
The picture at the top is a 'selfie' taken by the NASA Curiosity rover on SOL (Mars day) 1315. The jagged holes in the middle wheel 'tyre' represent serious damage. Testing on Earth before launch suggested these wheels could withstand driving over the sharpest, hardest rocks without puncturing. After a lot of analysis, it was determined that there was an unforeseen flaw in the mobility system. Here is the story.
I'll look at a number of mobility systems: some which steer smoothly and others which just 'skid-steer' like a tank. Why do NASA rovers all have 'rocker-bogie' suspension and how does it work?
I'm still struggling with going downhill - I've even swapped out the drive motors to pair of Ampflow A28-400-F48 with Vex victor BB controllers. I'm thinking it's either the 100:1 wormdrive gearbox or the fact I'm using a 2.2kw invertor generator and it objects to the back voltage. It does however work ok if I use a brake and drive the machine lightly against it. Any ideas?
@TegwynTwmffat I'm guessing that the motors speed up as you start to go downhill. From what you say, I think this is what's happening: Those VEX driver modules contain a PID control loop designed to maintain a set motor current and hence torque no matter what the load. It's meant to simulate the throttle action on a car. When the load reduces as the vehicle starts to go downhill, the motor current falls and the PID loop compensates by increasing the voltage and hence the speed. To maintain the speed constant, you must back off the throttle. The PID loop in my article does this for you because it measures speed not current. Your robot behaves like a basic IC-engined car; my robot behaves like the same car, but with cruise-control. I think that's why applying a mechanical (foot)brake works, by maintaining the torque level. It should be possible to put a tachometer-fed speed control loop based on a microcontroller around your driver modules. It may need some careful tweaking of K constants to avoid the loops 'fighting' each other however! Hope this helps.
@Bill Marshall Thanks for reply - it helps! I'm going t add an optical encoder to the machine and apply PID. I've further researched the problem and seems like it might be because the motor starts to act like a generator and send a higher voltage back to the VEX controller than it gives out. t might be cured by adding a small battery in parallel with the switching power supply which should absorb some of the spare electrons flying about.
Part 1 covered the theory behind PID control – a widely used method of ensuring that the mechanical output of an actuator (speed, position, etc) matches the value demanded by its controlling program. Now let's look at practical aspects in detail.
Updating FORTHdsPIC for real-time control of a wheeled mobile robot based on the Parallax BoE-Bot hardware.