FIRST Robotics Team 1619 Programming Workshop

1
FIRST RoboticsTeam 1619 Programming Workshop

• Programming the Control System

Mark Dotterweich Team 1619 Mentor
Presentation available at www.coloradofirst.org/k
ickoff
2
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

3
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

4
Joystick Control

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

5
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, pots)
• 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)

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

7
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

8
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

9
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 /

10
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 ltlt 1 rc_dig_in03 ltlt 2
• starting_pos rc_dig_in04 rc_dig_in05 ltlt
  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 /

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.
11
Notes on program speed

• You dont 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
• ltlt, gtgt, and may look strange but they work

12
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

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

14
State Machine Example from 2006 Robot (The
Black Pearl)
Ball shooter (flywheel) Ball release arm

• 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

8 states used for this operation (see next 5
pages)
Ball-store
15
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 (counterREADY_TO_OPEN_DOOR) //
  long enough to back up 2nd ball
• Manual_State STATE_OPENING_DOOR
  // change the state
• counter0 //reset the counter
• break / end STATE_REVERSE_BELTS /

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.
16
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
(counterREADY_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.
17
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
NEUTRAL63 // 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.
18
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.
19
One Shot State Machine Code (5)
case RESET_NEXT_BALL counter 1
BELT_MOTOR BELT_SPEED_LOADING //forward
load if (counterRELOAD_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
counter0 // 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.
20
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.intelitekdow
nloads.com/FRC2006/
21
Backup Reference Slides
22
Additional Getting Started Tips from Mark
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/documenta
  tion.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.

23
Additional Getting Started Tips from Mark
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.

24
Additional Getting Started Tips from Mark
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.

25
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?
  id482
• Microprocessor
• http//www.microchip.com
• http//www.microchip.com/stellent/idcplg?IdcServic
  eSS_GET_PAGEnodeId1335dDocNameen010319
• PID
• http//www.google.com/search?hlenlrqpidcontr
  ol
• Chief Delphi Programming Forum
• http//www.chiefdelphi.com/forums/forumdisplay.php
  ?f51
• C Programming with Robot Builder
• http//www-education.rec.ri.cmu.edu/robot_builder/

26
Use typedefs 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

27
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.

28
ROM and RAM

• Use rom for tables, you dont have the RAM
• rom char abc4 ABC
• And pointers to rom are different.
• rom char ptr
• static rom const unsigned int VT_Location 88
  
• 180, 180, 180, 222, 222, 264, 264, 264,
• 288, 162, 36, 99, 225, 288, 162, 36
• / Vision Tetra Locations 1 through 8 /

29
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 lt 0) return 0
• if (limit gt 255) return 255
• return (unsigned char) limit

30
Debugging

• PrintString Printfing over the serial port
• 11 LEDs on the OI label them too!
• Make a remote reset and program if your FRC is
  embedded

31
Structured Output (Programming)

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

32
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