1 Advanced Programming in the VEX Environment Peter Johnson Northrop Grumman Space Technology Programming Mentor, Beach Cities Robotics (FRC/FTC/VRC Team

1

Advanced Programming in the VEX EnvironmentPeter JohnsonNorthrop Grumman Space TechnologyProgramming Mentor, Beach Cities Robotics(FRC/FTC/VRC Team 294)1 Mar 2008

2

Agenda

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


Limiting range of motion with sensors Toggle buttons

Push once to open, push again to close Sequenced commands

8


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);



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)

{




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


“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)

{


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)

{


if (!grabber_changing && (rc_input < 64 || rc_input > 196)) // either button

{

if (grabber_closed)

{


}

else

{


}


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


if (ring_grabber_changing == 0 && (rc_input < 64 || rc_input > 196))

{

if (grabber_closed)

{


}

else

{


}


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;


}

}

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


}

else

{

timer1 = GetTimer(1);

if (timer1 > timer2)

{


StopTimer(1);

}

}

}

23


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

Multiple Autonomous Modes Driving X Inches Driving Straight Making Turns (Turning Y degrees) Sequencing Commands (Basic) Sequencing Command (Advanced)

27

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

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);



auto_config = 1+(jumper0==0)+2*(jumper1==0)+4*(jumper2==0);

if (auto_config == 1)

{

auto1();

}

else if (auto_config == 2)

{

auto2();

} …

}

32

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

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

Knowing the diameter of the wheel tells you how far the wheel has traveled (its circumference) for each rotation of the wheel shaft

35

encoderwheel rotationsgearratiorotations

wheelrotationsdiameterdist

encoderrotationsticks 90

Solving for Ticks

We want to find the number of ticks needed to go a certain distance, so let’s solve for ticks:

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

gearratiodiameter

distrotationsticks encoder

90

90

diameter

distrotationswheel

gearratiodiameter

dist

gearratio

rotationsrotations wheel

encoder

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;


while(1==1)

{



{


right_power = right_power - 1;

left_power = 255;

}


{


left_power = left_power – 1;

right_power = 255;

}



}

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!

44

Driving Straight – Almost Done!

left_power = 255;

right_power = 255;

factor = 16; // or 8, or...


while(1==1)

{



{


right_power = right_power – (tick_count_left-tick_count_right)/factor;

left_power = 255;

}


{


left_power = left_power – (tick_count_right-tick_count_left)/factor;

right_power = 255;

}



}

This loop corrects harder the further away you are from matching tick counts Only minor cleanups are needed to ensure you don’t saturate

45

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:

You will probably have to recalibrate if the surface or robot weight changes

50

360360 degreesticks

ticks

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

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;

}

}

}

54

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

http://www.chiefdelphi.com/

Documents

1 Advanced Programming in the VEX Environment Peter Johnson Northrop Grumman Space Technology Programming Mentor, Beach Cities Robotics (FRC/FTC/VRC Team