pseudocode helper functions: actuation: void rotate(direction) set motors set timer to stop for different angles void drive(direction) set motors void startcirclefollow(direction) start OC interrupt on certain frequency read (AD port) - short distance sensor if sensor value shows we are too close to the wall, steer away at high value else read long distance sensor - change right and left wheel duty cycle based on PID error and direction around the circle. int getWallAngle(void) return current wall angle char getBins(char teamNumber) determine bins based on current wall angle int getBallsInBin(char binNumber) use latest FSR info to determine number of balls in each bin unsigned char sortBins(index) sort the bins into an order based on the number of balls in each bin use latest FSR info return whatever index of the list requested (like highest bin position, etc) unsigned int getTimeLeft(void) update the time using the periodic cycling of the query statemachine event checkers boolean checkLimits(void) check if active state of limit switch pin compare to previous state get time of change to active state then keep checking time until pin has been active for debounce time post event boolean checkcurrent(void) read current (from motor driver board) using AD port check to make sure we are not in reverse (changes threshold of current) check if limit switches are triggered (guard on over current event) check if current value is above a threshold and that it wasnt in the last state post event void beaconchecking(void) if in beaconchecking state check time between rising edges compare this time to known periods of each beacon post an event w/beacon num as param (but only if it's not the same as the last beacon detected) state machines TeamIdentifyFSM state: pregame - do nothing - go to identify state if event is game start - game start comes from FSR - when ball amount goes positive state: identify - if there is a beacon detected then compare it bin frequencies for a given team -- post an event on success - else wait for a beacon FindBallsFSM state: Decide Direction - look at the wall angle and choose a safe direction to go (more area for sweeping before hitting the wall) - start following the circle in that direction - if we pass any beacons, save the bin number - on over current event, start the over current FSM -- on re-entry, use the updated direction (synced with over current FSM saved variables) - stop following after 20 seconds -- change to score ball state machine. ScoreBallsFSM state: decide on bin - pick bin based on highest score (on the first pass) - pick bin based on adjacent to highest score (on second pass) - drive the shortest/safest route to that bin (calculated using wall angle and last beacon seen) - on over current event (while circle following), start the over current FSM -- on re-entry, use the updated direction (synced with over current FSM saved variables) - score the balls, by rotating, reversing, and setting the servo to open - store the orientation (facing inward) OverCurrentFSM (sub find balls and score balls) state: over current response - save current direction - stop, reverse, rotate - reset direction state, start circle following in the opposite dir PushWallFSM state: decide which way to push - pick based on taking over (or cancelling if something is obstructing) the highest scoring bin. - go the direction that is shortest to achieve this - once you get here, reverse, rotate and go to the other side (in case opponent is going to push that way) QueryFSM state: init - write command to SPIDR state: wait for SPIF - upon a response from FSR, wait for 2 milliseconds - then send a null command state: wait for SPIF2 - store this response - wait for 2 milliseconds - send new command - return to first SPIF state MasterFSM: state: init during this state run team identify state machine upon team identify exiting, transition to finding balls state state: finding balls during this state, run finding balls FSM upon finding balls timeout (within that state machine) - go to score balls state on exit, store direction of travel and previous beacon seen state: score balls during this state, run score balls FSM upon successful score balls, store orientation and how many times we have scored if its the first score, go back to finding balls state else if its the second score or if it's nearly the end of the game, go into pushing wall state state: pushing wall during this state, run pushwallFSM run till end of game if (in any state) game time expires, run exit function exitfunction: stop all motors start both LEDs blinking