Advanced Programming Workshop

1. Advanced Programming Workshop November 18, 2006 Hauppage High School SPBLI - FIRST Simona Doboli Assistant Professor Computer Science Department Hosftra University Simona.Doboli@hofstra.edu Mark McLeod Advisor Team 358 Hauppauge Northrop Grumman Corp. Mark.McLeod@ngc.com

2. Agenda • Controller Limits • Sensor Overview • Encoders • Proportional-Integral-Derivative (PID) • Rangefinders • CMUCam2 • Gyroscope • Autonomous Variations • Wrap-up

3. Controller Limits/Quirks/Oddities • Do NOT use PWM’s 13 thru 16 • Servo spasms on startup • 2006 RC serious chip faults • Limits: • 128K bytes program space -- Read-Only Memory (rom) • 3,936 bytes data variable space -- Random Access Memory (ram) • 1024 bytes EEPROM -- special access memory • 256 bytes of global/static variables declared within any one MPLAB project file, e.g., user_routines.c • 120 bytes of variables declared within any single routine/function.

4. Sensor Overview • Touch – limit switches, “whiskers” • Position – potentiometers, encoders, gyroscopes, accelerometers • Proximity – Sonar, IR, Photoelectric • Vision – CMUCam2

5. Encoders • Polling vs. Interrupts • Must design both polling & interrupts • Relative vs. Absolute • Types: Optical, Mechanical, Magnetic • Ratings: • Pulses/sec (e.g., Vex encoder 1700 ticks/sec) • Mechanical Side loads, Ball bearing or bushing • Quadrature for direction • Good for fast rotating parts, e.g., wheels

6. Encoders- Initialize • user_routines.c // At top of file declare: extern long Right_Encoder_Count; // In User_Initialization() // initialize external interrupt 1 INTCON3bits.INT2IP = 0; // 0: interrupt 1 is low priority. Always 0 for us. INTCON2bits.INTEDG2 = 1; // 1: trigger when a tick starts INTCON3bits.INT2IE = 1; // 1: enable interrupt 1

7. Encoders - Process • user_routines_fast.c // At the top of the file declare long Right_Encoder_Count=0; // In InterruptHandlerLow () #pragma interruptlow InterruptHandlerLow save=PROD,section("MATH_DATA"),section(".tmpdata") if (INTCON3bits.INT2IF && INTCON3bits.INT2IE) { INTCON3bits.INT2IF = 0; // clear the interrupt flag Right_Encoder_Count++; }

8. Encoders • Using An Interrupt Value // Stop interrupt from counting (very briefly) INTCON3bits.INT2IE = 0; distance_traveled = Right_Encoder_Count; INTCON3bits.INT2IE = 1; // Restart interrupt • Sample Use if (distance_traveled > Where_I_Want_To_Be) pwm01 = pwm02 = 127; else pwm01 = 254; pwm02 = 0;

9. PID Algorithm Output = (Kp * E - Kd * DeltaE + Ki *SumE)/Ks E = Set – Actual //error DeltaE = E – LastE // derivative of the error SumE = E + SumE // integral terms Ks = scaling factor to avoid float values Ex. If Kp = 1.5, use Kp = 15 and Ks = 10.

10. PID Algorithm Output = (Kp * E - Kd * DeltaE + Ki *SumE)/Ks • Proportional component -> Fast reduction of error when error is large. • Derivative component for faster control: Reacts faster to abrupt changes in error. The derivative term starts playing a role close to the set point, when E is small, and it decreases the Output. • Integral component  Corrective action proportional to the amount of accumulated error (faster control).

11. PID Algorithm - Tuning Output = (Kp * E - Kd * DeltaE + Ki *SumE)/Ks • Start with proportional control (P): Kd and Ki = 0 • Increase Kp until the robot starts oscillating around the set point (damped oscillations) • Increase Kd (derivative term) until oscillations disappear. • Then play with Ki (integral term). Usually Ki is 1/Kd. It is needed to eliminate any error left. Then you need Ks.

12. PID Algorithm – The Code • A function where the output value is computed • The function is called every Td seconds. • Td is the sampling rate when new sensor readings are done. • Make sure your function executes in less than Td seconds.

13. int PIDAlgorithm() {   int output;    E = Set - Actual;   output = (Kp*E -Kd*(E - LastE) + Ki * SumE)/Ks;    LastE = E; if (output >= MAXOUTPUT)         output = MAXOUTPUT; else if (output <= -MAXOUTPUT)         output = -MAXOUTPUT;  else         SumE += E; // Convert output     return (output + 128); } PID Algorithm – The Code

14. int main(void) { while(1) { // startTimer // read sensors output = PIDAlgorithm(); // move motors // wait until Timer is equal to Td } } PID Algorithm – The Code

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

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

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

18. Closed-Loop FeedbackIR Algorithm (P) 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

19. CMUCam2Default Camera Code • Just does the camera tracking • Tracking.h Settings • PAN/TILT Gains • Reversing Servos • Camera/tracking menus • Store Camera setting changes • For PID use PAN_SERVO & TILT_SERVO

20. Tracking The Light • Searches for the light • When the light is spotted the robot is turned to face the light • P – power to the motors proportional to the error in angle • I – error builds the longer the robot is off target

21. Closed-Loop Feedback Camera Algorithm (PI) Initialize Camera Serial Communication Order Gimbal Servos to Track Camera Target Camera Interrupt Received CMUCam packet Pan servo Is centered ? No Yes Motor Stop Motor= distance error* KP + cumerror * KI * In this example KP=5 distance error=1 to 25

22. Gyroscope • Analog input • Must be checked (sampled) at precise intervals (need to use a timer) • Must be sampled at x2 or more of the rate the gyroscope produces new readings (google “Nyquist”) • Use gyro faster than the robot • Keep track in “raw” or native units

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

24. Gyroscope- Setup Timer // In user_routines.c within User_Initialization() OpenTimer3(TIMER_INT_ON & T3_16BIT_RW & T3_SOURCE_INT & T3_PS_1_8); WriteTimer3(60535); // All this gives us a 4ms timer to sample 250/sec // In user_routines_fast.c within InterruptHandlerLow() if (PIR2bits.TMR3IF) // TIMER 3 INTERRUPT { PIR2bits.TMR3IF = 0; // Clear Timer interrupt flag WriteTimer3(60535); // Reset Timer to overflow in 4ms Clockms += 4; // milliseconds (not needed) GyroTicks++; // How many samples of the gyro do we need? }

25. Gyroscope- Startup • Let the Gyro Warmup • Takes ~ 1/10 sec to startup • Measure the Neutral Position • It differs slightly from gyro to gyro, run to run, temperature

26. Gyroscope - Maintain Heading // Here is a coarse way to sample the gyro without a lot of fuss #define SAMPLE_RATE 40/250 // We sample 250/sec the gyro gives 40/sec if(GyroTicks > 0) { INTCONbits.GIEL = 0; // Disable low priority interrupts GyroTicks--; // Decrement without interrupts INTCONbits.GIEL = 1; // Re-enable low priority interrupts GyroSample = (int)Get_Analog_Value(rc_ana_in01) - GyroNeutral; GyroSample = GyroSample * SAMPLE_RATE; RawHeading += GyroSample; // Accumulate all the heading changes }

27. Gyroscope - Sample Use If (desired_heading > current_heading) { pwm01 = 254; // Keep the robot turning pwm02 = 254; } else { pwm01 = pwm02 = 127; }

28. Autonomous Variations • Time Cascade • Simple, easy to understand • Difficult/time-consuming to fine-tune • Sensor Feedback • Adapt/React to changes (us & them) • Function-based • Repeatable/Reusable/Testable/Dependable • Script-Based • Quick/Safe changes between matches

29. Autonomous Variations - Sample Project • Sample_Auto_1 -- Basic multi-step autonomous based on the approximate timing of the slow loop • Sample_Auto_2 -- One step up from Auto_1, this one does the same thing based on exact timing • Sample_Auto_3 -- Creates Functions for standard movements to do the same job as Auto_2 • Sample_Auto_4 -- A simple time-only scripting approach implementation of Auto_3 • Sample_Auto_5 -- More complex scripting allows for handling sensor feedback & extra command arguments Scripting could be further complicated by the ability to handle simultaneous operations (drive AND grab), looping within a script, whatever you can imagine and put to use.

30. Team 358 2006 Sensors • Shooter • Velocity control via tachometer feedback • Pot-based Turret “Servo” • Camera Targeting • Ball feed • IR ball detection • Pneumatic Reed-Switch to retract launcher • Drivetrain • Encoders • Controls • Shooter Goggles • Manual/Semi/Full Auto Shooting • Pot-Based Turret Steering & Limit

31. Wrap-Up • 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 • Sensors • Don’t Overly Complicate • Have Fall-back Ability

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

33. References • www.chiefdelphi.com/forums Programming forum • Kevin Watson: kevin.org/frc/ • PID algorithm and motion control http://www.barello.net/Papers/Motion_Control/index.htm

34. Kevin Watson Example Codekevin.org/frc/

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