Killer Autonomous Programming Make um say “Wow!” Presented By: Frank Larkin Lansdale Catholic Robotics, Team 272 FIRST Championships Forum, Atlanta Georgia April, 2005

Autonomous Program Development Cycle Joy Pain  Time – Lots Of It! A brief moment of optimism usually followed by a crushing downfall! Warning: DO NOT BE FOOLED!

Topics Big Al says… They put it there for you to use! Where does your code go? #defines can be fun! The OI Panel LEDs Competition Mode Autonomous Setup Define Your Steps Virtual Operator The Autonomous Program Using Encoders The Gyro Chip is my Friend! 2005 Programs Fly – By – Wire System

Big Al says…

They put it there for you to use! Use whatever field objects you can to guide you. Walls can be run along. –Shoes – set to glide below obstacles –Large bearings can easily run along walls or rails. Read the rules very carefully to make sure your “hardware solution” is legal and will work. If all else fails… –Hold breathe until you turn blue –Suggest W W W S! –Write a position paper If you must, write software!

Where They Want You To Put Your Code! The Default FRC Code strongly suggests that you keep your Autonomous Code separate…but do you? while (1) /* This loop will repeat indefinitely. */ { #ifdef _SIMULATOR statusflag.NEW_SPI_DATA = 1; #endif if (statusflag.NEW_SPI_DATA) /* 26.2ms loop area */ { /* I'm slow! I only execute every 26.2ms because */ /* that's how fast the Master uP gives me data. */ Process_Data_From_Master_uP(); /* You edit this in user_routines.c */ if (autonomous_mode) /* DO NOT CHANGE! */ { User_Autonomous_Code(); /* You edit this in user_routines_fast.c */ } Process_Data_From_Local_IO(); /* You edit this in user_routines_fast.c */ /* I'm fast! I execute during every loop.*/ } /* while (1) */

A Better Way! Make your autonomous code part of your normal code. Yes change where they said DO NOT CHANGE! while (1) /* This loop will repeat indefinitely. */ { #ifdef _SIMULATOR statusflag.NEW_SPI_DATA = 1; #endif if (statusflag.NEW_SPI_DATA) /* 26.2ms loop area */ { /* I'm slow! I only execute every 26.2ms because */ /* that's how fast the Master uP gives me data. */ Process_Data_From_Master_uP(); /* You edit this in user_routines.c */ } Process_Data_From_Local_IO(); /* You edit this in user_routines_fast.c */ /* I'm fast! I execute during every loop.*/ } /* while (1) */

MY Process_Data_From_Master_uP(); void Process_Data_From_Master_uP(void) { Getdata(&rxdata); /* Get fresh data from the master microprocessor. */ LC_Main(); Generate_Pwms(pwm13,pwm14,pwm15,pwm16); Putdata(&txdata); /* DO NOT CHANGE! */ } Get the data from the external inputs Process Them (LC_Main) Put the data to the outputs

LC_Main() void LC_Main(void) { int_TimeCount++; ShutOffAllLEDs(); PreCompetitionSetup(); AutonomousOperation(); NormalOperation(); } void PreCompetitionSetup() { unsigned int uint_GyroValue; if( competition_mode != TRUE ) return; // rest of pre-comp code below here } void AutonomousOperation() { if( autonomous_mode != TRUE ) { byt_AutoState = 0; byt_AutoSearchState = 0; return; } // rest of Autonomous code below here }

Horsepower 2004

Control Inputs #define Declarations Declarations make it easy to read and follow. Also they can be used by their normal reference… // Joy sticks and Wheels #define pot_TowerWinchp1_y #define pot_TowerTiltp2_y #define pot_LeftDriverJoystickp3_y #define pot_RightDriverJoystickp4_y #define pot_AutTimer1p1_wheel// used to determine how far // robot goes to goal #define pot_GoalClawp2_wheel // Switches #define SWITCH_OPEN0 #define SWITCH_CLOSED1 #define swt_RightArmDeployp1_sw_trig #define swt_RightArmRetractp1_sw_top #define swt_LeftArmDeployp2_sw_trig #define swt_LeftArmRetractp2_sw_top #define swt_BallSuctionp3_sw_trig #define swt_BallReleasep3_sw_top #define swt_ReverseControlsp4_sw_trig #define swt_FullPowerp4_sw_top #define swt_GyroNavigatep4_sw_aux1

Autonomous Program #define Declarations Declaration make it easy to read and follow… #define C_AUT_FIRST_PROGRAM0 #define C_AUT_NO_PROGRAM 0// do nothing #define C_AUT_GOAL_RETRIEVE 1 #define C_AUT_HIT_BALL 2 #define C_AUT_DEFAULT_PROGRAM 2 #define C_AUT_LAST_PROGRAM 2 #define C_AUT_STEP_PULL_BACK_GOAL90 #define C_AUT_LAST_STEP99 #define C_AUT_POWER_GO_SLOW_TO_GOAL 140 #define C_AUT_POWER_TILT_UP255 #define C_AUT_POWER_WINCH_UP255 #define C_AUT_POWER_WINCH_DOWN_TO_BALL #define C_AUT_POWER_WINCH_UP_WITH_BALL #define C_AUT_POWER_STOP127 #define C_AUT_POWER_HALF_FORWARD165 #define C_AUT_POWER_FULL_FORWARD255 #define C_AUT_COUNT_TILT_UP110 #define C_AUT_COUNT_WINCH_UP150 #define C_AUT_COUNT_WINCH_UP_BALL 20

The OI Panel LEDs Maim.h definition /******************************************************************* OI LED DEFINITIONS *******************************************************************/ #define LED_ON1 #define LED_OFF0 #define LED_OI_0 txdata.LED_byte1.bitselect.bit1 #define LED_OI_1 txdata.LED_byte1.bitselect.bit0 #define LED_OI_2txdata.LED_byte1.bitselect.bit3 #define LED_OI_3 txdata.LED_byte1.bitselect.bit2 #define LED_OI_4 txdata.LED_byte1.bitselect.bit4 #define LED_OI_5 txdata.LED_byte1.bitselect.bit5 #define LED_OI_6 txdata.LED_byte1.bitselect.bit6 #define LED_OI_7 txdata.LED_byte1.bitselect.bit7 #define LED_OI_8 txdata.LED_byte2.bitselect.bit0 #define LED_OI_9 txdata.LED_byte2.bitselect.bit1 #define LED_OI_10 txdata.LED_byte2.bitselect.bit2 // Function placed at the top of the main function shuts off all the // LEDs because your program will adjust them. void ShutOffAllLEDs() { LED_OI_0 = LED_OFF; LED_OI_1 = LED_OFF; LED_OI_2 = LED_OFF; LED_OI_3 = LED_OFF; LED_OI_4 = LED_OFF; LED_OI_5 = LED_OFF; LED_OI_6 = LED_OFF; LED_OI_7 = LED_OFF; LED_OI_8 = LED_OFF; LED_OI_9 = LED_OFF; LED_OI_10 = LED_OFF; }

Competition Mode In Competition mode OI inputs will work. These can be used to… –Set your Autonomous Mode –Center your Joysticks –Test your sensors –Set delay timers Yellow Light

Competition Mode In Competition mode OI inputs will work. These can be used to… –Center your Joysticks –Test Your sensors –Set Your Autonomous Mode –Set delay timers

Autonomous Setup Function: PreCompetitionSetup void PreCompetitionSetup() { unsigned int uint_GyroValue; if( competition_mode != TRUE ) return; int_TimeCount = 0; byt_CounterToGoal = pot_AutTimer1_IN; // set the autonomous program mode if( p1_sw_trig == SWITCH_CLOSED ) { if( p1_sw_top == SWITCH_OPEN ) byt_ButtonWasPressed = FALSE; if( p1_sw_top == SWITCH_CLOSED && byt_ButtonWasPressed == FALSE ) { byt_AutonomousProgram++; byt_ButtonWasPressed = TRUE; if( byt_AutonomousProgram > C_LAST_PROGRAM ) byt_AutonomousProgram = C_FIRST_PROGRAM; }

Function: PreCompetitionSetup Selecting You Auto Program switch( byt_AutonomousProgram ) { case C_AUT_GOAL_RETRIEVE: LED_OI_1 = LED_ON; break; case C_AUT_HIT_BALL: LED_OI_2 = LED_ON; break; case C_AUT_TURN_AROUND_ON_LEFT_SIDE: LED_OI_2 = LED_ON; break; case C_AUT_TURN_AROUND_ON_RIGHT_SIDE: LED_OI_3 = LED_ON; break; case C_AUT_GO_NEAR_ON_RIGHT_SIDE: LED_OI_5 = LED_ON; break; case C_AUT_NO_PROGRAM: default: LED_OI_0 = LED_ON; break; }

Function: PreCompetitionSetup Centering Your Controls else if( p2_sw_trig == SWITCH_CLOSED ) { if(user_display_mode == TRUE) /*User Mode is On */ txdata.LED_byte1.data = byt_CounterToGoal; } else // normal mode in precompetiiton setup is to zero the axis of joy stick { if( p1_y >= 126 ) LED_OI_0 = LED_ON; if( p1_y <= 128 ) LED_OI_1 = LED_ON; if( p2_y >= 126 ) LED_OI_2 = LED_ON; if( p2_y <= 128 ) LED_OI_3 = LED_ON; if( p3_y >= 126 ) LED_OI_4 = LED_ON; if( p3_y <= 128 ) LED_OI_5 = LED_ON; if( p4_y >= 126 ) LED_OI_6 = LED_ON; if( p4_y <= 128 ) LED_OI_7 = LED_ON; }

Function: PreCompetitionSetup Centering Your Controls else if( p2_sw_trig == SWITCH_CLOSED ) { if(user_display_mode == TRUE) /*User Mode is On */ txdata.LED_byte1.data = byt_CounterToGoal; } else // normal mode in precompetiiton setup is to zero the axis of joy stick { if( p1_y >= 126 ) LED_OI_0 = LED_ON; if( p1_y <= 128 ) LED_OI_1 = LED_ON; if( p2_y >= 126 ) LED_OI_2 = LED_ON; if( p2_y <= 128 ) LED_OI_3 = LED_ON; if( p3_y >= 126 ) LED_OI_4 = LED_ON; if( p3_y <= 128 ) LED_OI_5 = LED_ON; if( p4_y >= 126 ) LED_OI_6 = LED_ON; if( p4_y <= 128 ) LED_OI_7 = LED_ON; }

Function: PreCompetitionSetup Checking Other Stuff // test and set the gyro chip uint_GyroValue = Get_Analog_Value(ana_GyroChip_IN); if( int_GyroNormalSetting == 0 && uint_GyroValue > 500 ) int_GyroNormalSetting = uint_GyroValue; if( uint_GyroValue > int_GyroNormalSetting - C_GYRO_RANGE && uint_GyroValue < int_GyroNormalSetting + C_GYRO_RANGE ) { LED_OI_8 = LED_ON; } if( dig_Banner_Sees_Line_IN == DIGITAL_HIGH ) LED_OI_9 = LED_ON; //Tower Winch is Going Down: if( dig_TowerCableLoose_IN == DIGITAL_LOW ) LED_OI_10 = LED_ON; }

Function: PreCompetitionSetup Autonomous Delay Why use a delay? –Allow alliance to go first. –Keep them out of your way. Delay set like program selection. –Set in 2 second increments –Clock starts when autonomous period begins. –Display counts down to allow CBUs to feel warm and fuzzy that it is all working.

Function: PreCompetitionSetup Autonomous Delay Why use a delay? –Allow alliance to go first. –Keep them out of your way. Delay set like program selection. –Set in 2 second increments –Clock starts when autonomous period begins. –Display counts down to allow CBUs to feel warm and fuzzy that it is all working.

Define Your Steps Clearly Define what you want to do… 0) As you do steps below tilt up, then raise tower. 1) Drive at 45 degree angle to side bar of field. 2) Run along wall on shoe on right side of robot. 3) As you pass over white line fire arm to hit ball and stop. Leave arm out. 4) Backup a little to avoid hitting other ball and close arm 1 2 3,4

Horsepower 2004

The Virtual Operator Concept In Autonomous Mode controls are set to neutral positions –buttons set to 0 –joysticks set to 127 or center –But variables still exist!!!!!! Your Autonomous Functions can set these so they can be processed by your normal code as if an operator were doing the work. This eliminates “special” code for autonomous operations. Makes code flow much cleaner Uses less variable space

Autonomous Operation void AutonomousOperation() { if( autonomous_mode != TRUE ) { byt_AutoState = 0; byt_AutoSearchState = 0; return; } // stop all movement if there is any. // actions will be set in the // autonomous code if it needs to be pot_TowerWinch_IN = C_POWER_STOP; pot_TowerTilt_IN = C_POWER_STOP; pot_LeftDriverJoystick_IN = C_POWER_STOP; pot_RightDriverJoystick_IN = C_POWER_STOP; swt_RightArmRetract_IN = SWITCH_CLOSED; switch( byt_AutonomousProgram ) { case C_AUT_GOAL_RETRIEVE: AutoGoalRetrieve(); break; case C_AUT_HIT_BALL: AutoHitBall(); break; default: break; }

AutoHitBall() Part 0 – Postion Tower void AutoHitBall() { // do this independent of what state we are in // tilt up first to allow arm to clear if( int_TiltCount < C_AUT_COUNT_TILT_UP ) { pot_TowerTilt = C_AUT_POWER_TILT_UP; int_TiltCount++; } else if( int_WinchCount < C_AUT_COUNT_WINCH_UP ) { pot_TowerWinch = C_AUT_POWER_WINCH_UP; int_WinchCount++; }

Autonomous Program Part 1 – The State Machine We use an integer to tell us what “state” we are in. This tells us what part of the code to process. switch( byt_AutoState ) { case 0:// Initialization of this state int_TimeCount = 0; byt_AutoState++; case 1: // drive straight towards side pot_LeftDriverJoystick_IN = ; pot_RightDriverJoystick_IN = ; if( int_TimeCount > 20 ) { int_TimeCount = 0; // reset for next state byt_AutoState++; // bump to next state } break;

Autonomous Program Part 2 – Timer Based Actions We keep doing one thing until the counter is exceeded then do another or bump the state… case 2:// run to the side swt_GyroNavigate = TRUE; pot_LeftDriverJoystick = ; pot_RightDriverJoystick = ; if( int_TimeCount > 150 ) { LED_OI_8 = LED_ON; // tell CBUs we are working if( dig_Banner_Sees_Line == DIGITAL_HIGH ) { swt_RightArmDeploy = SWITCH_CLOSED; pot_LeftDriverJoystick = C_POWER_STOP; pot_RightDriverJoystick = C_POWER_STOP; int_TimeCount = 0; // reset timer byt_AutoState++; // bump to next state } if( int_TimeCount > 350 ) // in case we miss the line { pot_LeftDriverJoystick = C_POWER_STOP; pot_RightDriverJoystick = C_POWER_STOP; int_TimeCount = 0; // reset the timer for next state byt_AutoState = C_AUT_LAST_STEP; } break;

Autonomous Program Part 3 – Stop And Settle Inertia can cause all kinds of problems. Sometimes you must stop and wait a little for the robot to come to a full stop. case 3://stop swt_RightArmDeploy = SWITCH_CLOSED; pot_LeftDriverJoystick = C_POWER_STOP; pot_RightDriverJoystick = C_POWER_STOP; if( int_TimeCount > 30) { int_TimeCount = 0; // reset the timer state byt_AutoState++; // bump to next state } break;

Autonomous Program Part 4 – Stop And Settle Last two states for this program… case 4: //back up // keep arm deployed to keep it from hitting other ball swt_RightArmDeploy = SWITCH_CLOSED; pot_LeftDriverJoystick = 60; pot_RightDriverJoystick = 60; swt_GyroNavigate = TRUE; if( int_TimeCount > 40) { int_TimeCount = 0; // reset the timer for next state byt_AutoState = C_AUT_LAST_STEP; // set last state } break; case C_AUT_LAST_STEP:// STOP AND CLOSE default: swt_GyroNavigate = FALSE; swt_RightArmRetract = SWITCH_CLOSED; // close arm pot_LeftDriverJoystick = C_POWER_STOP; pot_RightDriverJoystick = C_POWER_STOP; break; }

Using Encoders Use Optical sensor on Encoder wheel to determine how far you have gone. Create encoder wheel mask with program Encoder_design.exe free on Internet (

Encoder Masks

Encoder Placement Place on wheel, gear, freshman..whatever rotates that you want to measure. For best accuracy place on part that rotates the most. More rotations = more accuracy Use 2, one left side one right. Create DIRECTION variable when left sensor changes subtract 1, when right changes add 1. If Direction = 0 you are going straight.

Encoder Placement Place on wheel, gear, freshman..whatever rotates that you want to measure. For best accuracy place on part that rotates the most. More rotations = more accuracy Use 2, one left side one right. Create DIRECTION variable when left sensor changes subtract 1, when right changes add 1. If Direction = 0 you are going straight.

Untested Encoder Code Example main.h uint compass = 5000; uint direction = compass; uint left_last_state = 0; uint right_last_state = 0; #define left_sensor rc_dig_in01 #define right_sensor rc_dig_in02 normal.c // going forward only if( left_sensor != left_last_state ) direction--; if(right_sensor != right_last_state ) direction++; if( direction < compass ) adjust_power_to_left(); if( direction > compass ) adjust_power_to_right(); left_last_state = left_sensor; right_last_state = right_sensor;

Encoder Issues Adjusting power may mean reducing power on side you want to turn to. Wheel Slip – Gunning motors may make wheels slip causing sensors to count without moving. Ramp up speed. Rotation speed – Too fast and counter may not keep up. What happens if I change the compass? Use the Force Luke! –Always think what is happening. –All problems can and should be explained.

The Gyro Chip A solid state Gyroscope. Indicates that we are tuning and how “hard”. Analog Input –Must use Get_Analog_Value function to retrieve it –Number at rest is around 536. –Do Not Hard Code –Turn left number raises –Turn right number decreases Shock mount unit –Use with caution because a 130 lb. robot can cause damage. Especially to you.

The Gyro Chip During Competition Mode read Chip to set at_rest_value –Then set an at_rest_high (+4) and at_rest_low (- 4) When sensing determine.. –if < at_rest_high (left) –If > at_rest_low (right) –Difference is amount or severity of turn Add or subtract to virtual compass. This indicates direction you are going.

The Gyro Chip Example main.h #define straight uint compass = straight; uint gyro_value = 0; uint gyro_normal = 0; uint gyro_high = 0; uint gyro_low = 0; uint gyro_diff=0; precompetition.c gyro_normal = Get_Analog_Value(rc_ana_in01); gyro_high = gyro_ normal +4; gyro_low = gyro_ normal -4; normal.c gyro_value = Get_Analog_Value(rc_ana_in01); if(gyro_value < gyro_high ) { gyro_diff = gyro_value - gyro_normal; compass = compass + gyro_diff; } elseif(gyro_value > gyro_low ) { gyro_diff = gyro_normal - gyro_value; compass = compass - gyro_diff; }

One program (8 in 1) that can move from any start position to any loading zone. –Same program just change a few variables to go different places –Uses encoders to know how far we have gone. Encoders are calibrated in inches. –Uses automatic positioning of arm to put claw in best position for loading. Second program loads a tetra on to side goal Programs

Fly By Wire System Tower uses potentiometers to know tilt and winch positions. –Single turn pot for tilt –10 turn pot for tower winch –Operator pushes button to position tower. –Selectors for start position, tertas on robot and goal –Automatically repositions as we drive.

Fly By Wire System Robot Goal Start Fly-By-Wire System Press and hold ½ second to record new position

Final Thought Unlike baseball crying is allowed in software development… but … …when your done, get back and make it work!