Where Am I? Where Am I? Where should I be? Where should I be? Tales from the dark side Tales from the dark side Peter Harrison Peter Harrison Minos 2007 Minos 2007
Jan 14, 2016
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