1 / 29

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

tino
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. Content is provided to you AS IS for your information and personal use only. Download presentation by click this link. While downloading, if for some reason you are not able to download a presentation, the publisher may have deleted the file from their server. During download, if you can't get a presentation, the file might be deleted by the publisher.

E N D

Presentation Transcript

  1. Where Am I?Where should I be? Tales from the dark side Peter Harrison Minos 2007

  2. New Mouse • Decimus • DC Motors • dsPIC processor • 4m/s top speed • 4m/s/s acceleration

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

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

  5. New Tools • MPLAB • compiler – GNU C • programming • simulator • ICD2 • programmer • debugger

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

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

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

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

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

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

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

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

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

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

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

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

  18. Velocity Profiles • starting and end speed are probably non-zero

  19. 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?

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

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

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

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

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

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

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

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

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

  29. less fish next time

More Related