Advanced Programming in the VEX Environment
Download
Report
Transcript Advanced Programming in the VEX Environment
Advanced
Programming
in the VEX
Environment
1
Peter Johnson
Northrop Grumman Space Technology
Programming Mentor, Beach Cities Robotics
(FRC/FTC/VRC Team 294)
1 Mar 2008
Agenda
2
Sensors Recap
Advanced Operator Control
Advanced Autonomous
Parting Thoughts
Sensors Recap
3
Sensors – A Programmer’s Best Friend
Limit Switch
Connects to 1 digital input
0 when closed, 1 when open
Use to limit range of mechanical motion in both
autonomous and operator control modes
Fragile - always have a mechanical hard stop too!
Bumper Switch
More robust than limit switch, but otherwise
operates identically
Can itself act as a mechanical hard stop
4
Sensors – A Programmer’s Best Friend
Optical Shaft Encoder
Connects to 1 or 2 interrupt ports
Interrupt count (90 ticks/revolution)
With 2 interrupt ports, can also tell direction
Most useful on drivetrain or anything that rotates
(like a lifting arm)
Useful for distance, rotation, and driving straight in
autonomous
Ultrasonic Range Finder
Connects to 1 interrupt port and 1 digital port
Senses distance to a object in inches (2 to 100)
Useful for determining distance in a particular
direction to walls, robots, or other objects
5
Programming Sensors
Limit Switches and Bumpers
input = GetDigitalInput(X)
Optical Encoder
StartEncoder(X)
PresetEncoder(X, 0)
ticks = GetEncoder(X)
Optical Quadrature Encoder
Same as encoder, except two inputs and functions
named with “Quad”
Ultrasonic Sensor
StartUltrasonic(interrupt, output)
distance = GetUltrasonic(interrupt, output)
6
Advanced Operator Control
7
Advanced Operator Control
Limiting range of motion with sensors
Toggle buttons
Push once to open, push again to close
Sequenced commands
8
Advanced Operator Control
Limiting Range of Motion with Sensors
9
Limiting Range of Motion With Sensors
Hard stops prevent physical motion past endpoint
But motors will keep trying to drive, unless you program
them not to!
Your driver will keep that button pushed
Why is this bad?
Possible physical damage:
Burn out motors
Strip gears / clutches
Drain battery (motors can use 1 A current at stall)
Don’t know when to stop driving motors in autonomous
The solution: Limit Switches and/or Bumpers!
Locate such that switch is activated when physical motion
reaches endpoint
Override motor control when switch active
10
Limiting Range of Motion – Take 1
while(1==1)
{
OIToPWM(1,1,1,0); // drive Motor 1 from Rx 1 Chan 1
min_limit = GetDigitalInput(1);
max_limit = GetDigitalInput(2);
if (min_limit == 0 || max_limit == 0)
{
SetPWM(1, 127);
}
}
What are the problems with this approach?
How would you fix these problems?
11
Limiting Range of Motion – Take 2
while(1==1)
{
rc_input = GetRxInput(1, 1);
min_limit = GetDigitalInput(1);
max_limit = GetDigitalInput(2);
if (min_limit == 0 && rc_input < 127)
{
SetPWM(1, 127);
}
else if (max_limit == 0 && rc_input > 127)
{
SetPWM(1, 127);
}
else
{
SetPWM(1, rc_input);
}
}
12
Limiting Range of Motion – Final Version
while(1==1)
{
rc_input = GetRxInput(1, 1);
min_limit = GetDigitalInput(1);
max_limit = GetDigitalInput(2);
if ((min_limit == 0 && rc_input < 64) ||
(max_limit == 0 && rc_input > 196))
{
SetPWM(1, 127);
}
else
{
SetPWM(1, rc_input);
}
}
13
Limiting Range of Motion – Looking Ahead
What would you need to change if the limits are
backwards? (you did test it, right?)
What would you need to change if the driver wants
the motor direction (controls) reversed?
How would you handle multiple limits? Example: a
grabber that had three limit switches:
Fully closed
Fully open
Object grabbed
How would you handle multiple motors? Example:
two motors driving an arm, mounted on opposite
sides
All of these scenarios can and will come up during
robot design and test!
14
Advanced Operator Control
“Toggle Button” Operation
15
“Toggle Button” Operation
Your driver comes to you and says:
“It’d be really nice if you could make this button
work like a toggle: I press the button once to open
the grabber, and again to close it. I don’t like
having to remember to press the top button to
close and the bottom button to open (or was that
the other way around?). Oh, and the next match is
in 15 minutes.”
Your response… “No, I don’t have time to test it!”
But the lunch break follows the next match, so you
forgo food and start working…
Fortunately the grabber is driven by a servo, so this
shouldn’t be too hard, right?
16
Toggle Button Operation – Take 1
int grabber_closed = 1;
while(1==1)
{
rc_input = GetRxInput(1, 1);
if (rc_input < 64 || rc_input > 196) // either button
{
if (grabber_closed)
{
SetPWM(1, 255); // open it
}
else
{
SetPWM(1, 50); // close it
}
grabber_closed = !grabber_closed; // update state
}
}
Great start.. but then it gets tested…
17
Toggle Button Operation – Uh oh
The grabber “stutters” on button presses… it will
partly open and then close again, or vice-versa
It seems to be random whether it ends up open or
closed
What’s the problem?
The code is in a forever loop, and as long as the
button is held down, the first if condition is true
The code is oscillating between open and closed
states
How to fix it?
Add a change in progress variable
18
Toggle Button Operation – Take 1
int grabber_closed = 1;
int grabber_changing = 0;
while(1==1)
{
rc_input = GetRxInput(1, 1);
if (!grabber_changing && (rc_input < 64 || rc_input > 196)) // either button
{
if (grabber_closed)
{
SetPWM(1, 255); // open it
}
else
{
SetPWM(1, 50); // close it
}
grabber_closed = !grabber_closed; // update state
grabber_changing = 1;
}
else if (rc_input > 64 && rc_input < 196) // neither button down
{
grabber_changing = 0;
}
}
Back to the testing floor…
19
Toggle Button Operation – Uh oh #2
It’s better, but… it still occasionally “stutters” on
button presses!
What’s going on? Let’s take a closer look at what
happens mechanically when a button on the
controller is pressed
This is called contact bounce
What does the button value look like when you
rapidly read it via GetRxInput()?
20
How to fix it?
Most reliable way is a timeout
Add a “changing” variable
Conditionalize the action on that variable
When a change occurs, start a timer
Don’t accept another input change for X ms
You can make it even more robust by resetting the
timer on each pulse (so it’s X ms from the last
transition)
21
Example Debouncer Code
rc_input = GetRxInput(1, 1);
if (ring_grabber_changing == 0 && (rc_input < 64 || rc_input > 196))
{
if (grabber_closed)
{
SetPWM(1, 255); // open it
}
else
{
SetPWM(1, 50); // close it
}
grabber_closed = !grabber_closed; // update state
ring_grabber_changing = 1;
}
else if (ring_grabber_changing == 1)
{
if (!(rc_input < 64 || rc_input > 196))
{
StartTimer(1);
timer1 = GetTimer(1);
timer2 = timer1 + 100;
ring_grabber_changing = 2;
}
}
22
Example Debouncer Code (cont’d)
else if (ring_grabber_changing == 2)
{
if (rc_input < 64 || rc_input > 196)
{
// oops, got another press within the timeout period
ring_grabber_changing = 1;
}
else
{
timer1 = GetTimer(1);
if (timer1 > timer2)
{
ring_grabber_changing = 0;
StopTimer(1);
}
}
}
23
Advanced Operator Control
Sequenced Commands
24
Sequenced Commands
Essentially a combination of operator control and a touch of
autonomous
Instead of doing one action when the driver presses a button, do
several
Example:
Raise arm
Open grabber
Lower arm
Simpler example:
Drive motor until it hits limit (without needing to hold button down)
Be careful
Make sure the driver really always wants the whole sequence to
happen
Make sure your limit switches are correct and robust!
How to code it?
Combine push-button operation code (previous slides) with
autonomous sequencing (coming up)
25
Advanced Autonomous
26
Advanced Autonomous
27
Multiple Autonomous Modes
Driving X Inches
Driving Straight
Making Turns (Turning Y degrees)
Sequencing Commands (Basic)
Sequencing Command (Advanced)
Advanced Autonomous
Multiple Autonomous Modes
28
Multiple Autonomous Modes
Why is it desirable to have multiple autonomous
modes?
Adjust strategy based on opponent’s strategy
Turn direction depends on side of field
Testing and calibration (more on these later)
Drive 10 feet
Rotate 360 degrees
29
Mode Selection
So this sounds like a pretty good idea.
Only… how do we do the selection?
Easy: use VEX jumpers in digital input ports!
Jumper inserted: input=0
Jumper not inserted: input=1
30
Binary Encoding
Ideally we would like to use as few inputs as possible
One jumper per mode sounds straightforward, until you
want 8 autonomous modes!
The solution: binary encoding of jumpers
Hint: Put a table like this one on your robot!
31
Dig1
Dig2
Dig3
Mode
Description
Out
Out
Out
1
Drive straight 5 feet and stop
Out
Out
In
2
Fancy auto - red
Out
In
Out
3
Fancy auto - blue
Out
In
In
4
In
Out
Out
5
In
Out
In
6
In
In
Out
7
Rotate 360 calibration
In
In
In
8
Drive 10 feet calibration
Code Structure
32
Read the jumper inputs in Autonomous()
Create functions auto1(), auto2(), etc. and call them based on the jumper
inputs
This helps better organize your code
void Autonomous(unsigned long ulTime)
{
jumper0 = GetDigitalInput(1);
jumper1 = GetDigitalInput(2);
jumper2 = GetDigitalInput(3);
auto_config = 1+(jumper0==0)+2*(jumper1==0)+4*(jumper2==0);
if (auto_config == 1)
{
auto1();
}
else if (auto_config == 2)
{
auto2();
} …
}
Advanced Autonomous
Driving X Inches
33
Encoders – Theory of Operation
Optical Encoders count the number of rotations of a
shaft
They do this by shining a light through a slotted
wheel onto a light sensor
This results in a pulse train
By hooking up the encoder output to an interrupt
port and counting the number of interrupts, we
know how many slots have passed by the sensor
Since the slots are equally spaced, we know the
angle the wheel (and thus the shaft) has traveled
Quadrature encoders are a bit more clever and can
determine the direction of rotation
I won’t cover quadrature encoders here
34
Going The Distance
Encoders count 90 ticks per revolution
ticks 90 rotations
encoder
Knowing the gear ratio between the encoder shaft
and the wheels tells you how many times the wheels
rotate for each rotation of the encoder shaft
rotations
wheel
gearratio rotations
encoder
Knowing the diameter of the wheel tells you how far
the wheel has traveled (its circumference) for each
rotation of the wheel shaft
dist diameter rotations
35
wheel
Solving for Ticks
We want to find the number of ticks needed to go a certain
distance, so let’s solve for ticks:
rotations
rotations
encoder
wheel
rotations
dist
diameter
wheel
gearratio
ticks 90 rotations
encoder
dist
diameter gearratio
90 dist
diameter gearratio
Now we just need two constants (wheel diameter and gear
ratio), and we’ll be able to determine how many ticks it
takes to go a certain distance!
Note the units of wheel diameter is the same as the units
of distance (gear ratio is dimensionless)
Let’s write a function to do this calculation…
36
Calculate Required Ticks
int calc_required_ticks(float dist)
// dist is in inches
{
float wheel_diameter = 2.75; // wheel diameter in inches
float gear_ratio = 1; // assume 1:1 gear ratio
return (90*dist) / (wheel_diameter*3.14*gear_ratio);
}
Or possibly better, since floating point math is SLOW:
int calc_required_ticks(int dist)
// dist is in inches
{
int wheel_diameter = 275; // wheel diameter in hundreths of an inch
int gear_ratio = 1;
// assume 1:1 gear ratio
// scale dist to hundreths of an inch and do integer math
return (90*dist*100) / (wheel_diameter*3*gear_ratio);
}
37
Finishing Up…
Now we just need a loop that drives until we hit the number
of ticks we calculated:
Test drive 10 feet
required_ticks = calc_required_ticks(10*12);
ticks = 0;
// reset encoders
// start PWMs and encoders
while (ticks < required_ticks)
{
ticks = GetEncoder(1);
}
// stop PWMs and encoders
38
Left and Right Encoders
For reasons to be revealed shortly, you probably want to have two
encoders: one on the right drive, one on the left drive.
Easy to modify loop to do this:
Note this stops when either encoder reaches its target
required_ticks = calc_required_ticks(10*12);
left_ticks = 0;
right_ticks = 0;
// reset encoders
// start PWMs and encoders
while (left_ticks < required_ticks &&
right_ticks < required_ticks)
{
left_ticks = GetEncoder(1);
right_ticks = GetEncoder(1);
}
// stop PWMs and encoders
39
Advanced Autonomous
Driving Straight
40
Driving Straight
Great! Now you’re driving exactly 20 inches!
But there’s a problem… for some reason your robot
seems to be curving to the left (or right)
Why isn’t it driving straight!?
Motors aren’t perfect
Gears aren’t perfect
Wheels aren’t perfect
In short, there are many uncontrolled factors that
can make one set of wheels rotate slower than the
other set with the same motor PWM setting
For the purposes of this discussion, we’ll assume
the wheel circumferences are identical – it’s
relatively easy to scale the encoder counts for that
41
The Goal
Ensure each side of the robot moves the same distance in
the same time
Fortunately we already have the sensors we need to do
this: encoders!
We can’t just do this at the end (wait for both encoder
counts to reach the desired number), as that doesn’t
prevent us from curving in the meantime, so…
Dynamically adjust each side’s drive motor power such that
the encoder counts match
We can’t increase the motor power beyond 255 to “catch
up” the slow side, so we need to decrease the motor power
of the faster side
42
Driving Straight – Attempt #1
left_power = 255;
right_power = 255;
// start drive PWMs and encoders
while(1==1)
{
// Get encoder tick counts
if (tick_count_right > tick_count_left)
{
// right’s ahead of left, decrease right power
right_power = right_power - 1;
}
else if (tick_count_left > tick_count_right)
{
// left’s ahead of right, decrease left power
left_power = left_power – 1;
}
// Update drive PWMs
// Check for distance and stop
}
What’s wrong with the above?
43
Driving Straight – Attempt #2
left_power = 255;
right_power = 255;
// start drive PWMs and encoders
while(1==1)
{
// Get encoder tick counts
if (tick_count_right > tick_count_left)
{
// right’s ahead of left, decrease right power
right_power = right_power - 1;
left_power = 255;
}
else if (tick_count_left > tick_count_right)
{
// left’s ahead of right, decrease left power
left_power = left_power – 1;
right_power = 255;
}
// Update drive PWMs
// Check for distance and stop
}
44
Better, but we seem to be drifting faster than we correct!
We could just up the -1, but there’s a better way… use the error!
Driving Straight – Almost Done!
left_power = 255;
right_power = 255;
factor = 16;
// or 8, or...
// start drive PWMs and encoders
while(1==1)
{
// Get encoder tick counts
if (tick_count_right > tick_count_left)
{
// right’s ahead of left, decrease right power
right_power = right_power – (tick_count_left-tick_count_right)/factor;
left_power = 255;
}
else if (tick_count_left > tick_count_right)
{
// left’s ahead of right, decrease left power
left_power = left_power – (tick_count_right-tick_count_left)/factor;
right_power = 255;
}
// Update drive PWMs
// Check for distance and stop
}
45
This loop corrects harder the further away you are from matching tick counts
Only minor cleanups are needed to ensure you don’t saturate
Using the Error
Using the error to proportionally determine how much to
adjust the drive strength by is one of the simplest versions
of a generic loop feedback controller known as “PID”
The PID factors:
“P” – proportional (to the error)
“I” – integral (total accumulation of error over time)
“D” – derivative (how fast the error is changing)
In the control loop just presented,
P=1/factor
I=0, D=0
In small robots like VEX, it’s rarely necessary to use the
other two PID factors
There’s not enough inertia to overwhelm the motors
I won’t delve into PID details (see Wikipedia instead)
46
Advanced Autonomous
Making Turns (Turning Y Degrees)
47
Making Turns
Just turning is easy… drive left forward and right back, or
vice-versa; but how to measure the angle?
One of the more reliable ways is to use a gyro sensor
Gyro measures rotational rate
Code integrates over time to determine angle
However, in FTC we can’t use one
Gyro isn’t a standard VEX sensor
Simply measuring time to turn is unreliable (just like
measuring distance by time)
Next best thing: optical encoders on the drivetrain
Even better, we already have them there for driving a
specified distance!
48
Turning Ticks
So how far do your wheels drive to turn a full
circle (360 degrees)?
Unfortunately, this can be quite hard to calculate
Wheel slip – depends on the surface
Non-centered axis of rotation
It’s easier to measure it empirically
Create an autonomous mode that tries to spin
exactly 360 degrees based on a tick count
Adjust the tick count until the robot rotates 360
degrees
49
Turning Ticks
Your rotation routine then just needs to scale that
360 degree rotation tick count by the number of
degrees to determine how many ticks to rotate
for:
ticks
ticks 360 degrees
360
You will probably have to recalibrate if the
surface or robot weight changes
50
Advanced Autonomous
Sequencing Commands (Basic)
51
Basic Command Sequencing
Write self-contained functions to drive X inches,
rotate Y degrees, stop, etc
Each function returns when the movement is
complete
void drive(int dist)
{
…returns when finished driving…
}
void rotate(int degrees)
{
…returns when finished rotating…
}
52
Functions with Limits
Let’s assume you have upper and lower limits on
the arm
You did have the mechanical team put limits on,
right?
You need to check these limits in autonomous
mode and stop the motors when the limits are hit
No driver to stop pressing the button down!
The concept:
Start the motor driving
Loop until the end limit is hit
Stop the motor driving
Return
53
Functions with Limits
54
void move_arm(int up)
{
if (up)
{
SetPWM(…);
}
else
{
SetPWM(…);
}
while (1==1)
{
upper_limit = GetDigitalInput(…);
lower_limit = GetDigitalInput(…);
if ((up && upper_limit == 0) || (!up && lower_limit == 0))
{
SetPWM(…, 127);
return;
}
}
}
Sequencing
Then string these functions together into a
sequence:
void auto1(void)
/* raises arm, drives 5 feet forward,
rotates 90 degrees, then drives 1 foot
backward and opens the grabber */
{
move_arm(1); // move arm up
drive(5*12);
rotate(90);
drive(-12);
set_grabber(1);
// open grabber
}
55
Advanced Autonomous
Sequencing Commands (Advanced)
56
Advanced Sequencing of Commands
Okay, you can drive, turn, move the arm up and
down, and open the grabber…
What if you want to do more than one of these
things simultaneously?
We won’t talk about driving and turning
simultaneously, but what about driving and moving
the arm up or down at the same time?
The functions we wrote in the previous section
won’t handle this!
They don’t return until the action is complete
57
Breaking Things Up
We need to break our earlier functions into two:
A function to start the action
A function to check if the action is complete
Then we can start two actions, and check if
either/both actions are completed before
proceeding to the next step
Be careful if only checking for one action to
complete; the other motor will keep driving if it
didn’t end first!
Note: Any function-local variables need to become
global variables so they’re shared between both
start and check functions
58
Broken-Up Move Functions (Example)
void start_move(int power)
{
auto_motor_left_power = power; // now global
auto_motor_right_power = power; // now global
// Preset encoders to zero
// Start encoders
// Start drive motors with SetPWM()
}
int move_check_end(int required_ticks, int power)
// returns 1 when move complete
{
// get encoder counts
// manipulate motor power for driving straight
// update motor PWMs
if (tick_count_right > required_ticks || tick_count_left > required_ticks)
{
// stop motor PWMs here
return 1; // move has completed!
}
return 0;
// need to keep on movin’
}
59
Updated Sequencing
void auto1(void)
{
required_ticks = calc_required_ticks(50);
start_move(255);
start_arm(1);
done = 0;
while (!done)
{
arm_check_end(1);
done = move_check_end(required_ticks, 255);
}
// might want to force-stop arm here in case move finished first
// next step in sequence
}
60
Even More Advanced Sequencing
What if you want your sequence to branch or loop,
instead of just always execute in forward order?
The solution: a state machine!
The basic structure: a forever loop around a big
switch (or if/else if/else if) statement
The switch condition is the “state”
The default is to stay in the current state
If you don’t update the state variable, you keep
running the same code thanks to the forever
loop
Update the state variable to move to any other
state
New state can be different based on input
conditions
61
Simple State Machine Example
state = 0;
// start in state 0
while (1==1)
{
if (state == 0)
{
required_ticks = calc_required_ticks(50);
start_move(255);
start_arm(1);
state = 1;
// immediately go to next state
}
else if (state == 1)
{
arm_check_end(10, 1);
done = move_check_end(required_ticks, 255);
if (done)
{
state = 2;
}
}
else if (state == 2)
{
// do nothing
}
}
62
Parting Thoughts
Start early!
Have fun!
Resources:
Chief Delphi: http://www.chiefdelphi.com/
63