Where am i where should i be
This presentation is the property of its rightful owner.
Sponsored Links
1 / 29

Where Am I? Where should I be? PowerPoint PPT Presentation


  • 80 Views
  • Uploaded on
  • Presentation posted in: General

Where Am I? Where should I be?. Tales from the dark side Peter Harrison Minos 2007. New Mouse. Decimus DC Motors dsPIC processor 4m/s top speed 4m/s/s acceleration. Decimus. Mass:300g Motors:Faulhaber 2224 Gear Ratio:4:1 Encoders:512 cpr Decoder:x2 Wheels:45mm dia.

Download Presentation

Where Am I? Where should I be?

An Image/Link below is provided (as is) to download presentation

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

Presentation Transcript


Where am i where should i be

Where Am I?Where should I be?

Tales from the dark side

Peter Harrison

Minos 2007


New mouse

New Mouse

  • Decimus

  • DC Motors

  • dsPIC processor

  • 4m/s top speed

  • 4m/s/s acceleration


Decimus

Decimus

  • Mass:300g

  • Motors:Faulhaber 2224

  • Gear Ratio:4:1

  • Encoders:512 cpr

  • Decoder:x2

  • Wheels:45mm dia.

  • Resolution:0.035mm


New processor

New Processor

  • 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


New tools

New Tools

  • MPLAB

    • compiler – GNU C

    • programming

    • simulator

  • ICD2

    • programmer

    • debugger


New software

New Software

  • 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


Position is key

Position is Key

  • Literature often about

    • speed control

    • position servo

  • Mouse needs both

  • Speed is change of position with time

  • Control the position

  • Speed control follows


Basic position control

Basic Position Control

  • ‘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


Digital servo

Digital Servo

  • 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


Servo sums 1

Servo Sums - 1

  • 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


Servo sums 2

Servo Sums – 2

  • 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


Servo sums 3

Servo Sums - 3

  • 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


Linear motion

Linear motion

  • 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


Servo sums 4

Servo Sums - 4

  • 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


Acceleration

Acceleration

  • 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


Servo sums 5

Servo Sums - 5

  • 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


Linear motion code

Linear Motion Code

  • 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


Velocity profiles

Velocity Profiles

  • starting and end speed are probably non-zero


Simple profile generation

Simple Profile Generation

  • 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?


Profiler state machine

Profiler State Machine

  • 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


Shall we brake

Shall we Brake?

  • 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()


Braking test

Braking test

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


Profiler code

Profiler Code

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


Now for the other wheel

Now for the other wheel

  • 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


Coordinated controllers

Coordinated Controllers

  • 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


Putting it all together 1

Putting it all together - 1

  • 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


Putting it together 2

Putting it together - 2

  • 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


References

References

  • 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


Where am i where should i be

less fish next time


  • Login