- 88 Views
- Uploaded on
- Presentation posted in: General

Where Am I? Where should I be?

Download Policy: Content on the Website is provided to you AS IS for your information and personal use and may not be sold / licensed / shared on other websites without getting consent from its author.While downloading, if for some reason you are not able to download a presentation, the publisher may have deleted the file from their server.

- - - - - - - - - - - - - - - - - - - - - - - - - - E N D - - - - - - - - - - - - - - - - - - - - - - - - - -

Where Am I?Where should I be?

Tales from the dark side

Peter Harrison

Minos 2007

- Decimus
- DC Motors
- dsPIC processor
- 4m/s top speed
- 4m/s/s acceleration

- Mass:300g
- Motors:Faulhaber 2224
- Gear Ratio:4:1
- Encoders:512 cpr
- Decoder:x2
- Wheels:45mm dia.
- Resolution:0.035mm

- Microchip dsPIC
- 16 bit
- single cycle 16x16 multiply
- fast divide – one clock per bit
- very fast – 32 mips
- extensive peripheral set
- free tools
- good range – common peripherals
- still a PIC so it is a bit odd

- MPLAB
- compiler – GNU C
- programming
- simulator

- ICD2
- programmer
- debugger

- All in C for portability
- clean existing code for GNU
- 80% Maximus code ports across
- new peripheral driver challenge
- motor control is the real challenge

- Literature often about
- speed control
- position servo

- Mouse needs both
- Speed is change of position with time
- Control the position
- Speed control follows

- ‘Simple’ servo application - PID
- motor holds a set point
- Proportional control gives standing error under load
- Derivative control improves response time
- Integral control not needed in a mouse

- Set point position is a number
- Encoder counts distance travelled
- Every tick:
posError = setPoint – currPosition

delta = posError – oldError

oldError = posError

pwmOut = kP × posError + kD × delta

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

- Each count is 0.035mm
- Greatest possible distance in the maze is a 13 cell diagonal
- Approx 2940mm
- About 84000 counts
- 16 bits not enough
- Need 24 or 32

- Use 32 bits, twos complement for position
- Fixed point 24.8
- Good for about ±293m
- Fractions needed later
- kP and kD usefully stored as 8.8 signed fixed point values

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

delta = posError – oldError

oldError = posError

pwmOut = kP × posError + kD × delta

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

- Constant velocity is a list of set points
- At each tick:
setPoint = setPoint + velocity

- velocity is a fixed point, signed value 8.8 bits
- fractional part is needed for acceleration
- Effect is a moving target

- with an 8.8 bit value for velocity we get numbers in the range ±127 or so
- resolution is 0.00013 m/s
- absurd but we will need that later
- can now move by 127 counts per tick
- that is, 127 * 0.035mm = 4.45mm
- maximum velocity is about 4.4m/s

- simply a change in velocity each tick
currentSpeed = currentSpeed + acceleration

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

- accelerating by one count per tick is 0.035mm/ms/ms
- = 35m/s/s
- the fractional part allows smaller increments of acceleration
- minimum is 35/256 = 0.137m/s/s
- 4m/s/s is 0.114 counts per tick
- stored as just 29

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

setPoint = setPoint + currentSpeed

posError = (setPoint – currPosition)/256

delta = posError – oldError

oldError = posError

pwmOut = kP × posError + kD × delta

- very smooth acceleration over a useful speed range

- starting and end speed are probably non-zero

- Only two phases
- acceleration (speed limited)
- deceleration

- no mid-point calculation
- caters for variable distance
- endpoint has position and speed
- only one decision - when do we hit the brakes?

- caller needs to know when done
- three states: accelerating, braking, finished
- simple state machine can help
function updateState()

if (state == ACCELERATING) then

if (brakesNeeded()) then

state = DECELERATING

targetSpeed = endSpeed

end if

end if

if (currentPosition >= targetPosition) then

state = FINISHED

targetSpeed = currentSpeed

end if

end function

- targetSpeed important to profiler action
- start a move with targetSpeed = vMax

- equations of motion:
|v×v – u×u| = |2×a×s|

v = end speed

u = current speed

a = acceleration

s = distance remaining

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

function brakesNeeded()

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

rhs = abs(2 * acceleration * remaining)

if (lhs >= rhs) then

return TRUE

else

return FALSE

end if

end function

if (state == ACCELERATING) then

if (brakesNeeded()) then

state = DECELERATING

targetSpeed = endSpeed

end if

end if

if (currentPosition >= targetPosition) then

state = FINISHED

targetSpeed = currentSpeed

end if

if (currentSpeed < targetSpeed) then

currentSpeed = currentSpeed + acceleration

if (currentSpeed > targetSpeed) then currentSpeed = targetSpeed

else

currentSpeed = currentSpeed – acceleration

if (currentspeed < targetSpeed) then currentSpeed = targetSpeed

end if

- wheelchair mice need two drive motors coordinated
- run two profilers
- not one per wheel
- one for translation
- one for rotation
- resolve components for each wheel
- PID for each wheel controls its position

- using rotation and translation controllers makes coordinated motion easier
- steering
- co-ordinated turns

- all the above applies to each controller
- easy to combine:
leftSpeed = translation.currentSpeed – rotation.currentSpeed

rightSpeed = translation.currentSpeed + rotation.currentSpeed

- now we know how fast each wheel should be turning

- use calculated wheel speeds to work out where the wheel should be
- use the encoders to find out where it actually is
- now we have an error for each wheel
- use that to get a translation error and a rotation error
translation.error = right.error + left.error

rotation.error = right.error – left.error

- PID operations on each error – translation and rotation
translation.output

rotation.output

- resolve outputs once again for each wheel
left.output = translation.output – rotation.output

right.output = translation.output + rotation.output

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

- Otten, D. 1990, ‘Building MITEE Mouse III’, Circuit Cellar Ink, no 15 and 16
- Barello, L. 2000, ‘Small Robot Motion Control: The Dilberts’, available at http://www.seattlerobotics.org/encoder/200011/SmallRobotMotionControl.htm
- Wescott, T. 2000, PID without a PHD, Embedded Systems, avaialable at: http://www.embedded.com/2000/0010/0010feat3.htm
- Ng Beng Kiat, Sample motor code, available at: http://www.np.edu.sg/alpha/nbk/H83062F/sampleco.html

less fish next time