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

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

Controller Limits/Quirks/Oddities Do NOT use PWM’s 13 thru 16 Do NOT use PWM’s 13 thru 16 Servo spasms on startup Servo spasms on startup 2006 RC serious chip faults 2006 RC serious chip faults Limits: 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.

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

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

Encoders - Initialize user_routines.c 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

Encoders - Process user_routines_fast.c 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++;}

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

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.

PID Algorithm Output = (Kp * E - Kd * DeltaE + Ki *SumE)/Ks Proportional component -> Fast reduction of error when error is large. 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. 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). Integral component  Corrective action proportional to the amount of accumulated error (faster control).

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

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

PID Algorithm – The Code int PIDAlgorithm() { int output; int output; E = Set - Actual; E = Set - Actual; output = (Kp*E -Kd*(E - LastE) + Ki * SumE)/Ks; output = (Kp*E -Kd*(E - LastE) + Ki * SumE)/Ks; LastE = E; 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 int main(void) {while(1){ // startTimer // startTimer // read sensors // read sensors output = PIDAlgorithm(); output = PIDAlgorithm(); // move motors // wait until Timer is equal to Td }}

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

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

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

Closed-Loop Feedback IR Algorithm (P) 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

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

Tracking The Light Searches for the light Searches for the light When the light is spotted the robot is turned to face 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 P – power to the motors proportional to the error in angle I – error builds the longer the robot is off target I – error builds the longer the robot is off target

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

Gyroscope Analog input Analog input Must be checked (sampled) at precise intervals (need to use a timer) 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”) Must be sampled at x2 or more of the rate the gyroscope produces new readings (google “Nyquist”) Use gyro faster than the robot Use gyro faster than the robot Keep track in “raw” or native units Keep track in “raw” or native units

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

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

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

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 }

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

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

Autonomous Variations - Sample Project Sample_Auto_1 -- Basic multi-step autonomous based on the approximate timing of the slow loop 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_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_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_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 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.

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

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

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

References Programming forum Programming forum Kevin Watson: kevin.org/frc/ Kevin Watson: kevin.org/frc/ PID algorithm and motion control _Control/index.htm PID algorithm and motion control _Control/index.htm _Control/index.htm _Control/index.htm

Kevin Watson Example Code kevin.org/frc/

Presentation Slides at: orTeam358.org Questions/Help please us. Questions/Help please us.