This paper describes the servo and motion control techniques used in my robots Dilbert and Dilbert II. Both robots use small DC permanent magnet motors and some sort of encoder scheme that allows the control programs to measure, in absolute terms, how much each motor shaft turns.
Dilbert uses modified Hitec R/C servos as the drive motors. The servos are gutted of all electronics and I only retain the motor and gear train. Dilbert also has a home made encoder system printed directly on the drive wheels with a home made quadrature readout using two Hamamatsu photo reflexive detectors. The Hitec servos are rated at about 1.2 seconds per revolution (RPS) at 4.8 v (standard R/C application). On Dilbert the motors are driven from a 9.6v supply and the motors spin the output shaft at a little over 2 RPS.
Dilbert II uses modified Maxon gear head motors from some surplus thermal printers. The original motors are gears down quite a bit, so I removed two stages of gearing. I also turned a custom axle that is press fit on the output shaft that has a commercial encoder wheel attached. Dilbert II output shaft rotates roughly 12 RPS with a supply voltage of 14.4v
Dilbert has a 4" diameter wheel with 45 encoder stripes for a total of 180 counts per revolution, or, about 1/15" per count. Dilbert II has 2" wheels and a 400-stripe encoder wheel. Due to processing limitations Dilbert II has a total resolution of 800 counts/revolution, or about .00785" per count!
Both Dilbert’s use the L293D, or equivalent, H-Bridge motor driver chip. In both cases the basic PWM rate is ~15 kHz. I use the locked-antiphase drive technique where a 50% PWM rate is Zero motor drive (motor always driven: 50% one way, 50% the other way makes for zero net drive). This works great on Dilbert, but on Dilbert II the Maxon motors have very low inductance so the idle current is roughly ~150 ma – about one 6th the current draw at 100% power. On Dilbert the same idle current level is only ~15 ma. Dilbert II could use a PWM rate of 40-60 kHz, but the micro controller doesn’t support those frequencies.
Servo Feedback Control
A servo is simply a mechanism that slaves some sort of action to some sort of input. A toaster oven is a servo: you set the temperature and the internal mechanism works to keep the oven temperature at the selected value. When talking about mechanical systems, typically some sort of input motion or signal is used to control the output motion of the system using some sort of feedback to maintain the output at a level selected by the input. The accelerator of a car is not a servo system because there is no feedback controlling the speed of a car. The operator provides that. A cruise control, however, is an example of a servo system. The cruise control measures the speed of the car, compares that to the desired velocity and adjusts the accelerometer to keep the speed of the car constant.
To precisely control the position of a motor output shaft some form of absolute position feed back is required. A typical and common method of obtaining precise, absolute shaft position information is via the incremental encoder system.
There are other, less precise, feedback mechanism that can be used to control the output of a motor, but since this paper describes precision, absolute, motion control systems, they will not be discussed.
With the encoder systems attached to the wheels of both Dilbert’s, precision position information is available at all times. There are several things that can be done with the position information.
Velocity Control
A common control mechanism, for small robots, is to control the velocity of a motor. On a typical robot, differential drive is used for steering. In order to go straight both motor shafts have to be turning at precisely the same rate (and the wheels have to be the same diameter as well…). With an encoder one can count how many clicks have gone by in a period and uses that as an indication of velocity. The error between the velocity set point and the actual velocity is used to control the power levels to the motors. In the book "Mobile Robots, from Inspiration to Implementation" there is a very elegant velocity control algorithm that not only controls velocity, but forces both wheels to turn the same amount even if one can’t make it’s velocity set point.
Velocity control often has an integral error term. That is, the algorithm accumulates the error and does further adjustments to the power levels to the motors to correct for and eliminate any velocity error accumulated over time. This might be hard to understand, but imagine if one wheel took just a bit longer to get to speed than the other wheel. During this time the robot would turn ever so slightly toward the slower wheel. Including the integral error term would tend to straighten out the curve over the next small period of time by making the slow wheel go a little faster until the original error was cancelled out.
One of the problems with Velocity control is how to measure very low velocities. In order to make the algorithms easy to code, most programs work on a fixed time interval. When the velocity starts to be a low number of counts per interval, the control over the motor power levels gets choppy. Various approaches have been used to counter this: a variable measuring period or measuring the period between counts. Both techniques require much more processing power since many factors, that would reduce to constants with a fixed time period, need to be calculated with each cycle.
Position Control
Position control loops are very much like the traditional R/C servos: you input a desired position and the servo logic drives the output shaft to that position with whatever force is required. Normally one doesn’t think of position control as being appropriate for doing motion control on small robots, but, in fact, it is a very good technique. The trick is to profile a series of positions that the robot is to be in, feeding that string of positions to the servo on a periodic basis. This is called Position Profiling.
With position profiling the forward velocity of the robot can be any arbitrary low amount. It might be that the motor shafts advance only every so often, but they will advance smoothly and the robot will crawl along. With profiling acceleration becomes easy to do as well. Acceleration will be discussed later in this paper.
DC Motors
Small DC motors are easy to model and control. Fortunately for us, they can be reduced to simple terms so our control algorithms are easy to code. The permanent magnet DC motor can be modeled as a device that produces torque proportional to the current flowing through it. It also produced a voltage proportional to the rotational velocity. One last bit of modeling completes the picture: there is a small series resistance in the model. Hence, if one stalls a motor, the current draw and torque produced will be the supply voltage divided by the resistance.
Commonly one reads that the RPM of a motor is proportional to the voltage across it’s terminals – and for most purposes that is true: at any given voltage, the motor will spin up in speed until the generator portion of the motor model matches the supply voltage. At that point no more current will flow into the motor and it will produce zero torque. Of course, there is some amount of friction, so there will be some amount of torque required to spin the motor, thus some amount of current needed. This current causes a voltage drop across the small series resistance in our motor model. This voltage drop takes away from supply voltage and causes the motor to spin a bit slower than the supply voltage would indicate.
In industrial controllers one typically sees a variety of control methods: constant speed, where the applied voltage is "adjusted" for the IR voltage drop across the internal resistance (the controller measures the current though the motor, calculates the voltage drop across the internal resistance and bumps the supply voltage to compensate); Constant torque, which is simply a constant current supply, and the incremental encoder or tachometer feedback systems which, of course, give absolute control over position and speed.
PWM
A typical way to control small DC motors is via PWM, which stands for Pulse Width Modulation. PWM comes in a variety of flavors. The only flavor that I have used has a fixed frequency with a variable on/off cycle. The hardware in Dilbert and Dilbert II has a selectable 8, 9 or 10 bit PWM. The "on" portion of the cycle can be set to any value from 0 to 2^n-1, or, for 8 bit PWM: 0-255. This means that there are 255 steps between 0% and 100% on, during a PWM cycle. As mentioned earlier the Dilbert’s both use locked-antiphase drive, which means that the control range is really –127 to +128. I find that 127 steps of power control are more than adequate for controlling small motors.
H-Bridge
The H-Bridge is the primary means for driving a small DC motor in the forward and reverse directions. A typical small motor driver chip is the L293D or the L298. These are very old designs, but they are cheap and work well. There are lots of newer chips available, but, fundamentally, they all do the same thing: They have a direction input and an enable. By selecting the appropriate directions for the two sides of the H-bridge and enabling the outputs, the computer can control the direction of the motor. By switching the direction and the enable bits the controller can control the direction as well as the amount of power delivered to the motor
There are fundamentally two ways to drive a small motor with a PWM signal: Sign-Magnitude and Locked-antiphase.
Sign-Magnitude
In sign-magnitude the controller sets the direction with the directions bits and then varies the amount of power to the motor with the PWM signal. Typically switching the ENABLE bit of the H-Bridge driver does this. The ratio of ON to OFF determines the amount of power going to the motor.
There is an alternative way to drive the motor: The Enable bit is left ON all the time. One half of the H-Bridge is set forward or backwards and the PWM signal drives the other half of the bridge. The effect of this technique is that the motor is alternatively driven and shorted with the ratio set by the PWM signal
Locked-antiphase
Locked-antiphase is similar to the second version of Sign-Magnitude in that the enable line is always ON. The PWM signal is used to switch both sides of the H-Bridge such that the motor is driven forward or backwards at all times.
Because both forward and reverse are determined by one signal line the amount of control available is only ½ the apparent PWM control range: Those numbers below 50% duty cycle are reverse and those above are forward.
One of the big advantages to locked-antiphase is that only one line is needed to completely control the H-Bridge motor driver. If you have an I/O constrained MCU, this might be a good option.
Brake vs. Coast
When a DC motor is spinning, it generates a voltage across the terminals, much like a generator. This voltage is a characteristic of the motor and can be found in the specification sheets as something like Volts/RPM. This voltage is also called the Back Electro-Motive Force (EMF). Back EMF can be used to re-charge the power source of the motor control circuit, or, it can be used to brake the spinning of the motor. With an H-Bridge this is accomplished by driving both sides of the H-Bridge the same direction. It is essentially shorting the motor terminals together.
On the L293, and similar devices, the ENABLE bit serves to disconnect the driver from the motor. When this happens, the motor simply spins and generates a voltage. The only time this voltage might be turned into a current is if the back-EMF exceeds the supply voltage and the forward voltage drop of the protection diodes in the H-Bridge driver. When the driver is ENABLED and shorting the motor terminals together, they are not really shorted together: in fact, one is held high, or low, and the other one conducts through the protection diodes in the driver. So, the motor will generate current and brake as long as the back-EMF is greater than one diode + one Vce voltage drop (about 1.1v).
When in brake mode, the motor tends to resist turning. Shorting the terminals together is essentially connecting a very low impedance load to the motor. When a battery is connected to the motor, it too looks like low impedance because, regardless of the current flow or direction, the battery voltage stays the same. The definition of impedance is R = delta V/delta I. Since the delta V stays constant (ideal battery) the resistance (impedance) is zero. The reason the motor only spins up to a certain amount is that the Back-EMF counters the voltage of the battery until there is nothing left to drive current through the motor.
When driving a motor with a PWM signal if one uses Locked-Antiphase, or the second Sign-Magnitude technique, the motor is always connected to a low impedance source (either shorted through the protection diodes, or connected to the supply voltage). The result is that for a given speed the motor will tend to resist speeding up or slowing down for exactly the same reasons that shorting the terminals together cause the motor to be in brake mode.
Driving the motor in Four Quadrants
Locked-Antiphase drives the motor in what is knows as "Four Quadrant Mode". The four quadrants are: Motor accelerating and generating power, Motor accelerating and consuming power, Motor decelerating and generating power and finally, Motor decelerating and consuming power.
In all cases either the motor is consuming electricity and trying to turn a load, or the motor is generating electricity and trying to "hold back" the load.
With Sign-Magnitude with the PWM controlled by the ENABLE line, only the second and fourth quadrants are being utilized. There is no control over the motor in the first and third quadrants. That is when the BRAKE mode comes in handy: for the first quadrant the load is accelerating (think of the robot going down a steep hill) and needs to BRAKE to keep the acceleration to the desired value. The third quadrant is just the opposite: the robot is going up a hill, but you want to slow it down faster than gravity; again, the only way to do this is by BRAKING.
When the motor is always connected to a low impedance source (e.g. locked-antiphase, or the second sign-magnitude configuration) all four quadrants are covered. As soon as the motor enters either the first or third quadrant, the H-Bridge absorbs the generated current and causes the necessary braking action to take place. This is all automatic with no special effort needed on the PWM controller.
Encoder Feedback
All quadrature decoders have at least two signal lines: the A and B lines. An SRS Encoder article has an excellent description of how quadrature signals are generated and decoded. (look about half way down)
Dilbert
In Dilbert the detectors are two photo-reflexive sensors mounted so that they detect the stripes roughly 50% out of phase with each other. That is, roughly ½ of a stripe after one detector senses the strip, the second sensor will detect it. The resulting signal, from the two detectors, looks like two square waves 90 degrees out of synch with each other. Because Dilbert is relatively slow and has few stripes it is possible to do quadrature decoding by polling the signal lines. See the photo gallery for Dilbert (Firebot) for images of the encoder.
See Appendix A: Quadrature Decoding State Machine for the source code example.
Dilbert II
In Dilbert II the resolution of the code wheel and the RPM of the wheels makes a polled state machine impossible. In the Atmel 8515 processor there are two external interrupt lines. These lines can be switched, under program control, from rising edge triggered to falling edge triggered. With this control full quadrature encoding can be implemented using interrupts for the A and B lines. However, in Dilbert II, there are two encoders that would require four interrupt lines, so something else has to be done. The decoding in Dilbert II is pseudo quadrature decoding. The A line from each encoder is fed to one of the interrupt lines. Upon either a positive or negative edge the interrupt routine examines the state of the B line to determine if the count is increasing or decreasing.
Pseudo-decoding works with half the resolution of full decoding. Since the positive and negative edge detection flips with each interrupt mechanical jitter on the code wheel will not generate spurious counts.
See Appendix B: Interrupt driven pseudo-quadrature decoding
PID
PID stands for Proportional Integral Derivative. It is a basic filter mechanism used to control some output based upon the aggregate function of factors. Here are some references for more details: PID and Servos
PID is actually a differential equation solved in the frequency domain – something way beyond my comprehension and this paper. However, with some assumptions and simplifications the math becomes very simple and lends itself to rational real world explanations.
The major simplification is to perform the algorithm at a fixed time rate. Then, everything having to do with time gets rolled up into the constants. Now the basic formula becomes:
Output = Kp * Proportional error – Kd * Derivative error + Ki * Integral error;
Let’s walk through the formula:
- The proportional term is an output value based upon the difference between the set point and the actual.
- The derivative term is based upon the first derivative of the proportional error, or, the velocity of the error. I.e. is the error getting smaller or larger as time goes by.
- The integral term is the opposite of the derivative term: it is the sum of all errors over time. The integral term is included to allow the controller to advance the output such that all errors are eventually cancelled out.
In a practical implementation of the PID algorithm, if one is using integer math, is to scale the terms (e.g. multiply by a factor) and divide the output by that factor. In this way fractional values can be used. In the Dilbert’s, the output factor Ko is used:
return (Output/Ko);
Tuning PID controllers
See the following references for more information on PID and tuning: Adjusting PID Gains
A quick and dirty way to tune is as follows:
Set Kd and Ki to zero and Ko to 1.
Increase Kp and move the robot with an impulse function (fancy way of saying, force it forward a bit and let go). If the robot races away, then you have the sign of your P term inverted: either swap the connection to the motor, or, use a negative number for Kp.
Increase Kp until the robot starts to oscillate, that is, it doesn’t just move towards the set point, but wiggles back and forth around it. The oscillations should die down quickly or you have Kp way too high. Some references suggest 60% of the value that causes strong oscillations is a good place to start.
Now, start increasing Kd. At some point Kd should cause the wriggling to stop and the robot should just snap towards the set point and come to a smooth halt when it reaches it. Kd is a velocity term and it comes to dominate the output function when the robot is near to it’s goal. Since it is subtracted (see above) it causes the power to be decreased, and, even reversed to halt the robot at just the right spot.
If Kd is too big, really bad oscillations will occur. Drop it back. You can fiddle with Kp and Kd to see just how snappy you can make your robot. In any case, if either is too large the robot will oscillate around its set point.
Ki is harder: if absolute position is you goal, you will need Ki. Ki is often the inverse of Kd. If Kd is 10, then you might expect Ki to be 1/10. This is where Ko comes into play. You will need to multiply Kp and Kd by some factor (say, 8) and set Ko to 8, that won’t change a thing. Then you can start to increase Ki to see what effect it has on the stability of your robot.
Basic Motion Control
Basic motion control couldn’t be simpler. In Dilbert II the motor control structure contains several elements for motion control: VelocitySetpoint, Velocity and Acceleration. All numbers are maintained as 8.8 signed binary numbers. It is the fractional numbers that give Dilbert so much control: Velocities can be as low as 1/256 of an encoder count/loop to as high as 127counts per loop. Fractional velocities work very well too: as the fractional part accumulates and rolls over, the encoder set point bumps up by one. This "extra" count happens on a periodic basis depending upon the fractional amount and results in a smooth motion at the desired velocity.
Please refer to Appendix D, basic motion control, for the code details.
The motion control code is called from the PID loop code at the PID loop rate. With each cycle the position set point is modified by the Velocity amount. The Acceleration value is used to ramp Velocity from where ever it is to the new VelocitySetpoint value.
To start motion the code simply has to write into the Motor Control Structure the desired velocity and the Motor Control task takes over. E.g.
Left->VelocitySetpoint = Right->VelocitySetpoint = 0x0A00;
In the above example, the velocity is 0A.00 hex, or 10.00. That means the robot will move forward ten encoder counts per control loop – assuming the motors can propel the robot forward that fast.
Acceleration must be set as well. On the Dilbert’s, acceleration is fixed and initialized during boot time at 0x0070. I picked a value that works well over the range of battery voltages I can expect, and how good the wheels are at gripping the floor. Since the acceleration is less than 1 (e.g. 00.70 hex) it takes two cycles before the robot even begins to move. The first cycle sets Velocity to 00.70; the second to 00.E0; the third to 01.50 and so on.
With a velocity of 10 and an acceleration of 0x00.70 it takes 22 cycles to ramp up to the VelocitySetpoint. Over those 22 cycles the robot moves the sum of (1 + 2 + 3 + … + 22) acceleration units, which comes out to 253 * 0x70 or 0x6E.B0. Since we are only concerned with the whole part, the robot moves 0x6E encoder counts before hitting the VelocitySetpoint. This number is important if Dilbert II is traveling with no particular end point in mind since he will travel that many encoder clicks beyond the point that the control program sets the velocity to zero. So, the control code needs to anticipate stopping and initiate stopping 0x6E counts early when traveling at a velocity of 10.
Most robotic contests have speed, as one of the objectives, so there is no reason, for the most part, to accelerate any slower than necessary. However there are times when it is necessary to alter the acceleration. For example when the robot’s path must describe an arc. This should be simple enough, yes? Just set the velocity of one wheel higher than the other and away it goes. Well, with acceleration that doesn’t work: the slower wheel will reach its velocity set point sooner than the faster wheel. What actually happens is that the robot goes "straight", both wheels accelerating in synch, until the slower wheel hits its set point, then the robot will starts to turn.
The solution is to modify the acceleration value as well: if wheel A turns ½ the speed of wheel B (to get the arc you want), then the acceleration on wheel A should be ½ that of wheel B.
How to travel a particular distance
It is easy to predict when motion will hit the set point velocity or reach zero. That is just the VelocitySetpoint/acceleration. What is harder is predicting when to set the velocity to zero in order to stop on a pre-determined point. The math is a little complicated because you need to integrate the acceleration and set the velocity set point back to zero the integrated amount before the desired stopping point. This is exactly what I did in the previous section in calculating the overshoot for a velocity of 10 with an acceleration of 0x70.
The math becomes simple for line segments of a fixed distance where the set point velocity is reached. All you need to do is set the velocity and wait for "distance/velocity" number of control cycles before setting the velocity to zero. Note: motion does not stop when the velocity set point is set to zero. It takes "velocity/acceleration" number of cycles for motion to stop.
What happens is that while the robot has not moved the anticipated amount during the time interval, it makes up the difference while decelerating back to zero.
How to turn a particular amount
Turning is pretty much the same as moving in a straight line. For a given velocity and acceleration you can calculate the precise number of cycles to rotate the desired amount. With spinning it is very easy since each wheel does the same thing, but inverted relative to each other. For Arcs it is a little different since you need to figure out the differential velocity and acceleration for each wheel. However, once that is done, the number of cycles can be calculated for just one side, the other will follow along in sync.
Appendixes
The following code examples are for the Atmel AVR microprocessors. The assembly code is for the IAR assembler and the C code is for the GCC-AVR compiler.
RTOS system calls are for AvrX, a small RTOS that I wrote for my own robotics work.
The following is the data structure used by the motor control and motion routines. The PWM output is eight bits and is initialized elsewhere (code not illustrated).
Code: Select all
typedef struct
{
long Setpoint; // 24.8 number
long Encoder; // Current position
int8_t Kp;
int8_t Kd;
int8_t Td;
int8_t Ki;
int8_t Ko; // Output Scale
int16_t PrevErr;
int16_t Ierror;
int16_t Velocity;
int16_t VelocitySetpoint;
int16_t Acceleration;
} MotorInfo, * pMotorInfo;
#define MAXOUTPUT 0x7FThis code is from Dilbert which has a slightly different motor control structure and names. Due to the slow nature of Dilbet's encoders (maximum of 360 state changes a second), the code loads, increments or decrements the encoder value and stores it back into the motor control structure.
Code: Select all
;+
;Left/RightEncoderInterrupt
;
;Called from PWM overflow interrupt. Does quadrature decoding of our
;encoder inputs. This gives us 4x resolution on our 45 segment encoder
;wheels (total counts/revolution = 180)
;
;There are a bunch of invalid state changes (state0 -> state2 for example)
; and this code completely ignores those possibilities. If the rate of
; change is lower than the polling rate of this code, then there can't
; be any invalid state changes.
;-
A = 1<<0 ; Encoder A Line
B = 1<<1 ; Encoder B line
STATE0 = 0 ; Increment if 0->1, 1->2, 2->3 or 3->0
STATE1 = B ; Decrement if 0->3, 3->2, 2->1 or 1->0
STATE2 = A + B
STATE3 = A
ProcessEncoder:
clr Xh
sbic SWITCH, Right_Enc_A
sbr Xh, A
sbic SWITCH, Right_Enc_B
sbr Xh, B
ldi Zl, low(RightMotor)
ldi Zh, high(RightMotor)
rcall DecodeState
clr Xh
sbic SWITCH, Left_Enc_A
sbr Xh, A
sbic SWITCH, Left_Enc_B
sbr Xh, B
ldi Zl, low(LeftMotor)
ldi Zh, high(LeftMotor)
DecodeState:
ldd Xl, Z+PreviousState
ldd Yl, Z+Encoder+1
ldd Yh, Z+Encoder
cp Xh, Xl
breq DecodeExit
State0:
cpi Xl, STATE0
brne State1
cpi Xh, STATE1
breq Increment
cpi Xh, STATE3
breq Decrement
rjmp DecodeExit
State1:
cpi Xl, STATE1
brne State2
cpi Xh, STATE2
breq Increment
cpi Xh, STATE0
breq Decrement
rjmp DecodeExit
State2:
cpi Xl, STATE2
brne State3
cpi Xh, STATE3
breq Increment
cpi Xh, STATE1
breq Decrement
rjmp DecodeExit
State3:
cpi Xh, STATE0
breq Increment
cpi Xh, STATE2
brne DecodeExit
Decrement:
sbiw Yl, 2 ; Save a word and a cycle by falling
Increment: ; through
adiw Yl, 1
DecodeExit:
std Z+PreviousState, Xh
std Z+Encoder+1, Yl
std Z+Encoder, Yh
ret This is code from Dilbert II which has a slightly different hardware configuration that Dilbert. The assember used, below, is GAS, or Gnu Assembler, so the directives are slightly different than those in Dilbert. The interrupt rate (state change rate) can be as high as 12000/second, so I reserve memory for the left and right incremental encoder count and clear that out periodically in the main motor control task. Atmel assembler (and GAS) doesn't have an op-code for add immediate, so, I use the subtract immediate (subi) with a negative number when adding. One last note: this is code for just the right side encoder.
Code: Select all
;+
; INT0_Interrupt
;
; Fields the A line for an encoder. Increments/Decrements a
; register counter per the state of the B line. Flips sense of
; external interrupt in preparation for the next state transition.
;
; Because we dont interrupt on the B line, this only does
; 1/2 quadurature decoding.
;
; ~40 cycles/interrupt (including call and return)
;-
.func _interrupt0_
.global _interrupt0_
_interrupt0_:
push R31
push R30
push R29
in R31, SREG
in R30, MCUCR
lds R29, RightEncoder
sbrc R30, ISC00 ; Was this an up or down event?
rjmp INT0_up
subi r29, lo8(-1)
sbis Right_Enc_B
subi R29, 2
sbr R30, 1<<ISC00 ; Switch Trigger to rising edge
rjmp INT0_done
INT0_up:
subi R29, 1
sbis Right_Enc_B
subi r29, lo8(-2)
cbr R30, 1<<ISC00 ; Switch Trigger to falling edge
INT0_done:
sts RightEncoder, R29
CommonExit:
out MCUCR, R30
out SREG, R31
pop R29
pop R30
pop R31
reti
.endfuncThe following C code illustrates just how sparse the PID algorithm can be. Implicit in all this is the periodic sampling rate, or Td. For Dilbert II Td is 10ms or 100hz.
Code: Select all
int DoPid(MotorInfo *p)
{
int Perror, output;
// Set point is an 24.8 number (+/- 1 mile@ 130/inch)
Perror = (p->Setpoint>>8) - p->Encoder;
// Derivative error is the delta Perror over Td
output = (p->Kp*Perror + p->Kd*(Perror - p->PrevErr) + p->Ki*p->Ierror)/p->Ko;
p->PrevErr = Perror;
// Accumulate Integral error *or* Limit output.
// Stop accumulating when output saturates
if (output >= MAXOUTPUT)
output = MAXOUTPUT;
else if (output <= -MAXOUTPUT)
output = -MAXOUTPUT;
else
p->Ierror += Perror;
// Convert output to 0xFF-0x7F-0x00 (Fwd, neutral, Rev)
// Locked anti-phase drive.
return (output - (MAXOUTPUT+1));
}The motion control is very simple. Adding an incremental position to the position set point on a periodic basis essentially sets the velocity. In the following code I broke out the "AddToPosition" so that Dilbert's wall following and line following code could "adjust" the relative wheel positions
Code: Select all
/*+ ---------------------------------------------------------
void DoMotion(MotorInfo *p)
Called at the loop rate to add "velocity" to the set point thus
effecting a motion.
Velocity is ramped up and down by "acceleration"
Velocity and Acceleration are 8.8 numbers. Velocity
is added to the setpoint (an 24.8 number)
-*/
void DoMotion(MotorInfo *p)
{
if (p->Velocity < p->VelocitySetpoint)
{
p->Velocity += p->Acceleration;
if (p->Velocity > p->VelocitySetpoint)
p->Velocity = p->VelocitySetpoint;
}
else
{
p->Velocity -= p->Acceleration;
if (p->Velocity < p->VelocitySetpoint)
p->Velocity = p->VelocitySetpoint;
}
AddToPosition(p, p->Velocity);
}
/*+ -------------------------------------------------------------
void AddToPosition(pMotorInfo, int)
Add an 8.8 value to the current setpoint.
Do this with interrupts off since the motor control
task might be reading the long value (e.g. four bytes)
-*/
void AddToPosition(pMotorInfo p, int16_t o)
{
char s = inp(SREG);
cli();
p->Setpoint += o; // Set point is 24.8 number
outp(s, SREG); // Restore interrupt status
}The following code illustrates the actual working Motor Control task using AvrX, the authors RTOS. This same code could be called as an ISR or Interrupt Service Routine with few modifications.
AVRX_TASKDEF() is a macro that takes the entry point, extra stack space needed and priority as the three arguments and builds the required data structures for C. I set pointers to the motor control structures because the GNU compiler actually produces better code through pointers than direct references to structure members (e.g. 'p->Encoder =' produces better code than 'Left.Encoder =').
Code: Select all
extern MotorInfo Left, Right;
AVRX_TASKDEF(MotorTask, 28, 2)
{
pMotorInfo pLeft=&Left, pRight=&Right;
int8_t LeftEnc, RightEnc;
extern int8_t RightEncoder, LeftEncoder;
MotorInit();
while(1)
{
AvrXStartTimer(&MotorTimer, MILLISECOND(10));
AvrXSetSemaphore(&MotorLoop); // Synchronize other motion tasks
BeginCritical(); // Read encoders
LeftEnc = LeftEncoder;
LeftEncoder = 0;
RightEnc = RightEncoder;
RightEncoder = 0;
EndCritical();
pLeft->Encoder += LeftEnc; // Update motor structures
pRight->Encoder += RightEnc;
DoMotion(pLeft); // Do motion control
outp(DoPid(pLeft), LEFT_DRIVEL);// Do PID and output power level
DoMotion(pRight);
outp(DoPid(pRight), RIGHT_DRIVEL);
AvrXWaitTimer(&MotorTimer); // Wait for timer to expire
}
}