Where Am I?Where Am I?Where should I be?Where should I be?

Tales from the dark sideTales from the dark side

Peter HarrisonPeter HarrisonMinos 2007Minos 2007

2

New MouseNew Mouse

DecimusDecimus DC MotorsDC Motors dsPIC processordsPIC processor 4m/s top speed4m/s top speed 4m/s/s acceleration4m/s/s acceleration

3

DecimusDecimus

Mass:Mass: 300g300g Motors:Motors: Faulhaber 2224Faulhaber 2224 Gear Ratio:Gear Ratio: 4:14:1 Encoders:Encoders: 512 cpr512 cpr Decoder:Decoder: x2x2 Wheels:Wheels: 45mm dia.45mm dia. Resolution:Resolution: 0.035mm0.035mm

4

New ProcessorNew Processor

Microchip dsPICMicrochip dsPIC 16 bit16 bit single cycle 16x16 multiplysingle cycle 16x16 multiply fast divide – one clock per bitfast divide – one clock per bit very fast – 32 mipsvery fast – 32 mips extensive peripheral setextensive peripheral set free toolsfree tools good range – common peripheralsgood range – common peripherals still a PIC so it is a bit oddstill a PIC so it is a bit odd

5

New ToolsNew Tools

MPLABMPLAB compiler – GNU Ccompiler – GNU C programmingprogramming simulatorsimulator

ICD2 ICD2 programmerprogrammer debuggerdebugger

6

New SoftwareNew Software

All in C for portabilityAll in C for portability clean existing code for GNUclean existing code for GNU 80% Maximus code ports across80% Maximus code ports across new peripheral driver challengenew peripheral driver challenge motor control is the real challengemotor control is the real challenge

7

Position is KeyPosition is Key

Literature often about Literature often about speed controlspeed control position servoposition servo

Mouse needs bothMouse needs both Speed is change of position with timeSpeed is change of position with time Control the position Control the position Speed control followsSpeed control follows

8

Basic Position ControlBasic Position Control

‘‘Simple’ servo application - PIDSimple’ servo application - PID motor holds a set pointmotor holds a set point Proportional control gives standing Proportional control gives standing

error under loaderror under load Derivative control improves response Derivative control improves response

timetime Integral control not needed in a Integral control not needed in a

mousemouse

9

Digital ServoDigital Servo

Set point position is a numberSet point position is a number Encoder counts distance travelledEncoder counts distance travelled Every tick:Every tick:

posError = setPoint – currPositionposError = setPoint – currPositiondelta = posError – oldErrordelta = posError – oldErroroldError = posErroroldError = posErrorpwmOut = kP pwmOut = kP × × posError + kD posError + kD ×× delta delta

Determination of kP and kD is left as Determination of kP and kD is left as an exercise for the readeran exercise for the reader

10

Servo Sums - 1Servo Sums - 1

Each count is 0.035mmEach count is 0.035mm Greatest possible distance in the Greatest possible distance in the

maze is a 13 cell diagonalmaze is a 13 cell diagonal Approx 2940mmApprox 2940mm About 84000 countsAbout 84000 counts 16 bits not enough16 bits not enough Need 24 or 32Need 24 or 32

11

Servo Sums – 2Servo Sums – 2

Use 32 bits, twos complement for Use 32 bits, twos complement for positionposition

Fixed point 24.8Fixed point 24.8 Good for about Good for about ±±293m293m Fractions needed laterFractions needed later kP and kD usefully stored as 8.8 kP and kD usefully stored as 8.8

signed fixed point valuessigned fixed point values

12

Servo Sums - 3Servo Sums - 3

in the PID, use only integer part of in the PID, use only integer part of the distance error and truncate to 16 the distance error and truncate to 16 bits allowing maximum error of 1mbits allowing maximum error of 1mposError = (setPoint – currPosition)/256posError = (setPoint – currPosition)/256

delta = posError – oldErrordelta = posError – oldError

oldError = posErroroldError = posError

pwmOut = kP pwmOut = kP × × posError + kD posError + kD ×× delta delta

even at top speed, we expect error even at top speed, we expect error to be a few mm or lessto be a few mm or less

13

Linear motionLinear motion

Constant velocity is a list of set Constant velocity is a list of set pointspoints

At each tick:At each tick:setPoint = setPoint + velocitysetPoint = setPoint + velocity

velocity is a fixed point, signed value velocity is a fixed point, signed value 8.8 bits8.8 bits

fractional part is needed for fractional part is needed for accelerationacceleration

Effect is a moving targetEffect is a moving target

14

Servo Sums - 4Servo Sums - 4

with an 8.8 bit value for velocity we with an 8.8 bit value for velocity we get numbers in the range get numbers in the range ±127 or so±127 or so

resolution is 0.00013 m/sresolution is 0.00013 m/s absurd but we will need that laterabsurd but we will need that later can now move by 127 counts per tickcan now move by 127 counts per tick that is, 127 * 0.035mm = 4.45mmthat is, 127 * 0.035mm = 4.45mm maximum velocity is about 4.4m/smaximum velocity is about 4.4m/s

15

AccelerationAcceleration

simply a change in velocity each ticksimply a change in velocity each tickcurrentSpeed = currentSpeed + accelerationcurrentSpeed = currentSpeed + acceleration

acceleration is 8.8 signed, fixed-pointacceleration is 8.8 signed, fixed-point next we get to see why the fixed next we get to see why the fixed

point numberspoint numbers

16

Servo Sums - 5Servo Sums - 5

accelerating by one count per tick is accelerating by one count per tick is 0.035mm/ms/ms0.035mm/ms/ms

= 35m/s/s= 35m/s/s the fractional part allows smaller the fractional part allows smaller

increments of accelerationincrements of acceleration minimum is 35/256 = 0.137m/s/sminimum is 35/256 = 0.137m/s/s 4m/s/s is 0.114 counts per tick4m/s/s is 0.114 counts per tick stored as just 29stored as just 29

17

Linear Motion CodeLinear Motion Code

having chosen consistent formats for all having chosen consistent formats for all the parameters, at each tick:the parameters, at each tick:currentSpeed = currentSpeed + accelerationcurrentSpeed = currentSpeed + acceleration

setPoint = setPoint + currentSpeedsetPoint = setPoint + currentSpeed

posError = (setPoint – currPosition)/256posError = (setPoint – currPosition)/256

delta = posError – oldErrordelta = posError – oldError

oldError = posErroroldError = posError

pwmOut = kP pwmOut = kP × × posError + kD posError + kD ×× delta delta

very smooth acceleration over a useful very smooth acceleration over a useful speed rangespeed range

18

Velocity ProfilesVelocity Profiles

starting and end speed are probably starting and end speed are probably non-zeronon-zero

time

speed

vmax

time

speed

vmax

19

Simple Profile GenerationSimple Profile Generation

Only two phasesOnly two phases acceleration (speed limited)acceleration (speed limited) decelerationdeceleration

no mid-point calculationno mid-point calculation caters for variable distancecaters for variable distance endpoint has position and speedendpoint has position and speed only one decision - when do we hit only one decision - when do we hit

the brakes?the brakes?

20

Profiler State MachineProfiler State Machine

caller needs to know when donecaller needs to know when done three states: accelerating, braking, finishedthree states: accelerating, braking, finished simple state machine can helpsimple state machine can help

function updateState()function updateState()if (state == ACCELERATING) thenif (state == ACCELERATING) then

if (brakesNeeded()) then if (brakesNeeded()) then state = DECELERATINGstate = DECELERATING targetSpeed = endSpeedtargetSpeed = endSpeedend ifend if

end ifend ifif (currentPosition >= targetPosition) thenif (currentPosition >= targetPosition) then

state = FINISHEDstate = FINISHEDtargetSpeed = currentSpeedtargetSpeed = currentSpeed

end ifend ifend functionend function

targetSpeed important to profiler actiontargetSpeed important to profiler action start a move with targetSpeed = vMaxstart a move with targetSpeed = vMax

21

Shall we Brake?Shall we Brake?

equations of motion:equations of motion:|v|v××v – uv – u××u| = |2u| = |2××aa××s|s|

v = end speedv = end speed

u = current speedu = current speed

a = accelerationa = acceleration

s = distance remainings = distance remaining

solving for s needs a divisionsolving for s needs a division easier to compare LHS with RHSeasier to compare LHS with RHS if LHS ≥ RHS then brakeif LHS ≥ RHS then brake called in a function brakesNeeded()called in a function brakesNeeded()

22

Braking testBraking test

function brakesNeeded()function brakesNeeded()

lhs = abs(endSpeed * endSpeed – velocity * velocity)lhs = abs(endSpeed * endSpeed – velocity * velocity)

rhs = abs(2 * acceleration * remaining)rhs = abs(2 * acceleration * remaining)

if (lhs >= rhs) thenif (lhs >= rhs) then

return TRUEreturn TRUE

elseelse

return FALSEreturn FALSE

end ifend if

end functionend function

23

Profiler CodeProfiler Code

if (state == ACCELERATING) thenif (state == ACCELERATING) thenif (brakesNeeded()) then if (brakesNeeded()) then state = DECELERATINGstate = DECELERATING targetSpeed = endSpeedtargetSpeed = endSpeedend ifend if

end ifend ifif (currentPosition >= targetPosition) thenif (currentPosition >= targetPosition) then

state = FINISHEDstate = FINISHEDtargetSpeed = currentSpeedtargetSpeed = currentSpeed

end if end if if (currentSpeed < targetSpeed) thenif (currentSpeed < targetSpeed) then

currentSpeed = currentSpeed + accelerationcurrentSpeed = currentSpeed + accelerationif (currentSpeed > targetSpeed) then currentSpeed = targetSpeedif (currentSpeed > targetSpeed) then currentSpeed = targetSpeed

elseelsecurrentSpeed = currentSpeed – accelerationcurrentSpeed = currentSpeed – accelerationif (currentspeed < targetSpeed) then currentSpeed = targetSpeedif (currentspeed < targetSpeed) then currentSpeed = targetSpeed

end ifend if

24

Now for the other wheelNow for the other wheel

wheelchair mice need two drive motors wheelchair mice need two drive motors coordinatedcoordinated

run two profilersrun two profilers not one per wheelnot one per wheel one for translationone for translation one for rotationone for rotation resolve components for each wheelresolve components for each wheel PID for each wheel controls its positionPID for each wheel controls its position

25

Coordinated ControllersCoordinated Controllers

using rotation and translation controllers using rotation and translation controllers makes coordinated motion easiermakes coordinated motion easier steeringsteering co-ordinated turnsco-ordinated turns

all the above applies to each controllerall the above applies to each controller easy to combine:easy to combine:

leftSpeed = translation.currentSpeed – rotation.currentSpeedleftSpeed = translation.currentSpeed – rotation.currentSpeed

rightSpeed = translation.currentSpeed + rotation.currentSpeedrightSpeed = translation.currentSpeed + rotation.currentSpeed

now we know how fast each wheel should now we know how fast each wheel should be turningbe turning

26

Putting it all together - 1Putting it all together - 1

use calculated wheel speeds to work use calculated wheel speeds to work out where the wheel should beout where the wheel should be

use the encoders to find out where it use the encoders to find out where it actually isactually is

now we have an error for each wheelnow we have an error for each wheel use that to get a translation error use that to get a translation error

and a rotation errorand a rotation errortranslation.error = right.error + left.errortranslation.error = right.error + left.error

rotation.error = right.error – left.errorrotation.error = right.error – left.error

27

Putting it together - 2Putting it together - 2

PID operations on each error – PID operations on each error – translation and rotationtranslation and rotationtranslation.outputtranslation.output

rotation.outputrotation.output

resolve outputs once again for each resolve outputs once again for each wheelwheelleft.output = translation.output – rotation.outputleft.output = translation.output – rotation.output

right.output = translation.output + rotation.outputright.output = translation.output + rotation.output

limit and apply to motors.limit and apply to motors. 214 cycles – 13.75us on the dsPIC214 cycles – 13.75us on the dsPIC

28

ReferencesReferences

Otten, D. 1990, ‘Otten, D. 1990, ‘Building MITEE Mouse III’, Building MITEE Mouse III’, Circuit Cellar Ink, Circuit Cellar Ink, no 15 and 16no 15 and 16

Barello, L. 2000, ‘Barello, L. 2000, ‘Small Robot Motion Control: The DilbertsSmall Robot Motion Control: The Dilberts’, ’, available at available at http://www.seattlerobotics.org/encoder/200011/SmallRobothttp://www.seattlerobotics.org/encoder/200011/SmallRobotMotionControl.htmMotionControl.htm

Wescott, T. 2000, Wescott, T. 2000, PID without a PHD,PID without a PHD, Embedded Systems, Embedded Systems, avaialable at: avaialable at: http://www.embedded.com/2000/0010/0010feat3.htmhttp://www.embedded.com/2000/0010/0010feat3.htm

Ng Beng Kiat, Ng Beng Kiat, Sample motor code, available at: Sample motor code, available at: http://www.np.edu.sg/alpha/nbk/H83062F/sampleco.htmlhttp://www.np.edu.sg/alpha/nbk/H83062F/sampleco.html

29

less fish next timeless fish next time