Autonomous Navigation Workshop

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

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

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

4. AutonomousDead Reckoning • Driving by timer without sensors • 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 • Unable to correct itself if disrupted • Significant changes to robot mechanical system or battery power can disrupt operations and require program to be modified

5. Autonomous Navigation

6. Autonomous Navigation Motors Sensors Feedback

7. Sensors FIRST 2006 • Yaw Rate Gyro (ADXRS150) • Measures angular rate (150o/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? • Rotate robot left or right while the gyro reading is less than Xo. • If robot rotates too fast (the rate of change of the gyro sensor)  reduce motor speed (good in any mode).

9. Sensors FIRST 2006 • 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? • Am I standing straight? • Measure pitch and roll in degrees. • Pitch = asin(Ax/1g); Roll asin(Ay/1g). • Orient an arm. • Collision detection.

11. Sensors FIRST 2006 • 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. a Speed a Distance

12. Sensors FIRST 2006 • 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. • Reverse (from pin 1 to pin 4): 90 us width. • Supply voltage: 28 V.

13. Sensors FIRST 2006 • 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 xo.

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

15. Sensors FIRST 2006 • 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 • Drive to the high goal (PID) • Fire Control

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

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

18. Proximity Sensors – IR Sensors • Emit modulated infrared (IR) energy and measure amount of (IR) returned. • Range: inches to several feet. • 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. • 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. • 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.

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

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

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

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

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

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

27. Rotation • 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 OnRev(LEFT); OnFwd(RIGHT); int last_rot = ROT_LEFT; while(abs(ROT_LEFT - last_rot) < n); OnFwd(RIGHT+LEFT); }

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

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

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

32. PD Algorithm • deltaAngle(n) = Kp * Error(n) + Kd * DeltaError(n) • 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? • Faster control: Reacts faster to abrupt changes in error.

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

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

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

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

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

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

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

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

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

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

43. CMUCam2Labview Interface • Servo Orientation • Camera Focus • Load Configuration • Tracking • Min/Max Servo Positions

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

45. Conclusions • Autonomous mode: • Fixed sequence of actions. • 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 infrared sensors to avoid obstacles or move along the walls.

46. Conclusions • 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: • 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) • Digikey.com • Newarkinone.com • Mouser.com • Bannerengineering.com • Senscomp.com • Alliedelec.com

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