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_at_hofstra.edu

Mark McLeod Advisor Team 358 Hauppauge Northrop
Grumman Corp. Mark.McLeod_at_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 PWMs 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_Count0
• // In InterruptHandlerLow ()
• pragma interruptlow InterruptHandlerLow
  savePROD,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 gt 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 -gt 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
PID Algorithm The Code

• if (output gt MAXOUTPUT)
•         output MAXOUTPUT
• else if (output lt -MAXOUTPUT)
•         output -MAXOUTPUT
•  else
•         SumE E
• // Convert output    
• return (output 128)

• int PIDAlgorithm()
•   int output 
•   E Set - Actual
•   output (KpE -Kd(E - LastE) Ki SumE)/Ks
•    LastE E

14
PID Algorithm The Code

• int main(void)
• while(1)
• // startTimer
• // read sensors
• output PIDAlgorithm()
• // move motors
• // wait until Timer is equal to Td

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 KP5 distance error1
to 25
17
How The Sensor WorksTimer / 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
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 KP5 distance error1
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
GyroSumGyroRaw/Sample Rate
Motor Stop
P(GyroSum-target) KP
GyroRawGyro - Neutral
ICumError KI
Done
CumError GyroSum-target)
In this example KP6/10 KI3
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 gt 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 gt 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
• Dont 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_at_hofstra.edu
• Mark.McLeod_at_ngc.com