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 canceled 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.

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:

· 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 canceled out.

The constants, Kp, Kd and Ki are just factors, or gain values used to
mix the various terms together to get the resulting equation. These factors will change
based upon the quality of the motors, the weight of the robot and the charge of the
batteries driving the motors. With enough information and proper specifications for your
components it is possible to actually compute the exact values for the various gains. In
practice, for hobby robotics, we just use trial and error to get good enough
values.

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:

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.

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.
You can find the gcc-avr compiler and tool chain here: Atmel for Dummies

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).

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 0x7F

Appendix A: Quadrature Decoding State Machine

This 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.

; 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

Appendix B: Pseudo-Quadrature decoding

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.

;+
; 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

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

/*+ ---------------------------------------------------------

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

}

Appendix E: Top Level Motor Control

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 =').