1. Autonomous Navigation Workshop Simona Doboli Assistant Professor Computer Science Department Hosftra University Email: Simona.Doboli@hofstra.edu January 14 th, 2006 Hauppage High School SPBLI - FIRST Mark McLeod Programming Coach Hauppauge Team 358 Northrop Grumman Corp. Mark.McLeod@ngc.com

2. Agenda General Points General Points Dead Reckoning Dead Reckoning Sensor-Based Navigation Sensor-Based Navigation Demonstrations Demonstrations –Lego – Rotation sensors / Light Sensor –Vex – Ultrasonic sensor –FRC – Encoders / Gyroscope –CMUCam2 Conclusions Conclusions Playtime Playtime

3. General Points Balance your drivetrain mechanically or through software, e.g., Balance your drivetrain mechanically or through software, e.g., #define BALANCE 16 #define BALANCE 16 pwm02 -= BALANCE * (pwm02 – 127) / 127; pwm02 -= BALANCE * (pwm02 – 127) / 127; Technique Technique –State Machine –Function Driven –Script Driven Multiple fallback implementations for when sensors or appendages break Multiple fallback implementations for when sensors or appendages break Pay close attention to LIMITS when designing for sensors Pay close attention to LIMITS when designing for sensors

4. Autonomous Dead Reckoning Driving by timer without sensors Driving by timer without sensors Simplest form of autonomous mode and a backup for sensor failure Simplest form of autonomous mode and a backup for sensor failure Good for short duration movements such as driving to an approximate spot on the field Good for short duration movements such as driving to an approximate spot on the field Unable to correct itself if disrupted Unable to correct itself if disrupted Significant changes to robot mechanical system or battery power can disrupt operations and require program to be modified Significant changes to robot mechanical system or battery power can disrupt operations and require program to be modified

5. Autonomous Navigation

6. Motors Sensors Feedback

7. Sensors FIRST 2006 Yaw Rate Gyro (ADXRS150) Yaw Rate Gyro (ADXRS150) –Measures angular rate (150 o /sec) along the Z axis. –Supply voltage 5V. –Output – analog. –Applications: Stability control, guidance.

8. Sensors FIRST 2006 What can you do with the gyro sensor in autonomous mode? What can you do with the gyro sensor in autonomous mode? –Rotate robot left or right while the gyro reading is less than X o. –If robot rotates too fast (the rate of change of the gyro sensor)  reduce motor speed (good in any mode). –Drive straight.

9. Sensors FIRST 2006 Dual Axis Accelerometer (ADXL 311) Dual Axis Accelerometer (ADXL 311) –Measures dynamic and static acceleration on both X and Y axis. –Applications: tilt or motion sensor. –Supply voltage 5 V. –Output – analog. Bandwidth 3KHZ. –Sampling frequency > 6 KHZ.

10. Sensors FIRST 2006 What can you do with the accelerometer? What can you do with the accelerometer? –Am I standing straight? Measure pitch and roll in degrees. Measure pitch and roll in degrees. Pitch = asin(Ax/1g); Roll asin(Ay/1g). Pitch = asin(Ax/1g); Roll asin(Ay/1g). –Orient an arm. –Collision detection.

11. Sensors FIRST 2006 What can you do with the accelerometer? What can you do with the accelerometer? –Move robot d feet with a desired speed. –Need to accelerate the first part and decelerate the last part. Distance Speed a a

12. Sensors FIRST 2006 Gear Tooth Sensors (2) Gear Tooth Sensors (2) –ATS651 speed and direction sensor. –Generates a pulse when a gear tooth is detected. –Speed of the gear: pulse rate. –Direction of the gear: pulse width. Forward (from pin 4 to pin 1): 45 us width. Forward (from pin 4 to pin 1): 45 us width. Reverse (from pin 1 to pin 4): 90 us width. Reverse (from pin 1 to pin 4): 90 us width. –Supply voltage: 5 V.

13. Sensors FIRST 2006 What can you do with the gear tooth sensors? What can you do with the gear tooth sensors? –Control the speed of the wheel. –Adjust for relative speed difference between wheels. –AUTONOMOUS MODE: Move d feet distance at an angle of x o. Move d feet distance at an angle of x o.

14. Sensors FIRST 2006 CMUCam2 Camera CMUCam2 Camera –RC default camera code-Kevin Watson –Labview / camera driven servos

15. Sensors FIRST 2006 What can you do with the camera? What can you do with the camera? –Track colors, e.g., illuminated target –Locate the high scoring goal –Orient the robot or a turret to the high goal Heading & angle or distance Heading & angle or distance –Drive to the high goal (PID) –Fire Control

16. Other Sensors – Proximity sensors Is something close to me that I will hit soon? Is something close to me that I will hit soon?

17. Proximity Sensors - Sonars Emit a sound and measure the time of flight  distance. Emit a sound and measure the time of flight  distance. Ranges: 1 – 30 feet, with a field of view of 30 o. Ranges: 1 – 30 feet, with a field of view of 30 o.

18. Proximity Sensors – IR Sensors Emit modulated infrared (IR) energy and measure amount of (IR) returned. Emit modulated infrared (IR) energy and measure amount of (IR) returned. Range: inches to several feet. Range: inches to several feet. Led IR sensors have ranges of 3-5 inches. Led IR sensors have ranges of 3-5 inches.

19. LEGO sensors – Touch Sensor If pressure is applied to it, an electrical signal is generated. If pressure is applied to it, an electrical signal is generated. What can you do with it? What can you do with it? –I already bumped into something. I better get back, or move around it.

20. Light Sensor Red LED emits light and a phototransistor measures the incoming light. Red LED emits light and a phototransistor measures the incoming light. What can you do with a light sensor? What can you do with a light sensor? –Recognize objects of certain colors. –Follow a line. Problems: Ambient light and battery level affect sensor readings. Problems: Ambient light and battery level affect sensor readings.

21. Rotation Sensor Measures the rotation angle relative to a reference position. Measures the rotation angle relative to a reference position. LEGO rotation sensor: Measures increments of 22.5 o. LEGO rotation sensor: Measures increments of 22.5 o. What can you do with rotation sensors? What can you do with rotation sensors? – Same as the gear tooth sensor.

22. Case Study: A LEGO robot Rotation Sensors Light Sensor DC Motors

23. Rotation Sensors Tire Gear (24) Motor Gear (16) Transmission Gear (40) Sensor Gear (8) 1 rot. Tire Gear  3 rot. Sensor Gear 1 rot. Tire Gear  3 rot. Sensor Gear X degrees Tire Gear  3*X degrees Sensor X degrees Tire Gear  3*X degrees Sensor

24. Rotation Sensors Sensor reading – multiple of 22.5 o. Sensor reading – multiple of 22.5 o. Calibrate distance: Calibrate distance: –Initialize rotation sensors to 0. –Move robot. –Until tire wheel moves one full rotation. –Stop motors. –Measure distance. Simpler  measure wheel diameter. Simpler  measure wheel diameter.

25. Move Algorithm // moves fwd (dist >0) or rev (dist 0) or rev (dist < 0) dist cm void move(int dist) { int degreesFwd = 3 * abs(dist)*360/DIAMETER_FULL_SPEED; int degreesFwd = 3 * abs(dist)*360/DIAMETER_FULL_SPEED; int n = degreesFwd *10/225; // desired increment of rot. sensor int n = degreesFwd *10/225; // desired increment of rot. sensor if (dist > 0) if (dist > 0) OnFwd(LEFT+RIGHT); OnFwd(LEFT+RIGHT); else else OnRev(LEFT+RIGHT); OnRev(LEFT+RIGHT); int last_rot = ROT_LEFT; int last_rot = ROT_LEFT; while (abs(ROT_LEFT - last_rot) < n); while (abs(ROT_LEFT - last_rot) < n); Off(RIGHT+LEFT); Off(RIGHT+LEFT);}

26. Issues with Move Problems with distance calibration: Problems with distance calibration: –Should account for motors’ inertia at different speeds (stopping distance). N teeth tire wheel N – p = full power, p decrease power slowly distance Or, measure distance of one wheel rotation at different speeds. Or, measure distance of one wheel rotation at different speeds.

27. Rotation Calibrate rotation angle. Calibrate rotation angle. –Rotate in both directions a set of angles on the rotation sensor, measure robot angles. –Fit a line then calculate tangent. –You should account for the speed of the motor too (stopping angle).

28. Rotation Algorithm // degrees may be only positive and greater than or equal 4 // postcondition - both motors are on forward void rotateLeft(int degrees) { int n = (degrees *4)/3; // increments on rotation sensor int n = (degrees *4)/3; // increments on rotation sensor OnRev(LEFT); OnRev(LEFT); OnFwd(RIGHT); OnFwd(RIGHT); int last_rot = ROT_LEFT; int last_rot = ROT_LEFT; while(abs(ROT_LEFT - last_rot) < n); while(abs(ROT_LEFT - last_rot) < n); OnFwd(RIGHT+LEFT); OnFwd(RIGHT+LEFT);}

29. Follow a black line Follow a black line (stay on the edge). Follow a black line (stay on the edge). Error(n) = Edge – LIGHT(n); Error(n) = Edge – LIGHT(n); –Positive – robot on black; Negative – on white. Action: Action: –Rotate left deltaAngle degrees if Error > 0 –Rotate right deltaAngle degrees if Error < 0 P Controller: P Controller: deltaAngle(n) = Kp * Error(n) deltaAngle(n) = Kp * Error(n)

30. P Algorithm error = edge – LIGHT; // read light sensor while (true) { deltaAngle = Kp*error; deltaAngle = Kp*error; if (abs(deltaAngle) > maxDeltaAngle) if (abs(deltaAngle) > maxDeltaAngle) deltaAngle = sign(deltaAngle) * maxDeltaAngle; if (deltaAngle >= 4) // insensitive area if (deltaAngle >= 4) // insensitive area rotateLeft(deltaAngle); rotateLeft(deltaAngle); else if (deltaAngle <= -4) else if (deltaAngle <= -4) rotateRight(deltaAngle); rotateRight(deltaAngle); error = edge - LIGHT; // read light sensor error = edge - LIGHT; // read light sensor }

31. PD Controller Error(n) = Edge – LIGHT(n); Error(n) = Edge – LIGHT(n); LastError = Error(n-1) LastError = Error(n-1) DeltaError(n) = Error(n) – Error(n-1); DeltaError(n) = Error(n) – Error(n-1); PD Controller PD Controller deltaAngle(n) = Kp * Error(n) + deltaAngle(n) = Kp * Error(n) + Kd * DeltaError(n) Kd * DeltaError(n)

32. PD Algorithm deltaAngle(n) = Kp * Error(n) + deltaAngle(n) = Kp * Error(n) + Kd * DeltaError(n) Kd * DeltaError(n) Effect of DeltaError: Effect of DeltaError: –DeltaAngle is proportional with the rate of change in error. –Derivative component amplifies noise. (limit its value). –Try negative values of Kd. Better than P controller? Better than P controller? –Faster control: Reacts faster to abrupt changes in error.

33. PD Algorithm while(true){ deltaAngle = Kp*error; deltaAngle = Kp*error; der = Kd*(error-lastError); der = Kd*(error-lastError); if (abs(der) > maxDerivative) if (abs(der) > maxDerivative) der= sign(der)*maxDerivative; der= sign(der)*maxDerivative; deltaAngle -= der; deltaAngle -= der; if (abs(deltaAngle) > maxDeltaAngle) if (abs(deltaAngle) > maxDeltaAngle) deltaAngle = sign(deltaAngle)* deltaAngle = sign(deltaAngle)* maxDeltaAngle; maxDeltaAngle; if (deltaAngle >= 4) rotateLeft(deltaAngle); rotateLeft(deltaAngle); else if (deltaAngle <= -4) rotateRight(deltaAngle); rotateRight(deltaAngle); lastError = error; lastError = error; error = edge - LIGHT; error = edge - LIGHT; }

34. PID Controller deltaAngle = Kp *Error(n) + Ki* Error(i) + Kd*(Error(n) – Error(n-1)) Kd*(Error(n) – Error(n-1)) Integral component  Corrective action proportional to the amount of accumulated error (faster control). Integral component  Corrective action proportional to the amount of accumulated error (faster control). Limit each P, I, D term and the cumulative error. Limit each P, I, D term and the cumulative error.

35. PID Algorithm sumError = 0; while(true){ deltaAngle = Kp*error; deltaAngle = Kp*error; der = Kd*(error-lastError); der = Kd*(error-lastError); if (abs(der) > maxDerivative) if (abs(der) > maxDerivative) der = sign(der)* maxDerivative; der = sign(der)* maxDerivative; deltaAngle -= derivative; deltaAngle -= derivative; sumError += error; sumError += error; if (abs(sumError) > maxSumError) if (abs(sumError) > maxSumError) sumError = sign(sumError)* sumError = sign(sumError)* maxSumError; maxSumError; deltaAngle += Ki*sumError; deltaAngle += Ki*sumError; if (abs(deltaAngle) > maxDeltaAngle) deltaAngle = sign(deltaAngle)* maxDeltaAngle; if (deltaAngle >= 4) rotateLeft(deltaAngle); rotateLeft(deltaAngle); else if (deltaAngle <= -4) rotateRight(deltaAngle); rotateRight(deltaAngle); lastError = error; lastError = error; error = edge - LIGHT; error = edge - LIGHT; }

36. Case Study: Vex On-Board Controls Ultrasonic Rangefinder IR Rangefinders Line Followers

37. Keep Your Distance Maintains a constant distance from an obstacle via ultrasonic & IR rangefinders Maintains a constant distance from an obstacle via ultrasonic & IR rangefinders User sets distance via potentiometer User sets distance via potentiometer P – power to the motors proportional to the error in distance P – power to the motors proportional to the error in distance Obstacle must be perpendicular to sensor to reflect echo Obstacle must be perpendicular to sensor to reflect echo Polaroid 6500

38. Closed-Loop Feedback Ultrasonic Algorithm (P) Initialize Send Trigger Pulse Listen & time echo Filter echo results Timer / interrupt process Echo Interrupt Compare requested distance to echo Right Distance ? YesNo Motor Stop Motor= distance error* KP * In this example KP=5 distance error=1 to 25

39. How The Sensor Works Timer / Interrupt Process 1.Program requests a sonar pulse 2.Pulse is set out 3.Program is told pulse is sent 4.Program is told when echo returns 5.Calculate the time it took 6.Wait before requesting another For Devantech SRF05 Rangefinder SRF05 (1-150”) $25

40. Closed-Loop Feedback IR Algorithm (PI) Initialize Get IR Sensed Distance Compare to requested distance Right Distance ? YesNo Motor Stop Motor= distance error* KP V = 1/(R +.42) to linearize input Sharp GP2Y0A02YK (6-60”) $16.50 GP2D120 (.5-30”) $12.50

41. Case Study: FRC Encoders Gyroscope Arm Telescoping Potentiometer Arm Angle Potentiometer

42. Closed-Loop Feedback Gyro-Based Turn (PI) Initialize Get Gyro Value GyroRaw+=Gyro - Neutral Timer Are we there yet ? YesNo Motor Stop Motor= P + I P=(GyroSum-target)* KP I=CumError* KI Done CumError +=GyroSum-target) GyroSum=GyroRaw/Sample Rate * In this example KP=6/10 KI=3

43. CMUCam2 Labview Interface Servo Orientation Camera Focus Load Configuration Tracking Min/Max Servo Positions

44. CMUCam2 Kevin Watson Camera Code Just does the camera tracking Just does the camera tracking Load initial calibration data Load initial calibration data Tracking.h Settings Tracking.h Settings –PAN/TILT Gains –Reversing Servos Camera/tracking menus through Hyperterminal Camera/tracking menus through Hyperterminal Camera settings stored permanently on the RC Camera settings stored permanently on the RC For PID use PAN_SERVO & TILT_SERVO For PID use PAN_SERVO & TILT_SERVO Confidence use pan_error and tilt_error Confidence use pan_error and tilt_error

45. Conclusions Autonomous mode: Autonomous mode: –Fixed sequence of actions. Advantage: simple and fast; but calibrate it to actual conditions. Advantage: simple and fast; but calibrate it to actual conditions. –More flexible solutions, but maybe too slow for 10 sec. Use camera to search for objects of certain color. Use camera to search for objects of certain color. Use infrared sensors to avoid obstacles or move along the walls. Use infrared sensors to avoid obstacles or move along the walls.

46. Conclusions P, PD, PID controllers. P, PD, PID controllers. –Try P first, if happy stick with it. –For faster reaction try PD. –If error is too great, try PI. –For both fast reaction and error, try PID

47. Conclusions Situations where you can use P,PD,PID: Situations where you can use P,PD,PID: – Drive straight (correct small errors in the relative speed of the two motors using the gyro sensor too). –Turn to a precise heading. –Drive an exact distance. –Follow a wall. –Home on a beacon or a retroreflective target –Follow an object of a certain color (using the camera).

48. Sensor Suppliers Acroname.com (easiest to window shop) Acroname.com (easiest to window shop) Digikey.com Digikey.com Newarkinone.com Newarkinone.com Mouser.com Mouser.com Bannerengineering.com Bannerengineering.com Senscomp.com Senscomp.com Alliedelec.com Alliedelec.com

49. Presentation slides at: Presentation slides at: www.cs.hofstra.edu/~sdoboli www.cs.hofstra.edu/~sdoboliwww.cs.hofstra.edu/~sdoboli orTeam358.org Questions/Help please email us. Questions/Help please email us. Simona.Doboli@hofstra.edu Mark.McLeod@ngc.com