Embed Size (px)
Citation preview
Advanced Programming Advanced Programming Workshop Workshop
Simona DoboliSimona DoboliAssistant ProfessorAssistant ProfessorComputer Science Computer Science
DepartmentDepartmentHosftra UniversityHosftra University
[email protected]@hofstra.edu
November 18, 2006Hauppage High School
SPBLI - FIRST
Mark McLeodMark McLeodAdvisorAdvisor
Team 358 Hauppauge Team 358 Hauppauge Northrop Grumman Northrop Grumman
[email protected]@ngc.com
AgendaAgenda
Controller LimitsController Limits Sensor OverviewSensor Overview EncodersEncoders Proportional-Integral-Derivative (PID)Proportional-Integral-Derivative (PID) RangefindersRangefinders CMUCam2CMUCam2 GyroscopeGyroscope Autonomous VariationsAutonomous Variations Wrap-upWrap-up
Controller Controller Limits/Quirks/OdditiesLimits/Quirks/Oddities Do NOT use PWM’s 13 thru 16Do NOT use PWM’s 13 thru 16 Servo spasms on startupServo spasms on startup 2006 RC serious chip faults2006 RC serious chip faults Limits:Limits:
– 128K bytes program space -- 128K bytes program space -- Read-Only Memory Read-Only Memory (rom)(rom)
– 3,936 bytes data variable space -- 3,936 bytes data variable space -- Random Access Random Access Memory (ram)Memory (ram)
– 1024 bytes EEPROM -- 1024 bytes EEPROM -- special access memoryspecial access memory
– 256 bytes of global/static variables declared 256 bytes of global/static variables declared within any one MPLAB project file, e.g., within any one MPLAB project file, e.g., user_routines.cuser_routines.c
– 120 bytes of variables declared within any single 120 bytes of variables declared within any single routine/function.routine/function.
Sensor OverviewSensor Overview
Touch – limit switches, “whiskers”Touch – limit switches, “whiskers” Position – potentiometers, Position – potentiometers,
encoders, gyroscopes, encoders, gyroscopes, accelerometersaccelerometers
Proximity – Sonar, IR, Proximity – Sonar, IR, PhotoelectricPhotoelectric
Vision – CMUCam2Vision – CMUCam2
EncodersEncoders
Polling vs. InterruptsPolling vs. Interrupts– Must design both polling & interruptsMust design both polling & interrupts
Relative vs. AbsoluteRelative vs. Absolute Types: Optical, Mechanical, MagneticTypes: Optical, Mechanical, Magnetic Ratings:Ratings:
– Pulses/sec (e.g., Vex encoder 1700 ticks/sec)Pulses/sec (e.g., Vex encoder 1700 ticks/sec)– Mechanical Side loads, Ball bearing or bushingMechanical Side loads, Ball bearing or bushing
Quadrature for directionQuadrature for direction Good for fast rotating parts, e.g., wheelsGood for fast rotating parts, e.g., wheels
EncodersEncoders- Initialize- Initialize user_routines.cuser_routines.c
// At top of file declare:// At top of file declare:
extern long Right_Encoder_Count;extern long Right_Encoder_Count;
// In User_Initialization()// In User_Initialization()
// initialize external interrupt 1 // initialize external interrupt 1
INTCON3bits.INT2IP = 0;INTCON3bits.INT2IP = 0; // 0: interrupt 1 is low // 0: interrupt 1 is low priority. Always 0 for us.priority. Always 0 for us.
INTCON2bits.INTEDG2 = 1; // 1: trigger when a INTCON2bits.INTEDG2 = 1; // 1: trigger when a tick startstick starts
INTCON3bits.INT2IE = 1;INTCON3bits.INT2IE = 1; // 1: enable interrupt 1// 1: enable interrupt 1
EncodersEncoders - Process - Process user_routines_fast.cuser_routines_fast.c
// At the top of the file declare// At the top of the file declarelong Right_Encoder_Count=0;long Right_Encoder_Count=0;
// In InterruptHandlerLow ()// In InterruptHandlerLow ()#pragma interruptlow InterruptHandlerLow #pragma interruptlow InterruptHandlerLow
save=PROD,section("MATH_DATA"),section(".tmpdata")save=PROD,section("MATH_DATA"),section(".tmpdata")
if (INTCON3bits.INT2IF && INTCON3bits.INT2IE) if (INTCON3bits.INT2IF && INTCON3bits.INT2IE) { {
INTCON3bits.INT2IF = 0; // clear the interrupt flagINTCON3bits.INT2IF = 0; // clear the interrupt flagRight_Encoder_Count++;Right_Encoder_Count++;
}}
EncodersEncoders
Using An Interrupt ValueUsing An Interrupt Value
// Stop interrupt from counting (very briefly)// Stop interrupt from counting (very briefly)INTCON3bits.INT2IE = 0; INTCON3bits.INT2IE = 0;
distance_traveled = Right_Encoder_Count;distance_traveled = Right_Encoder_Count;INTCON3bits.INT2IE = 1; // Restart interruptINTCON3bits.INT2IE = 1; // Restart interrupt
Sample UseSample Use
if (distance_traveled > Where_I_Want_To_Be)if (distance_traveled > Where_I_Want_To_Be)pwm01 = pwm02 = 127;pwm01 = pwm02 = 127;
elseelsepwm01 = 254; pwm02 = 0;pwm01 = 254; pwm02 = 0;
PID Algorithm PID Algorithm
Output = (Kp * E - Kd * DeltaE + Ki Output = (Kp * E - Kd * DeltaE + Ki *SumE)/Ks*SumE)/Ks
E = Set – Actual //error E = Set – Actual //error
DeltaE = E – LastE // derivative of the errorDeltaE = E – LastE // derivative of the error
SumE = E + SumE // integral termsSumE = E + SumE // integral terms
Ks = scaling factor to avoid float valuesKs = scaling factor to avoid float values
Ex. If Kp = 1.5, use Kp = 15 and Ks = 10.Ex. If Kp = 1.5, use Kp = 15 and Ks = 10.
PID Algorithm PID Algorithm
Output = (Kp * E - Kd * DeltaE + Ki *SumE)/KsOutput = (Kp * E - Kd * DeltaE + Ki *SumE)/Ks
Proportional component -> Fast reduction of Proportional component -> Fast reduction of error when error is large.error when error is large.
Derivative component for faster control: Derivative component for faster control: Reacts faster to abrupt changes in error. The Reacts faster to abrupt changes in error. The derivative term starts playing a role close to derivative term starts playing a role close to the set point, when E is small, and it the set point, when E is small, and it decreases the Output.decreases the Output.
Integral component Integral component Corrective action Corrective action proportional to the amount of accumulated proportional to the amount of accumulated error (faster control).error (faster control).
PID Algorithm - Tuning PID Algorithm - Tuning
Output = (Kp * E - Kd * DeltaE + Ki *SumE)/KsOutput = (Kp * E - Kd * DeltaE + Ki *SumE)/Ks
• Start with proportional control (P): Kd and Ki Start with proportional control (P): Kd and Ki = 0= 0
• Increase Kp until the robot starts oscillating Increase Kp until the robot starts oscillating around the set point (damped oscillations)around the set point (damped oscillations)
• Increase Kd (derivative term) until Increase Kd (derivative term) until oscillations disappear. oscillations disappear.
• Then play with Ki (integral term). Usually Ki Then play with Ki (integral term). Usually Ki is 1/Kd. It is needed to eliminate any error is 1/Kd. It is needed to eliminate any error left. Then you need Ks. left. Then you need Ks.
PID Algorithm – The PID Algorithm – The Code Code
• A function where the output value is A function where the output value is computedcomputed
• The function is called every Td The function is called every Td seconds. seconds.
• Td is the sampling rate when new Td is the sampling rate when new sensor readings are done. sensor readings are done.
• Make sure your function executes in Make sure your function executes in less than Td seconds.less than Td seconds.
PID Algorithm – The PID Algorithm – The CodeCode
int PIDAlgorithm()int PIDAlgorithm(){{ int output; int output; E = Set - Actual;E = Set - Actual;
output = (Kp*E -Kd*(E - LastE) output = (Kp*E -Kd*(E - LastE) + Ki * SumE)/Ks;+ Ki * SumE)/Ks;
LastE = E;LastE = E;
if (output >= MAXOUTPUT)if (output >= MAXOUTPUT) output = MAXOUTPUT;output = MAXOUTPUT;else if (output <= -else if (output <= -
MAXOUTPUT)MAXOUTPUT) output = -MAXOUTPUT;output = -MAXOUTPUT; elseelse SumE += E;SumE += E;
// Convert output // Convert output return (output + 128);return (output + 128);}}
PID Algorithm – The PID Algorithm – The CodeCode
int main(void)int main(void){{
while(1)while(1){{
// startTimer// startTimer // read sensors// read sensors output = PIDAlgorithm();output = PIDAlgorithm();// move motors// move motors// wait until Timer is equal to Td// wait until Timer is equal to Td
}}} }
Keep Your Distance Keep Your Distance
Maintains a constant distance from an Maintains a constant distance from an obstacle via ultrasonic & IR obstacle via ultrasonic & IR rangefindersrangefinders
User sets distance via potentiometerUser sets distance via potentiometer P – power to the motors proportional P – power to the motors proportional
to the error in distanceto the error in distance Obstacle must be perpendicular to Obstacle must be perpendicular to
sensor to reflect echosensor to reflect echo
Polaroid 6500
Closed-Loop Feedback Closed-Loop Feedback Ultrasonic Algorithm Ultrasonic Algorithm (P)(P)
Initialize
Send Trigger Pulse
Listen & time echo
Filter echo results Timer / interrupt process
EchoInterruptCompare requested distance to echo
Right Distance?
Yes No
Motor Stop Motor= distance error* KP
* In this example KP=5 distance error=1 to 25
How The Sensor WorksHow The Sensor WorksTimer / Interrupt Timer / Interrupt ProcessProcess
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 RangefinderSRF05 (1-150”) $25
Closed-Loop FeedbackClosed-Loop FeedbackIR Algorithm (P)IR Algorithm (P)
Initialize
Get IR Sensed Distance
Compare to requested distance
Right Distance?
Yes No
Motor Stop Motor= distance error* KP
V = 1/(R + .42)to linearize input
Sharp GP2Y0A02YK (6-60”) $16.50 GP2D120 (.5-30”) $12.50
CMUCam2CMUCam2Default Camera CodeDefault Camera Code Just does the camera trackingJust does the camera tracking Tracking.h SettingsTracking.h Settings
– PAN/TILT GainsPAN/TILT Gains– Reversing ServosReversing Servos
Camera/tracking menus Camera/tracking menus – Store Camera setting changesStore Camera setting changes
For PID use PAN_SERVO & For PID use PAN_SERVO & TILT_SERVOTILT_SERVO
Tracking The Light Tracking The Light
Searches for the lightSearches for the light When the light is spotted the robot is When the light is spotted the robot is
turned to face the lightturned to face the light P – power to the motors proportional P – power to the motors proportional
to the error in angleto the error in angle I – error builds the longer the robot is I – error builds the longer the robot is
off targetoff target
Closed-Loop Feedback Closed-Loop Feedback Camera Algorithm (PI)Camera Algorithm (PI)
Initialize
Received CMUCampacket
Order Gimbal Servos to Track Camera Target
CameraInterrupt
Pan servoIs centered
?Yes No
Motor Stop Motor= distance error* KP+ cumerror * KI * In this example
KP=5 distance error=1 to 25
Camera SerialCommunication
Gyroscope Gyroscope
Analog inputAnalog input Must be checked (sampled) at precise Must be checked (sampled) at precise
intervals (need to use a timer)intervals (need to use a timer) Must be sampled at x2 or more of the Must be sampled at x2 or more of the
rate the gyroscope produces new rate the gyroscope produces new readings (google “Nyquist”)readings (google “Nyquist”)
Use gyro faster than the robotUse gyro faster than the robot Keep track in “raw” or native unitsKeep track in “raw” or native units
Closed-Loop Feedback Closed-Loop Feedback Gyro-Based Turn (PI)Gyro-Based Turn (PI)
Initialize
Get Gyro Value
GyroRaw+=Gyro - Neutral
Timer
Are we there yet?
Yes No
Motor Stop
Motor= P + I
P=(GyroSum-target)* KP
I=CumError* KIDone
CumError +=GyroSum-target)
GyroSum=GyroRaw/Sample Rate
* In this example KP=6/10 KI=3
GyroscopeGyroscope- Setup Timer- 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? }
GyroscopeGyroscope- Startup- 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
GyroscopeGyroscope - Maintain Heading - 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}
GyroscopeGyroscope - Sample Use - Sample UseIf (desired_heading > current_heading){
pwm01 = 254; // Keep the robot turning
pwm02 = 254;}else{
pwm01 = pwm02 = 127;}
Autonomous Autonomous VariationsVariations Time CascadeTime Cascade
– Simple, easy to understandSimple, easy to understand– Difficult/time-consuming to fine-tuneDifficult/time-consuming to fine-tune
Sensor FeedbackSensor Feedback– Adapt/React to changes (us & them)Adapt/React to changes (us & them)
Function-basedFunction-based– Repeatable/Reusable/Testable/Repeatable/Reusable/Testable/
DependableDependable Script-BasedScript-Based
– Quick/Safe changes between matchesQuick/Safe changes between matches
Autonomous Autonomous VariationsVariations - Sample Project - Sample Project Sample_Auto_1Sample_Auto_1 -- Basic multi-step autonomous based on the -- Basic multi-step autonomous based on the
approximate timing of the slow loopapproximate timing of the slow loop
Sample_Auto_2Sample_Auto_2 -- One step up from Auto_1, this one does the -- One step up from Auto_1, this one does the same thing based on exact timingsame thing based on exact timing
Sample_Auto_3Sample_Auto_3 -- Creates Functions for standard movements -- Creates Functions for standard movements to do the same job as Auto_2to do the same job as Auto_2
Sample_Auto_4Sample_Auto_4 -- A simple time-only scripting approach -- A simple time-only scripting approach implementation of Auto_3implementation of Auto_3
Sample_Auto_5Sample_Auto_5 -- More complex scripting allows for handling -- More complex scripting allows for handling sensor feedback & extra command argumentssensor feedback & extra command arguments
Scripting could be further complicated by the ability to handle Scripting could be further complicated by the ability to handle simultaneous operations (drive AND grab), looping within a simultaneous operations (drive AND grab), looping within a script, whatever you can imagine and put to use.script, whatever you can imagine and put to use.
Team 358 Team 358 2006 Sensors2006 Sensors ShooterShooter
– Velocity control via tachometer feedbackVelocity control via tachometer feedback– Pot-based Turret “Servo”Pot-based Turret “Servo”– Camera TargetingCamera Targeting
Ball feedBall feed– IR ball detectionIR ball detection– Pneumatic Reed-Switch to retract launcherPneumatic Reed-Switch to retract launcher
DrivetrainDrivetrain– EncodersEncoders
ControlsControls– Shooter GogglesShooter Goggles– Manual/Semi/Full Auto ShootingManual/Semi/Full Auto Shooting– Pot-Based Turret Steering & LimitPot-Based Turret Steering & Limit
Wrap-UpWrap-Up
P, PD, PID controllers.P, PD, PID controllers.– Try P first, if happy stick with it.Try P first, if happy stick with it.– For faster reaction try PD. For faster reaction try PD. – If error is too great, try PI.If error is too great, try PI.– For both fast reaction and error, try For both fast reaction and error, try
PIDPID SensorsSensors
– Don’t Overly ComplicateDon’t Overly Complicate– Have Fall-back AbilityHave Fall-back Ability
Sensor SuppliersSensor Suppliers
Acroname.com (easiest to window Acroname.com (easiest to window shop)shop)
Digikey.comDigikey.com Newarkinone.comNewarkinone.com Mouser.comMouser.com Bannerengineering.comBannerengineering.com Senscomp.comSenscomp.com Alliedelec.comAlliedelec.com
ReferencesReferences
www.chiefdelphi.com/forums www.chiefdelphi.com/forums Programming forumProgramming forum
Kevin Watson: kevin.org/frc/Kevin Watson: kevin.org/frc/ PID algorithm and motion control PID algorithm and motion control
http://http://www.barello.net/Papers/Motion_Cwww.barello.net/Papers/Motion_Control/index.htmontrol/index.htm
Kevin Watson Example Kevin Watson Example CodeCodekevin.org/frc/kevin.org/frc/
Presentation Slides at:Presentation Slides at:
www.cs.hofstra.edu/~sdoboliwww.cs.hofstra.edu/~sdoboli
oror
Team358.orgTeam358.org
Questions/Help please email us. Questions/Help please email us.
[email protected]@hofstra.edu
[email protected]@ngc.com
