 |
|
Software Speed Control for an R/C Servo
Or, how to make a Growbot
move in a straight line.
Theodore Johnson
Introduction
R/C Servos are commonly used as the drive train for small simple robots. Their popularity is due to the packaging of a motor, a gear train, and drive electronics into a small and inexpensive package. R/C servos must be hacked to enable continuous rotation. Part of this hack bypasses the servo feedback mechanism, making speed control difficult. In this article, we show how the rotation speed of a R/C servo can be controlled well enough to make a Growbot move in a (sort of) straight line.
Background
A year ago, I received a Growbot as a gift, sparking an interest in building robots. The Growbot is a simple but nicely packaged robot, using a Basic Stamp as the controller and R/C servos for the drivetrain. I quickly became frustrated with the limitations of the Basic Stamp, so I replaced it with an Atmel 8535. However, I remained frustrated – I could not get the Growbot to move in a straight line.
An R/C servo (see http://www.seattlerobotics.org/guide/servos.html, http://www.seattlerobotics.org/encoder/200106/16csscnt.htm ) is intended to be an actuator in a model airplane, car, etc. A radio receiver on the vehicle creates a pulse train for the R/C servo, typically at 50 Hz. The duration of the on pulse determines the position of the servo. A short on pulse (e.g., 1ms) moves the servo to the far left, while a long on pulse (e.g. 2ms) moves the servo to the far right. Intermediate length pulses correspond to intermediate positions.
The output shaft of the R/C servo is attached to a potentiometer, which provides the feedback that allows the servo electronics to match shaft position to on pulse width. R/C servos are popular because they put all of the drive mechanics and electronics in a small and easily mounted package. They need only the (logic level) pulse train for control, making interfacing with a microcontroller simple. Finally they are mass produced and therefore cheap.
Some types of R/C servos can be hacked to enable continuous rotation. One part of the hack is to disconnect the output shaft from the potentiometer, which disables any feedback. The potentiometer is either centered, or is replaced by a pair of resistors which provide centered feedback to the servo electronics. The R/C servo can be fooled into continuous rotation, either continually moving the output shaft to the left (e.g., backwards) to the right (e.g., forwards). Because of variations in resistor settings, drive electronics, motor efficiency, gear train stickiness, etc., the rotation speed can vary significantly between servos (these are not precision devices).
The servos on my Growbot rotate at slightly different rates, and thus the robot moves in large circle rather than a straight line. I explored the weblore for techniques on R/C servo speed control, and found two common themes:
1) The rotation speed can be varied by varying the length of the on pulse.
2) But doing so is nearly impossible – there is no effective way to control the speed of an R/C servo using software alone (i.e., without replacing the drive electronics).
The theory behind 1) is that the drive electronics will move the output shaft faster if it has further to go. However this is only true for fine positioning. For most of the range, the servo will drive the motor at the maximum rate. Since the speed of servo rotation can only be varied in a small range around the middle pulse (which depends on the resistor settings …), most people eventually come to conclusion 2).
Software speed control, revisited
The drive electronics on an R/C servo are crude, but well suited to their intended application. A single positioning pulse will not drive the servo to the indicated position. Rather, the drive electronics will create an appropriate output pulse to drive the motor. The intensity of the output pulse dies rapidly, hence the need for a pulse train rather than a single pulse. The pulsing can be felt when holding a driven R/C servo.
While the pulse-train nature of the R/C servo drive is a complication in many applications, we can exploit it for speed control. By varying the duration of the off pulses we can drive the servos at varying intensities. We will vary the duration of the on pulse only for direction control. The effect can be seen in Figure 1, in which we hold the on pulse steady and vary the duration of the off pulse for the right and left servos on the robot (the ticks are counted using a wheel encoder, discussed further below). The rotation speed of the servo is proportional to 1/(off pulse) as long as the off pulse is greater than about 20 ms. In the restricted range (20 ms .. 100 ms), the rotation speed is nearly linear in (100ms – off pulse). This fact suggests that we can even use a PID algorithm to control the R/C servos!
Figure 1. Rotation speed vs. off pulse duration.
Building a Quadrature Encoder
During a first attempt at this project, I used an encoder wheel (see http://www.seattlerobotics.org/encoder/200109/dpa.html) with a single IR emitter/detector pair. If the wheel paused near the boundary between a black and a white strip, the detector would bounce and register several hundred spurious ticks. A quadrature encoder uses two pulse trains, 90 degrees out of phase. In a single-interrupt detection scheme, one pulse train interrupts the processor while the other is used to sense direction (see Figure 2). If the value of the interrupt line is the same as (different than) the sense line at the time of the interrupt, you are counting up (down), or vice versa. Quadrature encoders are naturally debounced – a transient on the interrupt line will cause a paired up-count/down-count.
Figure 2 Single-interrupt quadrature encoder pulse trains.
One difficulty in creating a quadrature encoder is to precisely place the IR emitter/detectors so that their pulse trains are 90 degrees out of phase. However, there are two tricks to that make the manufacturing simpler. The first is discussed in http://www.seattlerobotics.org/encoder/200109/dpa.html : use an encoder wheel which has an odd number of black/white strip pairs (that is, the total number of strips should be divisible by two but not by four). Then, detectors placed at 90 degree angles to each other will be 90 degrees out of phase, and we can use the perfboard pattern to place the detectors are right angles. The second trick is to notice that you don’t really need that much precision. With a single-interrupt detection scheme, the pulse trains don’t need to be 90 degrees out of phase, just out of phase (e.g. Figure 2). So we can be a bit sloppy. A two-interrupt detection scheme will give you twice the resolution, but it consumes two interrupts and requires precision in detector placement to be useful.
One of the two quadrature encoders that I built is shown in Figure 3. The emitters are connected in series, with a 100 ohm current limiting resistor. I used the perfboard to place the emitter/detectors at right angles (the top emitter/detector was not flush with the perf board after soldering, so I glued it in place). I used washers to shim the perf board so that the emitter/detectors are just far enough away from the wheel (shown to the left). I made the screw holes in the perfboard large enough to move the perfboard right or left a few millimeters. The encoder wheel has 34 segments, printed using the postscript file found at http://www.seattlerobotics.org/encoder/200109/dpa.html. If you do not have a postscript printer, install and use ghostscript, http://www.cs.wisc.edu/~ghost/.
Figure 3 Quadrature encoder hardware.
Although the pulse trains do not need to exactly 90 degrees out of phase, the closer the better. The on pulses might be more or less than a 50% duty cycles, and the phase shift between the interrupt and sense pulse trains can vary as the encoder wheel turns. In fact, centering the encoder disk precisely on the wheel is my biggest problem with these encoders – if the encoder disk is not centered, the phase shift between the interrupt and sense pulse trains will vary as the wheel turns.
Software Speed Control
The C-language program which drives the robot is here. The main() routine contains three programs to run, selectable at compile time. The program starting at line 935 produced the data for Figure 1. The program starting at line 897 tests the quadrature encoders. One function of this program is to show the current state of the (right or left) encoder pulse train, very useful for debugging. The program will also display the quadrature encoder counts as interpreted by the external interrupt handlers at lines 421-461. The first IF statement in these interrupt handlers interpret the encoder pulse train by the logic described in Figure 2, while the second IF statement enables interrupts on both rising and falling edges.
The pulse train which drives the R/C servos is the Timer1 interrupt handler at line 252. This servo driver is more complex that other servo drivers (e.g., http://www.seattlerobotics.org/encoder/200106/16csscnt.htm ) because I need to vary the length of both the on and the off pulses with a reasonable degree of precision. The interrupt handler and its associated driver code (lines 181-248) is essentially a pair of timers that fire state transitions. The code is complex, written in assembler language, and uses dedicated CPU registers to ensure that it executes within the 16 us resolution of Timer 1. For the purposes of driving continuous-rotation servos, this routine is overkill. The next slower setting of Timer 1 would produce 64 us ticks, which is more than sufficient and would have allowed the routine to be written in C.
Line 650 is a heartbeat timer with a .5 ms resolution, for firing lower-granularity events. The only event is to adjust the PID setpoints and power settings. It is probably not a good idea to execute these routines in the interrupt handler, but it simplifies the main routine.
Lines 470-648 implement a P-only motor control algorithm. Struct motor_str (line 473) defines the state of a wheel: its setpoint (in encoder ticks), the speed at which to advance the setpoint, controls for moving to a particular position, and the last power setting (for debugging and monitoring). User controls are to set the motor moving with no stopping point (the set_speed function, line 532), or to move to a particular stopping point (the move_to_pos function, line 550). The setpoints are adjusted by the adjust_setpoint function, line 578, which is called by the heartbeat timer. If the motor in question is intended to move to a stopping point, then move_to will be true in its control block. If so, the speed will be set to zero once the stopping point is reached.
The motor power is updated by the update_motor_power function, line 598. The “power” to be applied to the motor is proportional to the difference between the setpoint and the wheel position. The sign of the power determines the length of the on pulse, and the magnitude of the power determines the length of the off pulse. The servo pulse train is disabled if the power is to low – low power corresponds to a long off period, and an off period longer than the motor power update period is not useful.
Results
A hacked R/C servo is hardly an ideal device for fine positioning or speed control. Because each on pulse jerks the motor forwards or backwards, the movement is uneven at lower speeds. The jerkiness of the movement also makes wheel slippage more likely. Speed control is limited to a narrow band, and one must accept a significant dead spot around the stopping position to determine that the movement sequence is finished.
To test if the robot will now move in a straight line, I used the program in the main routine starting a line 850. This code makes the robot move forwards about three feet, then return to its starting point. If the floor and wheels are clean (to minimize wheel slippage) and the encoders are not acting up (e.g., the encoder disks are not well centered as is evident from Figure 3, and the long amount of time spent processing the heartbeat doesn’t help), the robot will usually return to within 4 inches from its starting point.
A four inch error margin might not seem impressive, but it shows that speed and positioning control is in fact possible through software control of an R/C servo. The simplicity of the hardware makes experimenting with speed and positioning control algorithms much easier.