FIRST Robotics Team 1619 Programming Workshop

FIRST RoboticsTeam 1619 Programming Workshop Programming the Control System Mark Dotterweich Team 1619 Mentor Presentation available at: www.coloradofirst.org/kickoff

Control System Overview • Four Basic Components • Operator Interface - “OI” • Robot Controller – “FRC” or “EDU” • Radio/Modem or tether • Input & Output Devices • Two Modes of Operation • Operator Controlled • Autonomous

Operator Interface (OI) • Receives operator control requests via: • Joysticks (1 to 4) • Digital inputs (e.g push button or toggle switches) • Analog inputs (e.g. potentiometers) • Competition Port (sets autonomous vs operator mode and team color) • Sends operator requests to robot controller via radio or tether • Provides LED panel for operator awareness • 11 of them are user programmable, • one is for robot battery voltage

Y 255 X 0 255 0 Joystick Control • Operates on an X & Y grid • X and Y ranges are from 0 to 255 (2^8) • Center point is (127, 127) • Values sent to robot are in (X,Y) pairs for each joystick

Robot Controller • Contains software that enables operation • Simple Input, Process, Output Loop Concept • Receives inputs from: • Operator Interface via Radio • Analog Devices (e.g. accelerometer, pot’s) • Digital Devices (e.g. limit switches) • Receives interrupts from: • Serial ports • Encoders • Processes data based on downloaded program • Sends outputs to: • Speed Controllers (motor control) • Servos (Camera mount/gimbal) • Switches (pneumatic controls) • Team Lights (red, blue)

Robot Controller (2) • Two Microchip PIC18F8520 8 bit computers on a chip. A Master and a Slave • 32k ROM, 2k RAM, 16 A/D, 5 PWM, 5 Timers, 2 Serial ports, 68 I/O • 40 MHz, 4 clocks per instruction (8 really), most instructions 2 bytes. • Master • Bi-directional communications to the OI • Operates many of the outputs • Watches the slave for proper functioning. • Is programmed by First • Slave • Runs the user program • Operates some of the outputs (PWM 13-16)

Programming Environment • You will need to read and re-read much of the documentation • Programming language is PIC C • Programming tools - Microchip MPLAB IDE • Editor, compiler, linker and debugger • C-BOT compiler • Easy-C Development Toolkit (see page 20) • Programs are downloaded from laptop to Robot Controller via IFI_Loader • Requires serial interface • USB to serial converter needed for most laptops

Default Program • Default program provided from IFI • Reference guide included in attachments • Provides example of: • One joystick drive • Two or four motors – 4 wheel drive – skid steer • Joystick switches controlling 1-4 relays on the robot

Program Structure • Basic loop • Flagged every 26.2ms (17ms on EDU) void main (void) { IFI_Initialization( ); User_Initialization( ); statusflag.NEW_SPI_DATA = 0; while (1) /* This loop will repeat indefinitely. */ { if (statusflag.NEW_SPI_DATA) /* 26ms loop area */ { /* I'm slow! I only execute every 26ms 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( ); /* I'm fast! I execute during every loop.*/ } /* while (1) */ } /* END of Main */

Program Structure 2 (example user code) void Process_Data_From_Master_uP(void) { static byte first_pass = TRUE; Getdata(&rxdata); /* Get fresh data from the master microprocessor. */ if (first_pass) { /* set which routine to run based on jumper setting */ auto_mode_switch = rc_dig_in01 | rc_dig_in02 << 1 | rc_dig_in03 << 2; starting_pos = rc_dig_in04 | rc_dig_in05 << 1; first_pass = FALSE; } // if (p2_sw_trig) if (autonomous_mode) Execute_Selected_Autonomous_Routine( ); else { Tank_Drive_Manual_Operation(); /* Get drive operations from OI - drivers */ Arm_Control_Manual_Operation(); /* Get arm operations from OI - drivers */ } Putdata(&txdata); /* DO NOT CHANGE! */ } Define & set any necessary variables Get data from input devices If first time in, get initialization settings (autonomous mode and starting position are set by dip-switches on robot) Can use an operator input to “fake” autonomous mode indicator. (Remember to comment out at competition!) If not in autonomous mode, then execute the manual operator input routines. Send data to output devices based on input and processing.

Notes on program speed • You don’t really need to worry • In general smaller = faster • use byte variables when possible • use unsigned if multiplying and can • use powers of two and shifting • <<, >>, & and | may look strange but they work

Autonomous Programming • Runs when “Autonomous_Mode” flag set • Controlled by FIRST via competition port • Autonomous code separated from normal code via “IF” statements that branch based on mode flag • Pseudo switch created to test auto code (p2_sw_trig on page 10) • Autonomous code can utilize the same sub-routines as those used during manual operation • Autonomous can fake the input (rxdata) from OI and you can use the same code. • Autonomous control generally implemented as a “State Machine”

State Machines • State Machines operate as a series of “states” that the machine transitions through • Typically, these consist of an initialization state and then a series of states that change based on various input conditions • Conditions include: • Time (elapsed or target times) • Sensor inputs (e.g. limit switches, location or distance sensors, rotation counters, etc.) • Most states are active in that some task is being performed, but there are “Wait” states that delay to allow some external event or condition to occur • Example tasks include driving a certain distance, raising an arm to a specific level, releasing an object (ball or tetra)

State Machine – Example from 2006 Robot (“The Black Pearl”) • Task – Execute shooting one ball and reload to have next ball ready. Steps required: • Backup balls in ball-store • Open ball release arm to let one ball drop • Close ball release arm • Reload balls in ball store • Ready for next shot Ball shooter (flywheel) Ball release arm 8 states used for this operation (see next 5 pages) Ball-store

“One Shot” State Machine Code switch(Manual_State) { case STATE_SHOULD_FIRE: if (FIRE_TRIGGER) // wait until trigger pressed to shoot balls { Manual_State = STATE_REVERSE_BELTS; counter = 0; // counting up } break; /* end STATE_SHOULD_FIRE */ case STATE_REVERSE_BELTS: counter += 1; BELT_MOTOR = BELT_SPEED_REVERSE; // reverse motors if (counter==READY_TO_OPEN_DOOR) // long enough to back up 2nd ball { Manual_State = STATE_OPENING_DOOR; // change the state counter=0; //reset the counter } break; /* end STATE_REVERSE_BELTS */ (continued on next 4 pages) Switch flag (Manual_State) is set at initialization to start at first state: (STATE_SHOULD_FIRE) Switch flag is changed to move from one state (case) to another. If trigger is not pulled, then this state never ends. This state moves all the balls but the first one backwards in the ball store. Counter is used to set the time fro the balls to go in reverse.

“One Shot” State Machine Code (2) case STATE_OPENING_DOOR: counter += 1; BELT_MOTOR = NEUTRAL; // turn off the motor ESCAPEMENT_MOTOR = NEUTRAL-63; // open ball escapement door if (counter==READY_TO_SHOOT || ESCAPEMENT_MOTOR_MAX) // if timer has expired or limit hit { Manual_State = STATE_SHOOTING_CANNON; // go to next state ESCAPEMENT_MOTOR = NEUTRAL; // turn off motor counter = 0; //reset counter } break; /* end STATE_OPENING_DOOR */ case STATE_SHOOTING_CANNON: if (FIRE_TRIGGER) { BELT_MOTOR = BELT_SPEED_SHOOTING; // turn forward the belt motor } else // when the driver releases the trigger stop firing { Manual_State = STATE_DONE_SHOOTING; // go to next state counter = 0; // reset the counter } break; /* end SHOOTING_CANNON */ This state opens the ball release. State changes on time or if a limit switch is triggered This state moves the balls forward towards the ball release – this will perform rapid fire if the trigger is not released.

“One Shot” State Machine Code (3) case STATE_DONE_SHOOTING: counter += 1; BELT_MOTOR = NEUTRAL; // ensure belt motor is off if (counter == ESCAPEMENT_MOTOR_CLOSE_DELAY_TIME) // delay before closing { // to allow balls to clear ESCAPEMENT_MOTOR = NEUTRAL+63; // close the door counter = 0; Manual_State = STATE_CLOSING_DOOR; // go to next state } break; /* end DONE_SHOOTING */ This state stops the balls from moving forward and closes the ball release.

“One Shot” State Machine Code (4) case STATE_CLOSING_DOOR: counter +=1; BELT_MOTOR = NEUTRAL; // ensure belt motor is off if (counter == READY_TO_BE_COMPLETED || ESCAPEMENT_MOTOR_MIN) // if limit switch or timer / /has expired go to next state { if (ESCAPEMENT_MOTOR_MIN || Retried_Close_Once) { Manual_State = RESET_NEXT_BALL; // release fully closed so go to idle state Retried_Close_Once = FALSE; // reset for next time } else { Manual_State = RESET_NEXT_BALL; // release got stuck - on a ball? so retry once Retried_Close_Once = TRUE; } ESCAPEMENT_MOTOR = NEUTRAL; // turn off door motor counter = 0; / /reset the counter } break; /* end STATE_CLOSING_DOOR */ This state closes the ball release and checks that the limit switch is triggered (i.e. the release is fully closed). If the limit switch is not triggered, then a ball might be stuck under the release arm and it needs to be opened and closed again.

“One Shot” State Machine Code (5) case RESET_NEXT_BALL: counter += 1; BELT_MOTOR = BELT_SPEED_LOADING; //forward load if (counter==RELOAD_NEXT_BALL_TIME) / /if it's been long enough to get next ball { Manual_State = STATE_COMPLETE; // change the state BELT_MOTOR = NEUTRAL; // turn off the motor counter=0; // reset the counter } break; /* end RESET_NEXT_BALL */ case STATE_COMPLETE: if (FIRE_TRIGGER) // if trigger is set go back to beginning of routine { counter = 0; // reset counter again just to be sure Manual_State = STATE_SHOULD_FIRE; // go to beginning of routine } break; /* end STATE_COMPLETE */ }//end of switch This state puts another ball into the ball release area and then switches to the final state. This is the last state. It resets the switch flag to start at the initial state (case) if the trigger is now depressed. If the trigger is not depressed, the machine will stay in this state.

EasyC Intro & References A visual, menu driven, programming environment that allows users to quickly and easily develop C programs for the FIRST competition while learning the C programming language. The EasyC interface incorporates the complete programming, compiling, and downloading of projects to your Robot Controller. EasyC libraries include capabilities for all sensors (limit switches, gear tooth, gyros, accelerometers, etc.) EasyC also includes various drive train capabilities including 2-wheel drive, 4 wheel drive, tank drive (2 & 4 wheel). 2006 EasyC Available at: http://www.intelitekdownloads.com/FRC2006/

Backup Reference Slides

Additional “Getting Started” Tips fromMark McLeod FRC #0358 (Robotic Eagles) Team Role: Mentor… • Beginners need the following to program the FIRST robot: • FRC (Full-size Robot Controller) RC Default Code • Microchip MPLAB & C compiler • IFI_Loader • A standard serial cable • A Windows PC with a serial port or USB-to-serial converter • All default code, updates, documentation, and support information is available at http://innovationfirst.com/FIRSTRobotics/documentation.htm, the Innovation FIRST (IFI) website, The default code “FRC RC Default Code” comes ready for basic driving and with a variety of sample I/O usages. • MPLAB is the FIRST supplied Windows based development environment (edit, compile, debug) that runs on your desktop computer and is usually provided to each team on a compact disk in the Robovation kit. MPLAB is free on-line at the Microchip website http://www.microchip.com/, but the C compiler is not free except via the Robovation CD and is only available online as a time-limited trial copy or for purchase. Manuals for using MPLAB come on the FIRST CD. • IFI_Loader (from the CD or IFI website above) is used to download the compiled code into the RC via a serial cable from your desktop or laptop computer. • Check the IFI website periodically for updates to the software that corrects issues fixes problems.

Additional “Getting Started” Tips fromMark McLeod FRC #0358 (Robotic Eagles) Team Role: Mentor… • The programming language used is “C” or PIC assembly. Tutorials in C can be found on the web, in your local bookstore or on the FIRST website, e.g., • C Programming Resource Library http://www.usfirst.org/robotics/C_help.htm • Learn C Programming - Developed by Carnegie Mellon and the National Robotics Engineering Consortium specifically for FIRST. http://www.rec.ri.cmu.edu/education/robot_builder/ • FIRST Robovation - A Primer for Success Learning Modules http://www.usfirst.org/robotics/robovation/primer/index.html • Newer laptops no longer come with serial ports. If you have this problem one solution is to use a USB/Serial converter. Various models are available at Radio Shack, CompUSA, or online. • Documentation is your friend. Take the time to at least leaf through each manual, so you have an idea of where information can be found. Most of the basic information beginners require can be found in the IFI documents or MPLAB documents: IFI Programming Reference Guide – basic how to program and download to the RC, hookup switches and sensors and do normal robot operations. • IFI RC Default Code Reference Guide – description of how the default code is structured and where users can add their own custom code. • MPLAB v6.xx Getting Started – basic how to use MPLAB and set options. • MPLAB C18 Users Guide – table of max numbers each variable type will store, compiler options, error messages, detailed descriptions of pragmas and some other advanced topics. • MPLAB C18 Libraries – details on timers, interrupts, and various other utility functions available to the programmer.

Additional “Getting Started” Tips fromMark McLeod FRC #0358 (Robotic Eagles) Team Role: Mentor… • Programming limits to the 2004 RC: • 30,720 bytes program space is available to the user, after 2004 IFI code. • 1,343 bytes of ram available to the user, after 2004 code overhead. • 256 bytes of global variables available within any one MPLAB file. • 120 bytes of variables within a single routine. • The program and data space your code has used can be checked either by the status line at the bottom of the IFI_Loader window or via the optional .map file that MPLAB will generate for you. • Common beginner problems include improper MPLAB project setup, trying to pack too much code into the controller, printf misunderstands, using too small a variable type to hold the largest possible intermediate as well as the largest final value of a calculation, and lack of proper variable typecasting. • Visit the ChiefDelphi Programming forum for discussions on all programming topics, problems, and issues http://www.chiefdelphi.com/forums.

Other Useful Links • FIRST • Introduction to C Programming for FIRST • This presentation developed by David Maxwell of Innovation First, is for absolute beginners who want to get started programming the 2004 FIRST Robot Controller. • http://www.usfirst.org/community/frc/content.aspx?id=482 • Microprocessor • http://www.microchip.com • http://www.microchip.com/stellent/idcplg?IdcService=SS_GET_PAGE&nodeId=1335&dDocName=en010319 • PID • http://www.google.com/search?hl=en&lr=&q=pid+control • Chief Delphi Programming Forum • http://www.chiefdelphi.com/forums/forumdisplay.php?f=51 • C Programming with Robot Builder • http://www-education.rec.ri.cmu.edu/robot_builder/

Use typedef’s and #define !! • Many already in user_routines.h • typedef unsigned char byte; • typedef unsigned short int word; • typedef unsigned short int uword; • typedef signed int short sword; • typedef signed char s8; • typedef signed char sword8; • #define LEFT_MOTOR pwm01 • #define pwm01 txdata.rc_pwm01 // use the compiler

Variables in functions() • static word remember_me; // within a function • Uses up some of the 2k RAM • word forget_me; • Uses up some stack space, re-used by other functions.

ROM and RAM • Use “rom” for tables, you don’t have the RAM • rom char abc[4] = “ABC”; • And pointers to rom are different. • rom char *ptr; static rom const unsigned int VT_Location [8][8] = { {180, 180, 180, 222, 222, 264, 264, 264}, {288, 162, 36, 99, 225, 288, 162, 36} }; /* Vision Tetra Locations 1 through 8 */

PMWMath made things smaller /******************************************************************************* * FUNCTION NAME: PWMMath * PURPOSE: Limits the mixed value for one joystick drive. * ARGUMENTS: * Argument Type IO Description * -------- ---- -- ----------- * add1 int I positive term * add2 int I positive term * sub1 int I subtractive term * RETURNS: unsigned char *******************************************************************************/ unsigned char PWMMath (unsigned char add1, unsigned char add2, unsigned char sub1) { int limit; limit = (int) add1 + (int) add2 - (int) sub1; if (limit < 0) return 0; if (limit > 255) return 255; return (unsigned char) limit; }

Debugging • PrintString & Printf’ing over the serial port • 11 LED’s on the OI – label them too! • Make a remote reset and program if your FRC is embedded

Structured Output (Programming) • All writes to an output (motor) in one function. • Things will change • output port • direction of motor • damping • bias • deadband • …

PID Control Equations • “P” Proportional – change the output some % of the error from the set point. • “I” Integral – remember the error so you actually get there. • “D” Derivative – dampen the oscillation. • Tuning is the challenge. • See CameraDrive() in our user_routines.c

FIRST Robotics Team 1619 Programming Workshop