Battleship

Stanford ME 218b Winter 2013 Project

James Bensson, David Choy, Jason Gonzalez, Craig Western


The Game

Project Task

Create a fully autonomous robot to compete in the game of Battleship.

Full project description

Battleship Game Description

The battleship game consists of two robots facing off in a head-to-head match that lasts no longer than two minutes. After two minutes or when one robot has been hit, the game is over and the robot with the highest number of points is declared the victor. In the case of a tie, a single sudden-death round will determine the winner.

Scoring

To gain points a robot must successfully hit either the opposing robot, the stationary ships on the opposing robot's side, or the power plant on the opposing robot's side with a NERF ball.

Point Values

Note: A robot will lose the game if it hits the opposing robot and has fewer points.

Field

The playing field is shown below and consists of an 8'x8' area with 3.5" outer walls and a 5.5" center dividing wall. The field is equipped with a vision system that charts the position of each robot and stationary ships based on visible markers. This tracking information is made available to each robot via wireless communication. Robots may cover their visible markers and become invisible to the vision system only while they are not actively shooting.

Resupply Depot

At the center of each side of the field lies a NERF ball resupply depot, shown below. Since a robot can only carry a maximum of 5 NERF balls at a time, when a robot is out of balls it may go to the resupply depot to request more. Once at the resupply depot, the robot must send a series of 10 IR LED pulses to the supply depot's IR detector in order for a ball to drop. These pulses must have an on time of 10 ms (+/-0.1ms) and an off time of 30 ms (+/-0.1ms).

Power Plant

Each robot's supply depot is controlled by its corresponding power plant. The power plant is located on the back wall of each side at an unknown y location. When a power plant is hit by a NERF ball, the supply depot stops delivering balls for a 10 second period. To locate the opponent's power supply, a robot must detect the specific IR signal emitted by that power supply's beacon. The beacons emit a 50% duty cycle square wave IR signal at different frequencies. The red side beacon has a period of 20ms, while the blue side beacon has a period of 14 ms.

Stationary Ships

The last components on the field are each robot's stationary ships, shown below. Before the start of the match, each team is given an equal number of stationary ships (between 3 and 5), that they can place anywhere on their side of the field. If a robot runs into one of its stationary ships, the stationary ship marker will fall, and the ship will be considered hit, giving points to the opposing team.


Function Description

Project Specifications

Design Motivation

The motivation for our design came directly out of the strategy that we wanted our robot to implement. The desired strategy was as follows:

  1. Start each battleship match fully loaded with NERF balls at the cross formed by black tape in front of the power supply.
  2. Turn and shoot, quickly and efficiently, at the opposing team's ships.
  3. If ahead by more than two ships, shoot at the other robot.
  4. When out of balls, turn back to the power supply and reload.
  5. When fully reloaded, drive back to the black tape cross and return to step 1.

Design Implementation

In order to accomplish the above strategy we first examined all the robot actions that we deemed necessary and then settled upon corresponding electromechanical devices that were needed to achieve each action.

Movement

Goal: Be able to move to all positions on the field.

Drive Motors: Each team was given 2 drive motors that were used for movement.

Position Detection

Goal: Be able to accurately sense our current position (while visible or invisible to the field) and the position of other structures on the field.

Forward Artillery Controller (FAC) Communication: The field vision system communicated directly with each robot via SPI, giving coordinates of each marked target on the field. However, to eliminate wire noise, buffer circuits were needed to accurately send and receive signals.

Tape Sensors: Although unused in our final strategy, originally they were included in the design with the goal of efficiently traveling up and down the black tape while being invisible to the field.

Drive Motor Encoders: Used in the design to travel specific distances or turn certain angles quickly while being invisible to the field.

IR Photodiode: Used to detect the position of the opposing side's power plant.

Shooting

Goal: Be able to consistently and accurately hit opponent structures.

Shooter Motor: Used to accelerate a NERF ball down an inclined track and launch it at opposing targets.

Encoder on Shooter: Used to perform proportional integral (PI) control on the shooter motor so it could be calibrated for accurate shooting.

Dispenser Servo: Used to hold balls. Designed to turn and drop balls down a ramp to be shot.

Ball Out Coin Sensor: Used to determine when a NERF ball was successfully shot.

Resupplying Balls

Goal: Be able to consistently access more balls from the resupply depot as quickly as possible.

IR LEDs: Used to pulse the resupply depot in order for a ball to drop down.

Red LED: Used to display that the robot was querying for a ball.

Bump Sensor: Used to alert the robot that it had hit the supply depot and could stop its motors.

Ball In Coin Sensor: Used to alert the robot when a ball had successfully entered into its dispenser.

Covering Identification Marker

Goal: To be able to cover robot marker and become invisible to the field at any time.

Marker Cover Servo: Used to turn a piece of paper over the robot marker and become invisible to the field.

Game On Recognition

Goal: To display that the robot was aware of a game on.

Green LED: Designed to turn on during match play.

Robot Power

Goal: To fully power the robot for over 2 minutes.

NiCd Batteries: Used to supply 7.2V of power, which was then sent through various voltage regulators, to successfully power the robot.


Videos

Videos are recorded from the team's perspective; the robot described below is shown in the foreground of the following videos.

Round 1

Bonus Round 2


Schematics

Motor Encoders

Encoder circuits were built using the prepackaged tape sensors (QRB1143) that consist of an IR LED and a phototransistor angled in such a way that the reflectivity of a surface can be measured. The phototransistor was placed within a transresistive circuit in a sourcing configuration with the output of the op-amp tied to the inverting input of a comparator. Hysteresis was built around the comparator in order to reject noise but give clean rising and falling edges as the encoder pattern passed in front of the tape sensor.

Encoders were used on all of the DC motors on our robot in order to close the loop and control the motors. The drive motors were controlled using 48 stripes of alternating black and white wedges forming a circle on the drive wheels. Each edge represented a turn of 7.5 degrees of the wheel. With this pattern, the distance traveled in straight lines or the angles covered when turning could be accurately measured. The shooter motor only had one white stripe, causing a single rising edge on the comparator output per revolution of the shooter flywheel. Using a lower number of encoder stripes was possible because this motor ran in the thousands of RPMs, as compared to tens of RPM of the drive motors.

Drive Motor Drivers

The TLE-5206 H-bridge was chosen to drive the two drive motors in order to take advantage of the MOSFETs on the output stage, reducing the power loss in the H-bridge and delivering the maximum possible voltage to the motors. Initial designs used the L293, but the bi-polar transistors used as the switching element reduced the voltage across the motor by a maximum of 3.6V. With 7.2V batteries, a maximum of 3.6V was available to power the motor. With the TLE-5206, nearly all of the 7.2V could be used to drive the motors, giving a greater speed range.

Motor Characterization

R = 12.5 Ω

L = 3.99 mH

τ = 0.319 ms

Shooter Motor Driver

The shooter motor had a maximum speed of 5200 RPM when run using an L293 with 5V, which, using the worst-case specifications, only had 1.2V at the output. This was still more than enough to shoot balls at the maximum distance allowed of 6', so the choice was made to continue to use the L293. 7.2V was used as the motor supply in order to afford the most flexibility.

Motor Characterization

R = 3.1 Ω

L = 0.85 mH

τ = 0.27 ms

Shooter Calibration

Bump Sensor

A bump sensor circuit was created for the ability to sense when the robot had reached the resupply depot. A limit switch was placed between 5V and a digital input pin on the E128 such that running into the supply depot would cause the pin to go high. A 10K pull-down resistor was placed on the E128 side to keep the pin in a low state when the limit switch is open. With this circuit, when the switch gets activated and the pin went high, the robot knew it is at the resupply depot, and it could turn its drive motors off and begin requesting balls.

Tape Sensor

Tape sensors were originally to be used to aid in tape following, allowing the robot to approach and depart the supply depot quickly and consistently. In order to improve the ability to follow the tape, the same prepackaged tape sensors (QRB1143) were to be used in the same transresistive circuit as was used in the first stage of the motor encoder circuits. Instead of taking the output of the op-amp and tying it to a comparator input, the signal was run into one of the E128 A/D converters, giving the resolution necessary to make small adjustments to the motor speed proportional to the amount the tape sensors had drifted onto the tape. During integration, we found that it was easier to navigate the field without the tape sensors, instead using the robot coordinates from the FAC to determine our position and orientation at any given time. Thus, we kept the tape sensor circuits on the robot itself but they went unused.

Ball Status Sensors

In order to keep track of the number of balls in the robot's possession, ball sensors were used in the same fashion as coin sensors. The circuits looked the same as the motor encoder circuits, except without the prepackaged LED/phototransistor tape sensors. Instead, a pair of IR LED and phototransistor was placed opposite where the ball entered the ball dispenser from the resupply depot and another pair mounted where the ball left the shooter. Thus, if unobstructed, the phototransistor would detect the IR light coming from the LED, causing the output of the comparator to remain high. When a ball broke that beam, the comparator output would go low, allowing the E128 to keep count of the number of balls that had entered and left the robot.

Power Plant Beacon Sensor

In order to detect the pulsing beacon of the power plant, we used a similar setup as the ball status sensor, except without the emitting IR LED. We used a higher gain on the op-amp to increase our sensitivity in picking up the beacon. We also placed a high-pass filter between the op-amp and comparator to account for changes in the background IR levels. This way, the comparator only saw spikes when the signal changed and we were able to remove the DC component of the signal.

SPI Communication

In order to minimize the amount of noise in the sensitive SPI communication lines between the E128 and the FAC, the following buffer circuit was used. Since the signals were travelling across multiple inches, running close to potential capacitive noise sources such as other signal lines, Schmitt trigger buffers were used to reduce the potential impact of those noise sources. The sensitive signals were those that needed to have clean rising and falling edges (CLK, MOSI, MISO). These signals were fed to the inputs of the 74HC14 near the signal source before travelling through lengths of wire toward the final signal receiver. On the receiver end, the signal passed through another 74HC14. Using two Schmitt triggers per line kept the signal with the same polarity, since each Schmitt trigger inverted the signal. As seen in the schematic below, pull down resistors on the input side of the receiver Schmitt trigger caused more current to flow through the lengths of wire between the two buffers. The purpose of these resistors was to ensure that the signal current traversing the multiple inches of wire was orders of magnitude greater than the potential noise currents caused by capacitive noise sources. The resistors were sized such that the signal current would be on the order of milliamps while noise currents would be on the order of microamps. With this buffer circuit, there were no noise issues seen on the sensitive SPI lines.

Power Distribution

Another attempt to reduce noise issues resulted in isolating the power supplies as much as possible between the logic level circuits and the motor circuits. Since the PWM nature of the motor control would result in large currents changing very rapidly, it was decided that it was best to reduce the conductive coupling between the motor circuits and logic circuits. To do so, an individual 7.2V battery was used to power the E128 and the sensor circuits, and another 7.2V battery to power all of the motors. The two battery grounds were connected by a single wire as close to the battery terminals as possible to ensure that reference ground remained the same for both circuits. For logic power, 7.2V were directly fed to the E128, since that board has its own 5V switching regulator. However, for the sensor circuits, an LM2940 provided a regulated 5V line for the sensors' power line. This linear voltage regulator was chosen because it had low-dropout specifications. Other linear regulators had up to 2V of dropout, meaning that for a 5V regulator, if the 7.2V batteries ever drop down to 7V, the regulator will no longer be able to provide a regulated voltage output, causing fluctuations with changing current demands. For the motor power, the two drive motors and the shooter motor were all provided the 7.2V battery voltage directly to the corresponding motor driver board. This was done to provide the most flexibility with the motor control. Sagging battery voltage was not a concern since encoders on these motors would allow for consistent motor control despite changing supply voltage levels. A second LM2940 was used to provide another regulated 5V line for the two servos. Appropriately sized fuses were placed between the two batteries and a toggle switch to provide for adequate circuit protection. The toggle switch allowed for an easy way to cut power to both logic and motor circuits.

Microcontroller Line Distribution

This logic-level distribution board contained sockets for the E128 ribbon cables that connected to the various E128 inputs and outputs. The logic-level 5V regulator was placed on this board.

This schematic shows each Molex connector that was dedicated to carrying power and ground to each sensor circuit and a single signal line back from each sensor circuit to the E128. Each sensor circuit was given its own power and ground to minimize the conductive coupling between sensor circuits in an attempt to preserve the clean rising and falling edges on the encoder and ball sensor circuits. This schematic also shows a graphical representation of the E128 pin assignments.


Drawings

Original Concept Sketch

During design and development of our robotic platform, the following considerations were taken into account. The overall structure was designed to occupy a projected footprint of less than 1 sq. ft. as specified. Standoffs of various lengths were used to form four levels that supported the drive trains and core circuitry; the shooter ramp; the ball dispenser; and the FAC, respectively. The structure was made primarily of Duron with metal fasteners and standoffs, as seen below, with dimensions given in inches.

Front View

Side View

Drive trains were assembled entirely with fasteners in order to maximize reliability. In each drive train, axles ran through three vertical boards mounted using L-brackets and secured by fastening down a press-fit horizontal plate on their top surfaces. This method allowed vertical forces on the drive train to be transferred fully to the lower Duron plate through secure fasteners. Sleeve bearings inserted into the boards allowed smooth rotation of the axle. A shaft collar placed against the outer sleeve bearing prevented unwanted outward movement of the axle, and a spider coupling was used to attach the axle to the motor shaft. Loctite was used in securing the set screws in order to prevent loosening due to motor vibrations. Wheels were fitted with radial patters featuring alternating black-and-white stripes, used to reflect the incoming beam of a tape sensor and form a position encoder on each wheel. Finally, tape sensor mounts for tape following were placed at the front of the device and designed to allow positioning with two translational and one rotational degree of freedom to facilitate calibration.

The shooter and ball storage/dispenser were designed to minimize mechanical complexity and to rely primarily on gravity for ball transfer. From the supply depot, balls entered the robot using a funnel-like structure that guaranteed that any balls successfully entering the device would land in the correct dispenser hole. Multiple holes in the rotary dispenser were used to store extra balls. Clockwise rotation of the dispenser using a servo motor caused balls to fall through a hole and to be redirected onto the downward-sloping part of the shooter ramp. A PI-controlled, DC motor was used as a flywheel acting on the ball from above, giving it topspin and propelling it toward a ramp permanently set at 50° that directed the ball out of the device toward targets. After one iteration of testing, 50° was found to be an acceptable angle for both short-range targets and for shots at the maximum distance of 6 feet. The motor was calibrated early on to optimize height from the ramp, a parameter that was designed to be adjustable, as well as to determine distance-to-RPM mapping.

The top level was designed simply to support the FAC, the IR beacon detector, and the cloak. During the cloaking process, a servo motor rotated an L-shaped structure designed to cover enough of the FAC that the fiducial would be invisible to the vision system.


Pseudo-code

State Chart


Code & Header Listings

Headers

Const.h

/* Header file with constants for pins, etc. */

#include <hidef.h>
#include <mc9s12e128.h>
#include <S12e128bits.h>
#include <Bin_Const.h>

#define FIDUCIAL_COVER_IO DDRT
#define FIDUCIAL_COVER_PORT PTT
#define FIDUCIAL_COVER_BIT BIT0HI

#define WHEEL_ENCODER_LEFT_IO DDRT
#define WHEEL_ENCODER_LEFT_PORT PTT
#define WHEEL_ENCODER_LEFT_BIT BIT4HI

#define WHEEL_ENCODER_RIGHT_IO DDRT
#define WHEEL_ENCODER_RIGHT_PORT PTT
#define WHEEL_ENCODER_RIGHT_BIT BIT5HI

#define WHEEL_ENCODER_SHOOTER_IO DDRT
#define WHEEL_ENCODER_SHOOTER_PORT PTT
#define WHEEL_ENCODER_SHOOTER_BIT BIT6HI

#define WHEEL_PWM_LEFT_CHANNEL 0

#define WHEEL_PWM_RIGHT_CHANNEL 1

#define WHEEL_PWM_SHOOTER_CHANNEL 2

#define FIDUCIAL_SERVO_CHANNEL 0

#define BALL_DISPENSER_SERVO_CHANNEL 1

#define WHEEL_POLARITY_LEFT_IO DDRU
#define WHEEL_POLARITY_LEFT_PORT PTU
#define WHEEL_POLARITY_LEFT_BIT BIT4HI

#define WHEEL_POLARITY_RIGHT_IO DDRU
#define WHEEL_POLARITY_RIGHT_PORT PTU
#define WHEEL_POLARITY_RIGHT_BIT BIT5HI

#define IR_PLANT_SENSOR_IO DDRT
#define IR_PLANT_SENSOR_PORT PTT
#define IR_PLANT_SENSOR_BIT BIT7HI

#define LED_QUERY_BALL_IO DDRS
#define LED_QUERY_BALL_PORT PTS
#define LED_QUERY_BALL_BIT BIT2HI

#define SS_LINE_IO DDRS
#define SS_LINE_PORT PTS
#define SS_LINE_BIT BIT7HI

#define BUTTON_POWER_OFF_IO DDRP
#define BUTTON_POWER_OFF_PORT PTP
#define BUTTON_POWER_OFF_BIT BIT1HI

#define BUMP_SENSOR_IO DDRP
#define BUMP_SENSOR_PORT PTP
#define BUMP_SENSOR_BIT BIT2HI

#define IR_QUERY_BALL_IO DDRS
#define IR_QUERY_BALL_PORT PTS
#define IR_QUERY_BALL_BIT BIT3HI

#define BALL_SHOT_IO DDRP
#define BALL_SHOT_PORT PTP
#define BALL_SHOT_BIT BIT3HI

#define BALL_ENTER_HOPPER_IO DDRP
#define BALL_ENTER_HOPPER_PORT PTP
#define BALL_ENTER_HOPPER_BIT BIT5HI

#define LED_GAME_ON_IO DDRP
#define LED_GAME_ON_PORT PTP
#define LED_GAME_ON_BIT BIT4HI

//#define TAPE_SENSOR_LEFT_IO AD
#define TAPE_SENSOR_LEFT_PORT PTAD
#define TAPE_SENSOR_LEFT_BIT BIT0HI
#define LEFT_TAPE_SENSOR_CHANNEL 0

//#define TAPE_SENSOR_RIGHT_IO AD
#define TAPE_SENSOR_RIGHT_PORT PTAD
#define TAPE_SENSOR_RIGHT_BIT BIT1HI
#define RIGHT_TAPE_SENSOR_CHANNEL 1


//Maximum Number of Balls
#define MAX_BALLS 4

//Ship Scores
#define FLEET_SHIP_VALUE 1000
#define BATTLESHIP_VALUE 1
#define POWER_PLANT_VALUE 100

#define AD_RECHECK_TIME 5 //milliseconds

#define DRIVE_MOTOR_MOVE_POST_PARAM 2
#define FAC_MOTOR_MOVE_POST_PARAM 3
#define TAPE_SENSING_END_REACHED_PARAM 5

#define AT_DEPOT_TIMEOUT 7000 // ms
#define BALL_SHOT_TIMEOUT 1500 // ms

DispenserControl.h

/******************
 Header file for DispenserControl.c
*******************/

#ifndef DispenserControl_H
#define DispenserControl_H

void InitDispenserControl(void);
void MakeSpaceForNewBall(void);
void DropBall(void);

#endif /* DispenserControl_H */

DriveMotorService.h

/******************
 Header file for DriveMotorService
*******************/

#ifndef DriveMotorService_H
#define DriveMotorService_H

#include "ES_Types.h"
#include "ES_Configure.h"
#include "ES_Events.h"

typedef enum
{
    LEFT,
    RIGHT
} Side_t;

typedef enum
{
    CW,
    CCW
} Direction_t;

typedef enum
{
    TURNING_CW,
    TURNING_CCW,
    DRIVING_FORWARD,
    DRIVING_BACKWARD,
    FINDING_POWER_CW,
    FINDING_POWER_CCW,
    MANUALLY_DRIVEN
} Drive_State_t;

typedef enum
{
    DROP_RIGHT_DC,
    DROP_LEFT_DC,
    EQUATE_SPEEDS
} UpdateSpeed_State_t;



//Motor Drive Functions

//Passing in actual duty cycle. If motor A and motor B are both set to the same duty cycle they
// will drive at the same speeds.
void DriveManual(Side_t side, Direction_t direction, int DutyCycle);

void DriveForward(unsigned int inches);
void DriveBackward(unsigned int inches);
void Turn(unsigned int angle, Direction_t direction);
void StopDrive(void);
void StopLeftDrive(void);
void StopRightDrive(void);
void FindEnemyPowerPlant(Direction_t direction); //Starts turning in direction specified until the enemy power plant is detected
Drive_State_t QueryDriveMotorService(void); //Returns the current state of the drive service, if stopped, returns the last state
void InitPowerPlantDetector(void);

boolean InitDriveMotorService (uint8_t Priority);
void IncrementOCOverflow(void);
ES_Event RunDriveMotorService(ES_Event ThisEvent);
boolean PostDriveMotorService(ES_Event ThisEvent);
#endif /* DriveMotorService_H */

ES_Configure.h

/****************************************************************************
 Module
     ES_Configure.h
 Description
     This file contains macro definitions that are edited by the user to
     adapt the Events and Services framework to a particular application.
 Notes

 History
 When           Who     What/Why
 -------------- ---     --------
 02/13/13 09:30 choy      started coding
*****************************************************************************/

#ifndef CONFIGURE_H
#define CONFIGURE_H

/****************************************************************************/
// The maximum number of services sets an upper bound on the number of
// services that the framework will handle. Reasonable values are 8 and 16
// HOWEVER: at this time only a value of 8 is supported.
#define MAX_NUM_SERVICES 8

/****************************************************************************/
// This macro determines that nuber of services that are *actually* used in
// a particular application. It will vary in value from 1 to MAX_NUM_SERVICES
#define NUM_SERVICES 4

/****************************************************************************/
// These are the definitions for Service 0, the lowest priority service
// every Events and Services application must have a Service 0. Further
// services are added in numeric sequence (1,2,3,...) with increasing
// priorities
// the header file with the public fuction prototypes
#define SERV_0_HEADER "DriveMotorService.h"
// the name of the Init function
#define SERV_0_INIT InitDriveMotorService
// the name of the run function
#define SERV_0_RUN RunDriveMotorService
// How big should this services Queue be?
#define SERV_0_QUEUE_SIZE 3

/****************************************************************************/
// The following sections are used to define the parameters for each of the
// services. You only need to fill out as many as the number of services
// defined by NUM_SERVICES
/****************************************************************************/
// These are the definitions for Service 1
#if NUM_SERVICES > 1
// the header file with the public fuction prototypes
#define SERV_1_HEADER "MasterMachine.h"
// the name of the Init function
#define SERV_1_INIT InitMasterMachine
// the name of the run function
#define SERV_1_RUN RunMasterMachine
// How big should this services Queue be?
#define SERV_1_QUEUE_SIZE 3
#endif

/****************************************************************************/
// These are the definitions for Service 2
#if NUM_SERVICES > 2
// the header file with the public fuction prototypes
#define SERV_2_HEADER "KeyMapper.h"
// the name of the Init function
#define SERV_2_INIT InitKeyMapper
// the name of the run function
#define SERV_2_RUN RunKeyMapper
// How big should this services Queue be?
#define SERV_2_QUEUE_SIZE 3
#endif

/****************************************************************************/
// These are the definitions for Service 3
#if NUM_SERVICES > 3
// the header file with the public fuction prototypes
#define SERV_3_HEADER "FACDriveService.h"
// the name of the Init function
#define SERV_3_INIT InitFACDriveService
// the name of the run function
#define SERV_3_RUN RunFACDriveService
// How big should this services Queue be?
#define SERV_3_QUEUE_SIZE 3
#endif

/****************************************************************************/
// These are the definitions for Service 4
#if NUM_SERVICES > 4
// the header file with the public fuction prototypes
#define SERV_4_HEADER "TestService.h"
// the name of the Init function
#define SERV_4_INIT TestServiceInit
// the name of the run function
#define SERV_4_RUN TestServiceRun
// How big should this services Queue be?
#define SERV_4_QUEUE_SIZE 3
#endif

/****************************************************************************/
// These are the definitions for Service 5
#if NUM_SERVICES > 5
// the header file with the public fuction prototypes
#define SERV_5_HEADER "TestService.h"
// the name of the Init function
#define SERV_5_INIT TestServiceInit
// the name of the run function
#define SERV_5_RUN TestServiceRun
// How big should this services Queue be?
#define SERV_5_QUEUE_SIZE 3
#endif

/****************************************************************************/
// These are the definitions for Service 6
#if NUM_SERVICES > 6
// the header file with the public fuction prototypes
#define SERV_6_HEADER "TestService.h"
// the name of the Init function
#define SERV_6_INIT TestServiceInit
// the name of the run function
#define SERV_6_RUN TestServiceRun
// How big should this services Queue be?
#define SERV_6_QUEUE_SIZE 3
#endif

/****************************************************************************/
// These are the definitions for Service 7
#if NUM_SERVICES > 7
// the header file with the public fuction prototypes
#define SERV_7_HEADER "TestService.h"
// the name of the Init function
#define SERV_7_INIT TestServiceInit
// the name of the run function
#define SERV_7_RUN TestServiceRun
// How big should this services Queue be?
#define SERV_7_QUEUE_SIZE 3
#endif

/****************************************************************************/
// the name of the posting function that you want executed when a new
// keystroke is detected.
// The default initialization distributes keystrokes to all state machines
#define POST_KEY_FUNC ES_PostAll

/****************************************************************************/
// Name/define the events of interest
// Universal events occupy the lowest entries, followed by user-defined events
typedef enum {  ES_NO_EVENT = 0,
                ES_ERROR,  /* used to indicate an error from the service */
                ES_INIT,   /* used to transition from initial pseudo-state */
                ES_NEW_KEY, /* signals a new key received from terminal */
                ES_TIMEOUT, /* signals that the timer has expired */
                ES_ENTRY, /* standard entry into state */
                ES_ENTRY_HISTORY, /* entry into state with history */
                ES_EXIT, /* exit out of state */

                /* User-defined events start here */
                EV_GAME_START, // 8
                EV_GAME_OVER,
                EV_BALL_DROPPED,
                EV_MOVE_COMPLETE,
                EV_BALL_SHOT,
                EV_BUMP_DETECTED,
                EV_OUT_OF_BALLS,
                EV_MAX_BALLS_ACQUIRED, // 15
                EV_IN_POSITION_TO_SHOOT,
                EV_POWER_PLANT_DETECTED,
                EV_OUTPUT_COMPARE,
                EV_CLOSE_TO_DEPOT,
                EV_CHANGE_FAC_DRIVE_STATE
             } ES_EventTyp_t ;

/****************************************************************************/
// These are the definitions for the Distribution lists. Each definition
// should be a comma seperated list of post functions to indicate which
// services are on that distribution list.
#define NUM_DIST_LISTS 0
#if NUM_DIST_LISTS > 0
#define DIST_LIST0 PostTemplateFSM
#endif
#if NUM_DIST_LISTS > 1
#define DIST_LIST1 PostTemplateFSM
#endif
#if NUM_DIST_LISTS > 2
#define DIST_LIST2 PostTemplateFSM
#endif
#if NUM_DIST_LISTS > 3
#define DIST_LIST3 PostTemplateFSM
#endif
#if NUM_DIST_LISTS > 4
#define DIST_LIST4 PostTemplateFSM
#endif
#if NUM_DIST_LISTS > 5
#define DIST_LIST5 PostTemplateFSM
#endif
#if NUM_DIST_LISTS > 6
#define DIST_LIST6 PostTemplateFSM
#endif
#if NUM_DIST_LISTS > 7
#define DIST_LIST7 PostTemplateFSM
#endif

/****************************************************************************/
// This are the name of the Event checking funcion header file.
#define EVENT_CHECK_HEADER "EventCheckers.h"

/****************************************************************************/
// This is the list of event checking functions
#define EVENT_CHECK_LIST CheckBallIn, CheckBallOut, CheckBumpSensor

/****************************************************************************/
// These are the definitions for the post functions to be executed when the
// correspnding timer expires. All 8 must be defined. If you are not using
// a timers, then you can use TIMER_UNUSED
#define TIMER_UNUSED ((pPostFunc)0)
#define TIMER0_RESP_FUNC PostMasterMachine
#define TIMER1_RESP_FUNC PostMasterMachine
#define TIMER2_RESP_FUNC PostMasterMachine
#define TIMER3_RESP_FUNC PostMasterMachine
#define TIMER4_RESP_FUNC PostKeyMapper
#define TIMER5_RESP_FUNC PostFACDriveService
#define TIMER6_RESP_FUNC PostMasterMachine
#define TIMER7_RESP_FUNC PostMasterMachine

/****************************************************************************/
// Give the timer numbers symbolc names to make it easier to move them
// to different timers if the need arises. Keep these definitons close to the
// definitions for the response functions to make it easier to check that
// the timer number matches where the timer event will be routed

#define FAC_TIMER 0
#define DRIVE_AD_TIMER 1
#define BALL_TIMER 2
#define GAME_TIMER 3
#define KEY_MAP_TIMER 4
#define FAC_DRIVE_TIMER 5
#define AT_DEPOT_TIMER 6
#define BALL_SHOT_TIMER 7

#endif /* CONFIGURE_H */

EventCheckers.h

// prototypes for test event checkers

boolean CheckBallIn(void);
boolean CheckBallOut(void);
boolean CheckBumpSensor(void);

#include <bitdefs.h>

// #defines that go with the Event Checkers

FACDriveService.h

/****************************************************************************

  Header file for FACDriveService.c

 ****************************************************************************/

#ifndef FACDriveService_H
#define FACDriveService_H

// Event Definitions
#include "ES_Configure.h"
#include "ES_Types.h"

// typedefs for the states
// State definitions for use with the query function
typedef enum
{
    FAC_Drive_Off,
    DriveForward_Calibrate,
    DriveForward_Turn,
    DriveForward_Drive,
    DriveBackward_Calibrate,
    DriveBackward_Turn,
    DriveBackward_Drive,
    Turn_Calibrate,
    Turn_Drive
} FACDriveState_t ;


// Public Function Prototypes

boolean InitFACDriveService ( uint8_t Priority );
boolean PostFACDriveService( ES_Event ThisEvent );
ES_Event RunFACDriveService( ES_Event ThisEvent );
FACDriveState_t QueryTemplateSM ( void );


void FAC_DriveStop(void);
void FAC_Turn(float x, float y);             //Turns the robot towards (x,y)
void FAC_DriveForward(float x, float y);     //Has the robot drive forward to (x,y)
void FAC_DriveBackward(float x, float y);    //Has the robot drive backward to (x,y)


#endif /* FACDriveService_H */

FiducialCover.h

/******************
 Header file for FiducialCover.c
*******************/

#ifndef FiducialCover_H
#define FiducialCover_H

void InitFiducialCover(void);
void CoverFiducial(void);
void UncoverFiducial(void);

#endif /* FiducialCover_H */

FiducialVisibility.h

/****************************************************************************
 Header file for Fiducial Visibility
 ****************************************************************************/

#ifndef FiducialVisibility_H
#define FiducialVisibility_H


// typedefs for the states
// State definitions for use with the query function
typedef enum
{
    VISIBLE,
    INVISIBLE
} FiducialVisibilityState_t ;


// Public Function Prototypes

ES_Event RunFiducialVisibility( ES_Event CurrentEvent );
void StartFiducialVisibility ( ES_Event CurrentEvent );
FiducialVisibilityState_t QueryFiducialVisibility ( void );

#endif /* FiducialVisibility_H */

GameMaster.h

/****************************************************************************
 Template header file for Game master machine
 ****************************************************************************/

#ifndef GameMaster_H
#define GameMaster_H


// typedefs for the states
// State definitions for use with the query function
typedef enum
{
    WAITING,
    PLAYING
} GameMasterState_t ;


// Public Function Prototypes

ES_Event RunGameMaster( ES_Event CurrentEvent );
void StartGameMaster ( ES_Event CurrentEvent );
GameMasterState_t QueryGameMaster ( void );

#endif /* GameMaster_H */

Gameplay.h

/****************************************************************************
 Header file for Gameplay (strategy)
 ****************************************************************************/

#ifndef Gameplay_H
#define Gameplay_H


// typedefs for the states
// State definitions for use with the query function
typedef enum
{
    SHOOTING,
    RETRIEVING_BALLS,
    DRIVING_FORWARD_GAMEPLAY,
    DROPPING_EXTRA_BALLS,
    MOVING_TO_SHOOT,
    MOVING_TO_DEPOT
} GameplayState_t ;


// Public Function Prototypes

ES_Event RunGameplay( ES_Event CurrentEvent );
void StartGameplay ( ES_Event CurrentEvent );
GameplayState_t QueryGameplay ( void );

#endif /* Gameplay_H */

KeyMapper.h

/****************************************************************************
 Header file for Keyboard mapper to events

 ****************************************************************************/

#ifndef KeyMapper_H
#define KeyMapper_H

// Public Function Prototypes

boolean InitKeyMapper ( uint8_t Priority );
boolean PostKeyMapper( ES_Event ThisEvent );
ES_Event RunKeyMapper( ES_Event ThisEvent );

#endif /* KeyMapper_H */

MasterMachine.h

/****************************************************************************
 Template header file for Hierarchical Sate Machines AKA StateCharts

 ****************************************************************************/

#ifndef MasterMachine_H
#define MasterMachine_H

// State definitions for use with the query function
typedef enum
{
    RUNNING
} MasterMachineState_t ;

// Public Function Prototypes

ES_Event RunMasterMachine(ES_Event CurrentEvent);
void StartMasterMachine(ES_Event CurrentEvent);
boolean PostMasterMachine(ES_Event ThisEvent);
boolean InitMasterMachine(uint8_t Priority);


#endif /* MasterMachine_H */

MovingToDepot.h

/****************************************************************************
 Header file for Moving to Depot state machine
 ****************************************************************************/

#ifndef MovingToDepot_H
#define MovingToDepot_H

// typedefs for the states
// State definitions for use with the query function
typedef enum
{
    TURNING_TOWARDS_CENTER_TAPE,
    MOVING_TOWARDS_CENTER_TAPE,
    TURNING_TOWARDS_DEPOT,
    MOVING_BACKWARD,
    TURNING_TOWARDS_TAPE,
    TAPE_SENSING,
    TURNING_TO_DEPOT,
    BACKING_TO_DEPOT
} MovingToDepotState_t ;


// Public Function Prototypes

ES_Event RunMovingToDepot( ES_Event CurrentEvent );
void StartMovingToDepot ( ES_Event CurrentEvent );
MovingToDepotState_t QueryMovingToDepot ( void );

#endif /* MovingToDepot_H */

PositionLib.h

/******************
 Header file for Position Library
*******************/

#ifndef PositionLib_H
#define PositionLib_H

signed int CalculateAngle(float x_self, float y_self, float x_other, float y_other);
signed int CalculateDeltaAngle(float x_self, float y_self, unsigned int angle_self, float x_other, float y_other);
float CalculateDistance(float x_self, float y_self, float x_other, float y_other);
float CalculatePowerPlantDistance(float x_self, float y_self, unsigned int angle_self);


#endif /* PositionLib_H */

PWM.h

/******************
 Header file for PWM library
*******************/

#ifndef PWM_H
#define PWM_H

#define MAX_CHANNELS 6

typedef enum
{
    PWM_OK = 0,
    PWM_ERROR
} PWM_Status_t;

typedef enum
{
    PWM_POLARITY_LOW = 0,
    PWM_POLARITY_HIGH,
    PWM_POLARITY_TOGGLE
} PWM_Polarity_t;


PWM_Status_t PWM_Init(unsigned char Channel);
PWM_Status_t PWM_Set_Frequency(unsigned char Channel, unsigned long Frequency);
PWM_Status_t PWM_Set_Duty_Cycle(unsigned char Channel, unsigned char DC);
PWM_Status_t PWM_Set_Polarity(unsigned char Channel, PWM_Polarity_t Polarity);
PWM_Status_t PWM_Release(unsigned char Channel);


#endif /* PWM_H */

QueryFAC.h

/******************
 Header file for Querying the FAC
*******************/

#ifndef QueryFAC_H
#define QueryFAC_H

// Typedef for a point in space
typedef struct
{
    float x;
    float y;
    signed int angle;
} Point;

// State definitions for use with the query function
typedef enum
{
    FAC_WAITING,
    FAC_QUERYING
} QueryFAC_State_t;

// Typedefs for the colors of a team
typedef enum
{
    TEAM_COLOR_NONE = 0,
    TEAM_COLOR_RED,
    TEAM_COLOR_BLUE
} Robot_Color_t;

// Time to wait between asking FAC to query all states in (1.024 ms) ticks
static const unsigned int TimeToWaitBetweenQuerying = 3;

// Our team/robot number
static const unsigned char Self = 4;

// Public Function Prototypes

ES_Event RunQueryFAC(ES_Event CurrentEvent);
void StartQueryFAC(ES_Event CurrentEvent);
QueryFAC_State_t QueryQueryFAC(void);

void ResetData(void);
unsigned char IsGameOn(void);
unsigned char GetOpponentNumber(void);
Robot_Color_t GetColor(unsigned char TeamNumber);
signed int GetScoreDifferential(void); // Returns number of points we are leading by
unsigned char GetNearestOpponentShip(void); // Calculates closest opponent ship based on y coordinate
unsigned long GetTimestamp(void); // Get "time" of newest data received

/*
 * Coordinates remapped to standard math coordinates
 *     (0,0) at bottom left
 *     0 Degrees mapped to pointing right
 *
 * Get- functions returns value in inches/degrees
 * Get-Raw functions returns value in ticks (as reported from FAC)
 * GetLastKnown- functions returns last seen/known value in inches/degrees
 *     Useful for querying robot values in case of being cloaked
 * GetLastKnown-Raw functions returns last seen/known value in ticks (as reported from FAC)
 */

float GetX(unsigned char ShipNumber);
unsigned char GetXRaw(unsigned char ShipNumber);
float GetLastKnownX(unsigned char TeamNumber);
unsigned char GetLastKnownXRaw(unsigned char TeamNumber);
float GetY(unsigned char ShipNumber);
unsigned char GetYRaw(unsigned char ShipNumber);
float GetLastKnownY(unsigned char TeamNumber);
unsigned char GetLastKnownYRaw(unsigned char TeamNumber);
unsigned int GetRotation(unsigned char ShipNumber);
unsigned char GetRotationRaw(unsigned char ShipNumber);
unsigned int GetLastKnownRotation(unsigned char TeamNumber);
unsigned char GetLastKnownRotationRaw(unsigned char TeamNumber);

#endif /* QueryFAC_H */

RetrievingBalls.h

/****************************************************************************
 Header file for Retrieving Balls state machine
 ****************************************************************************/

#ifndef RetrievingBalls_H
#define RetrievingBalls_H


// typedefs for the states
// State definitions for use with the query function
typedef enum
{
    PULSING,
    WAITING_FOR_BALL
} RetrievingBallsState_t ;


// Public Function Prototypes

ES_Event RunRetrievingBalls( ES_Event CurrentEvent );
void StartRetrievingBalls ( ES_Event CurrentEvent );
RetrievingBallsState_t QueryRetrievingBalls ( void );

unsigned char GetNumBallsLoaded(void);

#endif /* RetrievingBalls_H */

Servo.h

/******************
 Header file for Servo library
*******************/

#ifndef Servo_H
#define Servo_H

#define SERVO_MAX_CHANNELS 8

typedef enum
{
    SERVO_OK = 0,
    SERVO_ERROR
} Servo_Status_t;


Servo_Status_t Servo_Init(void);
Servo_Status_t Servo_Enable(unsigned char Channel);
Servo_Status_t Servo_Set_Pulse_Width(unsigned char Channel, unsigned int Width);
Servo_Status_t Servo_Release(unsigned char Channel);


#endif /* Servo_H */

ShooterMotorService.h

/******************
 Header file for ShooterMotorService
*******************/

#ifndef ShooterMotorService_H
#define ShooterMotorService_H



void InitShooterMotorService (void);
void SetShooterDistance( float Distance);
void TurnShooterOn(void);
void TurnShooterOff(void);

void IncrementShooterOCOverflow(void);
void IncrementShooterEncoderOverflow(void);



#endif /* ShooterMotorService_H */

Shooting.h

/****************************************************************************
 Header file for Shooting state machine
 ****************************************************************************/

#ifndef Shooting_H
#define Shooting_H


// typedefs for the states
// State definitions for use with the query function
typedef enum
{
    ALIGNING,
    WAITING_FOR_DISPENSER
} ShootingState_t ;


// Public Function Prototypes

ES_Event RunShooting( ES_Event CurrentEvent );
void StartShooting ( ES_Event CurrentEvent );
ShootingState_t QueryShooting( void );

unsigned char GetNumBallsLeft(void);

#endif /* Shooting_H */

ShootingStrategy.h

/******************
 Header file for ShootingStrategy.c
*******************/

#ifndef ShootingStrategy_H
#define ShootingStrategy_H

void ExecuteShootingStrategy(void);

#endif /* ShootingStrategy_H */

TapeSensors.h

/******************
 Header file for TapeSensors.c
*******************/

#ifndef TapeSensors_H
#define TapeSensors_H

void InitTapeSensors(void);
void MoveForwardOnTape(void);
void MoveBackwardOnTape(void);
boolean AtFrontLine(void);
boolean NearDepot(void);

#endif /* FiducialCover_H */

Sources

DispenserControl.c

/****************************************************************************
 Module
   DispenserControl.c

 Description
   This Module implements the the control for the ball dispenser
****************************************************************************/
/*----------------------------- Include Files -----------------------------*/
#include "ES_Configure.h"
#include "ES_Framework.h"
#include "Const.h"
#include "PWM.h"
#include <stdio.h>
#include "Servo.h"


#include "DispenserControl.h"

/*----------------------------- Module Defines ----------------------------*/
/*---------------------------- Module Variables ---------------------------*/
//Initially Starting with max balls
static signed char numBalls = MAX_BALLS;
static unsigned int position[] = {730, 1080, 1480, 1900, 2320};


void InitDispenserControl(void)
{
    Servo_Init();
    Servo_Enable(BALL_DISPENSER_SERVO_CHANNEL);
    Servo_Set_Pulse_Width(BALL_DISPENSER_SERVO_CHANNEL, position[numBalls]);
}


void MakeSpaceForNewBall(void)
{
    numBalls++;
    if (numBalls > MAX_BALLS)
    {
        numBalls = MAX_BALLS;
    }
    Servo_Set_Pulse_Width(BALL_DISPENSER_SERVO_CHANNEL, position[numBalls]);
}


void DropBall(void)
{
    numBalls--;
    if (numBalls < 0)
    {
        numBalls = 0;
    }
    Servo_Set_Pulse_Width(BALL_DISPENSER_SERVO_CHANNEL, position[numBalls]);
}

DriveMotorService.c

/****************************************************************************
 Module
   DriveMotorService.c

****************************************************************************/
/*----------------------------- Include Files -----------------------------*/

#include "ES_Configure.h"
#include "ES_Framework.h"
#include "ES_Events.h"
#include <hidef.h>
#include <mc9s12e128.h>
#include <S12e128bits.h>
#include <Bin_Const.h>
#include "S12eVec.h"
#include "Const.h"
#include "PWM.h"
#include <stdio.h>
#include <math.h>
#include "PositionLib.h"
#include "QueryFAC.h"
#include "DriveMotorService.h"
#include "ShooterMotorService.h"
#include "MasterMachine.h"
#include "FACDriveService.h"
/*----------------------------- Module Defines ----------------------------*/


//Motor Wheel Constants
#define ENCODER_TICKS_PER_REV 24
#define WHEEL_DIAMETER        3     //inches
#define AXIS_WHEEL_RADIUS     4.6325   //inches
#define PI                    3.1415926

//Input Capture Constants
#define MS_IC                 1500

//PWM Defines
#define DRIVE_PWM_FREQ        5000

//Note: CW and CCW refer to the robot body, not necessarily the particular wheel
//These refer to the percentage of drive that is desired
#define TURN_CW_PERCENTAGE          100
#define TURN_CCW_PERCENTAGE         100
#define FORWARD_PERCENTAGE          100
#define BACKWARD_PERCENTAGE         80
#define FIND_POWER_PERCENTAGE 		100

//How much faster L wheel is than R wheel
// If LR_DC_DIFF = 10, L_WHEEL_DC = 80, R_WHEEL_DC = 90, drives straight
#define LR_DC_DIFFERENTIAL	 1

//How many ticks one encoder is allowed to get above the other before
// adjustments are made
#define ALLOWABLE_TICK_DIFFERENTIAL 1
//How much duty cycle should drop when one motor is going faster than the other
#define DC_DROP_FOR_POSITION_CONTROL	1


//Power Plant Constants
#define RED_POWER_PERIOD      20 //milliseconds
#define BLUE_POWER_PERIOD     14 //milliseconds
#define PLUS_MINUS            3



/*---------------------------- Module Functions ---------------------------*/
void interrupt _Vec_tim1ch5 RightEncoderResponseInterrupt(void);
void interrupt _Vec_tim1ch4 LeftEncoderResponseInterrupt(void);
void interrupt _Vec_tim1ovf EncoderOverflowResponseInterrupt(void);
void interrupt _Vec_tim1ch7 PowerPlantResponseInterrupt(void);
void DriveManual_Internal( Side_t side, Direction_t direction, int DutyCycle, boolean CalledFromOutsideModule);

void InitEncoderTimer(void);
void InitRightMotor(void);
void InitLeftMotor(void);


/*---------------------------- Module Variables ---------------------------*/
static uint8_t MyPriority;


//Left Motor Control
static int tickCount_L = 0;
static int desiredTicks_L = 0;
static int DutyCycle_L;

//Right Motor Control
static int tickCount_R = 0;
static int desiredTicks_R = 0;
static int DutyCycle_R;

//Encoder Overflow
static unsigned int numOverFlowsEncoders = 0;

//Power Plant Detection
static int EnemyPowerPeriod;

//Motor Drive State
Drive_State_t CurrentState;

static unsigned long lastPeriod_L;
static unsigned long lastPeriod_R;

static unsigned char MoveCompleteEventSent = 0;
static unsigned char PowerPlantFoundEventSent = 0;


/*------------------------------ Module Code ------------------------------*/
/****************************************************************************
 Function
     InitDriveMotorService

 Parameters
     uint8_t : the priorty of this service

 Returns
     boolean, False if error in initialization, True otherwise

 Description
     Saves away the priority.
     Initializes the output compare system for PI control
     Initializes input capture for encoder
     Initializes PWM drive on motor
****************************************************************************/
boolean InitDriveMotorService(uint8_t Priority)
{

    ES_Event ThisEvent;
    MyPriority = Priority;

    InitEncoderTimer();
    InitRightMotor();
    InitLeftMotor();

    CurrentState = MANUALLY_DRIVEN;
    StopDrive();

    ThisEvent.EventType = ES_INIT;
    if (ES_PostToService( MyPriority, ThisEvent) == True)
    {
        return True;
    }
    else
    {
        return False;
    }


}
/****************************************************************************
 Function
      DriveManual

 Parameters
      Side_t side
      Direction_t direction
      int RPM

 Returns
      None

 Description
      Changes the polarity and duty cycle of each motor depending on the input
      specifications.
****************************************************************************/
void DriveManual(Side_t side, Direction_t direction, int DutyCycle)
{

    if (side == LEFT)
    {
        DutyCycle -= LR_DC_DIFFERENTIAL;
    }
    if (DutyCycle < 0)
    {
        DutyCycle = 0;
    }
    if (DutyCycle > 100)
    {
        DutyCycle = 100;
    }

    DriveManual_Internal(side, direction, DutyCycle, True);
}

/****************************************************************************
 Function
      Turn

 Parameters
      int angle
      Direction_t direction

 Returns
      None

 Description
      Has the motor turn a specified angle
****************************************************************************/
void Turn(unsigned int angle, Direction_t direction)
{

    //desiredTicks = (# revs) * (ticks per rev)
    // # revs = (arc length) / (PI * WheelDiameter)
    // arc length = angle (in rad) * (distance from wheel to center of axis)
    DisableInterrupts;
    desiredTicks_L = (int)(((float)angle * PI / 180) * AXIS_WHEEL_RADIUS / (PI * WHEEL_DIAMETER) * ENCODER_TICKS_PER_REV + 0.5);
    desiredTicks_R = desiredTicks_L;

    TIM1_TFLG1 =  _S12_C4F;
    TIM1_TFLG1 =  _S12_C5F;

    EnableInterrupts;

    if (desiredTicks_L == 0)
    {
        ES_Event ThisEvent;
        ThisEvent.EventType = EV_MOVE_COMPLETE;
        PostMasterMachine(ThisEvent);
        StopDrive();
        tickCount_L = 0;
        desiredTicks_L = 0;
        return;
    }

    tickCount_L = 0;
    tickCount_R = 0;

    MoveCompleteEventSent = 0;

    switch(direction)
    {

        case CW:
            CurrentState = TURNING_CW;
            DutyCycle_L = TURN_CW_PERCENTAGE - LR_DC_DIFFERENTIAL;
            DutyCycle_R = TURN_CW_PERCENTAGE;
            DriveManual_Internal(LEFT, CW, DutyCycle_L, False);
            DriveManual_Internal(RIGHT, CCW, DutyCycle_R, False);
            break;

        case CCW:
            CurrentState = TURNING_CCW;
            DutyCycle_L = TURN_CCW_PERCENTAGE - LR_DC_DIFFERENTIAL;
            DutyCycle_R = TURN_CCW_PERCENTAGE;
            DriveManual_Internal(LEFT, CCW, DutyCycle_L, False);
            DriveManual_Internal(RIGHT, CW, DutyCycle_R, False);
            break;
    }
}


/****************************************************************************
 Function
      DriveForward

 Parameters
      int inches

 Returns
      None

 Description
      Has the motor drive forward the specified number of inches
****************************************************************************/
void DriveForward(unsigned int inches)
{
    CurrentState = DRIVING_FORWARD;

    //desiredTicks = (# revs) * (ticks per rev)
    // # revs = (inches) / (PI * WheelDiameter)
    DisableInterrupts;
    desiredTicks_L = (int)(((float)inches) / (PI * WHEEL_DIAMETER) * ENCODER_TICKS_PER_REV + 0.5);
    desiredTicks_R = desiredTicks_L;

    TIM1_TFLG1 =  _S12_C4F;
    TIM1_TFLG1 =  _S12_C5F;

    EnableInterrupts;

    if (desiredTicks_L == 0)
    {
        ES_Event ThisEvent;
        ThisEvent.EventType = EV_MOVE_COMPLETE;
        ThisEvent.EventParam = DRIVE_MOTOR_MOVE_POST_PARAM;
        PostMasterMachine(ThisEvent);
        PostFACDriveService(ThisEvent);
        StopDrive();
        tickCount_L = 0;
        desiredTicks_L = 0;
        return;
    }

    tickCount_L = 0;
    tickCount_R = 0;

    MoveCompleteEventSent = 0;

    DutyCycle_L = FORWARD_PERCENTAGE - LR_DC_DIFFERENTIAL;
    DutyCycle_R = FORWARD_PERCENTAGE;
    DriveManual_Internal(LEFT, CW, DutyCycle_L, False);
    DriveManual_Internal(RIGHT, CW, DutyCycle_R, False);
}



/****************************************************************************
 Function
      DriveBackward

 Parameters
      int inches

 Returns
      None

 Description
      Has the motor drive forward the specified number of inches
****************************************************************************/
void DriveBackward(unsigned int inches)
{

    CurrentState = DRIVING_BACKWARD;

    //desiredTicks = (# revs) * (ticks per rev)
    // # revs = (inches) / (PI * WheelDiameter)
    DisableInterrupts;
    desiredTicks_L = (int)(((float)inches) / (PI * WHEEL_DIAMETER) * ENCODER_TICKS_PER_REV + 0.5);
    desiredTicks_R = desiredTicks_L;

    TIM1_TFLG1 =  _S12_C4F;
    TIM1_TFLG1 =  _S12_C5F;

    EnableInterrupts;

    if (desiredTicks_L == 0)
    {
        ES_Event ThisEvent;
        ThisEvent.EventType = EV_MOVE_COMPLETE;
        ThisEvent.EventParam = DRIVE_MOTOR_MOVE_POST_PARAM;
        PostMasterMachine(ThisEvent);
        PostFACDriveService(ThisEvent);
        StopDrive();
        tickCount_L = 0;
        desiredTicks_L = 0;
        return;
    }

    tickCount_L = 0;
    tickCount_R = 0;
    MoveCompleteEventSent = 0;

    DutyCycle_L = BACKWARD_PERCENTAGE - LR_DC_DIFFERENTIAL;
    DutyCycle_R = BACKWARD_PERCENTAGE;
    DriveManual_Internal(LEFT, CCW, DutyCycle_L, False);
    DriveManual_Internal(RIGHT, CCW, DutyCycle_R, False);
}


/****************************************************************************
 Function
      StopDrive

 Parameters
      None

 Returns
      None

 Description
      Stops both motors
****************************************************************************/
void StopDrive(void)
{
    DriveManual_Internal( LEFT, CW, 0, False);
    DriveManual_Internal( RIGHT, CW, 0, False);
}



/****************************************************************************
 Function
      FindEnemyPowerPlant

 Parameters
      Direction_t direction

 Returns
      None

 Description
      Turns the robot in the specified direction until the power plant is detected
****************************************************************************/
void FindEnemyPowerPlant(Direction_t direction)
{
    PowerPlantFoundEventSent = 0;

    DutyCycle_L = FIND_POWER_PERCENTAGE - LR_DC_DIFFERENTIAL;
    DutyCycle_R = FIND_POWER_PERCENTAGE;

    switch(direction)
    {
        case CW:
            CurrentState = FINDING_POWER_CW;
            DriveManual_Internal(LEFT, CW, DutyCycle_L, False);
            DriveManual_Internal(RIGHT, CCW, DutyCycle_R, False);
            break;

        case CCW:
            CurrentState = FINDING_POWER_CCW;
            DriveManual_Internal(LEFT, CCW, DutyCycle_L, False);
            DriveManual_Internal(RIGHT, CW, DutyCycle_R, False);
            break;
    }
}


/****************************************************************************
 Function
     PostDriveMotorService

 Parameters
     EF_Event ThisEvent ,the event to post to the queue

 Returns
     boolean False if the Enqueue operation failed, True otherwise

 Description
     Posts an event to this state machine's queue
****************************************************************************/
boolean PostDriveMotorService( ES_Event ThisEvent )
{
    return ES_PostToService( MyPriority, ThisEvent);
}


/****************************************************************************
 Function
    RunDriveMotorService

 Parameters
   ES_Event : the event to process

 Returns
   ES_Event, ES_NO_EVENT if no error ES_ERROR otherwise

 Description
   If the
****************************************************************************/
ES_Event RunDriveMotorService( ES_Event ThisEvent )
{
    ES_Event ReturnEvent;
    ReturnEvent.EventType = ES_NO_EVENT; // assume no errors

    if (CurrentState == FINDING_POWER_CW || CurrentState == FINDING_POWER_CCW)
    {
        if (ThisEvent.EventType == EV_POWER_PLANT_DETECTED && !PowerPlantFoundEventSent)
        {
            ES_Event ThisEvent;
            ThisEvent.EventType = EV_MOVE_COMPLETE;
            PostMasterMachine(ThisEvent);
            StopDrive();
            PowerPlantFoundEventSent = 1;
        }
    }


    return ReturnEvent;
}


/****************************************************************************
 Function
    QueryDriveMotorService

 Parameters
   None

 Returns
   Drive_State_t

 Description
   Returns the currentState of the DriveMotorService. If the motor is stopped
   the last moving state is returned.
****************************************************************************/
Drive_State_t QueryDriveMotorService(void)
{
    return CurrentState;
}



/*--------------------------PRIVATE FUNCTIONS-------------------------------*/
/****************************************************************************
 Function
      DriveManual_Internal

 Parameters
      Side_t side
      Direction_t direction
      int RPM

 Returns
      None

 Description
      Changes the polarity and duty cycle of each motor depending on the input
      specifications.
****************************************************************************/
void DriveManual_Internal( Side_t side, Direction_t direction, int DutyCycle, boolean CalledFromOutsideModule)
{
    if (CalledFromOutsideModule)
    {
        CurrentState = MANUALLY_DRIVEN;
        tickCount_L = 0;
        tickCount_R = 0;
    }

    switch(side)
    {
        case LEFT:
            PWM_Set_Duty_Cycle(WHEEL_PWM_LEFT_CHANNEL, (unsigned char)DutyCycle);
            switch(direction)
            {
                case CW:
                    PWM_Set_Polarity(WHEEL_PWM_LEFT_CHANNEL, PWM_POLARITY_LOW);
                    WHEEL_POLARITY_LEFT_PORT  |= WHEEL_POLARITY_LEFT_BIT;
                    break;
                case CCW:
                    PWM_Set_Polarity(WHEEL_PWM_LEFT_CHANNEL, PWM_POLARITY_HIGH);
                    WHEEL_POLARITY_LEFT_PORT  &= ~WHEEL_POLARITY_LEFT_BIT;
                    break;
            }
            break;

        case RIGHT:
            PWM_Set_Duty_Cycle(WHEEL_PWM_RIGHT_CHANNEL, (unsigned char)DutyCycle);
            switch(direction)
            {
                case CW:
                    PWM_Set_Polarity(WHEEL_PWM_RIGHT_CHANNEL, PWM_POLARITY_LOW);
                    WHEEL_POLARITY_RIGHT_PORT  |= WHEEL_POLARITY_RIGHT_BIT;
                    break;
                case CCW:
                    PWM_Set_Polarity(WHEEL_PWM_RIGHT_CHANNEL, PWM_POLARITY_HIGH);
                    WHEEL_POLARITY_RIGHT_PORT  &= ~WHEEL_POLARITY_RIGHT_BIT;
                    break;
            }
            break;
    }
}






/****************************************************************************
 Function
   LeftEncoderResponseInterrupt

 Parameters
   None

 Returns
   None

 Description
   Interrupt Function: Not to be called by code
   Calculates the last period between left encoder ticks
****************************************************************************/
void interrupt _Vec_tim1ch4 LeftEncoderResponseInterrupt(void)
{

    static unsigned long currentTime_L;
    static unsigned long lastTime_L = 0;

    TIM1_TFLG1 =  _S12_C4F;   //Clear Flag

    if (TIM1_TFLG2 & _S12_TOF == _S12_TOF && TIM1_TC4 < 0x8000)
    {
        numOverFlowsEncoders++;
        TIM1_TFLG2 = _S12_TOF;
    }

    currentTime_L = ((unsigned long)numOverFlowsEncoders << 16) + TIM1_TC4;


    //Checking for a valid pulse
    // (Note: 1ms = 1500 ticks)
    if (currentTime_L - lastTime_L < 30000)
    {
        return;
    }


    tickCount_L++;

    if (desiredTicks_L != 0 && tickCount_L == desiredTicks_L)
    {
        ES_Event ThisEvent;
        ThisEvent.EventType = EV_MOVE_COMPLETE;
        ThisEvent.EventParam = DRIVE_MOTOR_MOVE_POST_PARAM;
        PostMasterMachine(ThisEvent);
        PostFACDriveService(ThisEvent);
        StopDrive();
        tickCount_L = 0;
        desiredTicks_L = 0;

        MoveCompleteEventSent = 1;

    }

    lastPeriod_L = currentTime_L - lastTime_L;
    lastTime_L = currentTime_L;
    //printf("%lu\r\n",currentTime_L);
}

/****************************************************************************
Function
  RightEncoderResponseInterrupt

Parameters
  None

Returns
  None

Description
  Interrupt Function: Not to be called by code
  Calculates the last period between right encoder ticks
****************************************************************************/
void interrupt _Vec_tim1ch5 RightEncoderResponseInterrupt(void)
{
    static unsigned long currentTime_R;
    static unsigned long lastTime_R = 0;

    TIM1_TFLG1 =  _S12_C5F;   //Clear Flag



    if (TIM1_TFLG2 & _S12_TOF == _S12_TOF && TIM1_TC5 < 0x8000)
    {
        numOverFlowsEncoders++;
        TIM1_TFLG2 = _S12_TOF;
    }


    currentTime_R = ((unsigned long)numOverFlowsEncoders << 16) + TIM1_TC5;

    //Checking for a valid pulse
    // (Note: 1ms = 1500 ticks)
    if (currentTime_R - lastTime_R < 30000)
    {
        return;
    }


    tickCount_R++;

    //Update Speeds to make sure one wheel isn't going much significantly faster than the other
    if (!MoveCompleteEventSent)
    {
        //EquatePosition();
    }

    if (desiredTicks_R != 0 && tickCount_R == desiredTicks_R)
    {
        /*
        ES_Event ThisEvent;
        ThisEvent.EventType = EV_MOVE_COMPLETE;
        ThisEvent.EventParam = DRIVE_MOTOR_MOVE_POST_PARAM;
        PostMasterMachine(ThisEvent);
        StopDrive();
        tickCount_R = 0;
        desiredTicks_R = 0;

        MoveCompleteEventSent = 1;
        */
    }

    lastPeriod_R = currentTime_R - lastTime_R;
    lastTime_R = currentTime_R;
}


/****************************************************************************
Function
  EncoderOverflowResponseInterrupt

Parameters
  None

Returns
  None

Description
  Interrupt Function: Not to be called by code
  Increments the number of overflows for input capture
****************************************************************************/
void interrupt _Vec_tim1ovf EncoderOverflowResponseInterrupt(void)
{
    numOverFlowsEncoders++;
    TIM1_TFLG2 = _S12_TOF;

    IncrementShooterEncoderOverflow();
}



/****************************************************************************
Function
  PowerPlantResponseInterrupt

Parameters
  None

Returns
  None

Description
  Interrupt Function: Not to be called by code
  Posts Event if Power plant is detected
****************************************************************************/
void interrupt _Vec_tim1ch7 PowerPlantResponseInterrupt(void)
{

    static int lastTime = 0;
    static int currentTime;
    static float lastPeriod = 0; //in milliseconds
    static int counter = 0;

    TIM1_TFLG1 =  _S12_C7F;   //Clear Flag

    currentTime = TIM1_TC7;

    lastPeriod = (float)(currentTime - lastTime) / MS_IC;

    if (lastPeriod < EnemyPowerPeriod + PLUS_MINUS && lastPeriod > EnemyPowerPeriod - PLUS_MINUS)
    {
        if (++counter % 3 == 0)
        {
            ES_Event ThisEvent;
            ThisEvent.EventType = EV_POWER_PLANT_DETECTED;
            PostDriveMotorService(ThisEvent);
        }

    }
    else
    {
        counter = 0;
    }

    lastTime = currentTime;
}




/****************************************************************************
 Function
   InitLeftMotor

 Parameters
   None

 Returns
   None

 Description
   Initializes the Left Motor: Encoder Response, PWM system, and Polarity Control
****************************************************************************/
void InitLeftMotor(void)
{
    /*-----Initialize Right Encoder Input Capture on Timer 1, Timer 4, Port T4----*/

    TIM1_TIOS     &=    ~_S12_IOS4;  //Input Capture
    TIM1_TIE      |=     _S12_C4I;   //Enable Interrupt

    TIM1_TCTL3    |=    _S12_EDG4A;  //Capture on any rising
    TIM1_TCTL3    &=    ~_S12_EDG4B; //Flag Rising Edges

    TIM1_TFLG1    =     _S12_C4F;    //Clear Flag

    EnableInterrupts;                //Globally Enable Interrupts



    /*----------Initialize PWM System for Right Motor------------------*/
    PWM_Init(WHEEL_PWM_LEFT_CHANNEL);
    PWM_Set_Frequency(WHEEL_PWM_LEFT_CHANNEL, DRIVE_PWM_FREQ);
    //PWM_Set_Polarity(WHEEL_PWM_LEFT_CHANNEL, PWM_POLARITY_LOW);
    PWM_Set_Duty_Cycle(WHEEL_PWM_LEFT_CHANNEL, 0);

    /*----------Initialize Polarity Port On the Right Motor------------*/
    WHEEL_POLARITY_LEFT_IO    |= WHEEL_POLARITY_LEFT_BIT;
    //Initializing Polarity to High
    WHEEL_POLARITY_LEFT_PORT  |= WHEEL_POLARITY_LEFT_BIT;
}



/****************************************************************************
 Function
   InitRightMotor

 Parameters
   None

 Returns
   None

 Description
   Initializes the Right Motor: Encoder Response, PWM system, and Polarity Control
****************************************************************************/
void InitRightMotor(void)
{
    /*-----Initialize Right Encoder Input Capture on Timer 1, Timer 5, Port T5----*/
    TIM1_TIOS     &=    ~_S12_IOS5;  //Input Capture
    TIM1_TIE      |=     _S12_C5I;   //Enable Interrupt

    TIM1_TCTL3    |=    _S12_EDG5A;  //Capture on any rising
    TIM1_TCTL3    &=    ~_S12_EDG5B; //Flag Rising Edges

    TIM1_TFLG1    =     _S12_C5F;    //Clear Flag


    EnableInterrupts;                //Globally Enable Interrupts



    /*----------Initialize PWM System for Right Motor------------------*/
    PWM_Init(WHEEL_PWM_RIGHT_CHANNEL);
    PWM_Set_Frequency(WHEEL_PWM_RIGHT_CHANNEL, DRIVE_PWM_FREQ);
    //PWM_Set_Polarity(WHEEL_PWM_RIGHT_CHANNEL, PWM_POLARITY_LOW);
    PWM_Set_Duty_Cycle(WHEEL_PWM_RIGHT_CHANNEL, 0);

    /*----------Initialize Polarity Port On the Right Motor------------*/
    WHEEL_POLARITY_RIGHT_IO    |= WHEEL_POLARITY_RIGHT_BIT;
    //Initializing Polarity to High
    WHEEL_POLARITY_RIGHT_PORT  |= WHEEL_POLARITY_RIGHT_BIT;
}

/****************************************************************************
 Function
   InitTimer

 Parameters
   None

 Returns
   None

 Description
   Initializes the Enconder Timer for Drive Motors and Shooter Motor
****************************************************************************/
void InitEncoderTimer(void)
{
    /*-----Initialize Encoder Input Captures on Timer 1, Timers 4-7, Port T4-7----*/
    TIM1_TSCR1    =     _S12_TEN;
    // Note: This Timer is Shared By the ShooterMotor Encoder As Well
    /*---------------------------------------------------
       TIM2 Timing Details
       Dividing 24MHz by 16, 1.5 MHz clock, 1 ms = 1500 ticks
    ----------------------------------------------------*/
    TIM1_TSCR2    |=     _S12_PR2;

    TIM1_TSCR2    |=     _S12_TOI;   //Enable Overflow Interrupt
    TIM1_TFLG2 = _S12_TOF;

}




/****************************************************************************
 Function
   InitPowerPlantDetector

 Parameters
   None

 Returns
   None

 Description
   Initializes the power plant detector
****************************************************************************/
void InitPowerPlantDetector(void)
{

    /*-----Initialize Right Encoder Input Capture on Timer 1, Timer 7, Port T7----*/
    //TIM1_TSCR1    =     _S12_TEN;
    // Note: This timer is shared by the shooter motor and drive motor encoders
    /*---------------------------------------------------
       TIM2 Timing Details
       Dividing 24MHz by 16, 1.5MHz clock, 1 ms = 1500 ticks
    ----------------------------------------------------*/
    //TIM1_TSCR2    =     _S12_PR2;

    TIM1_TIOS     &=    ~_S12_IOS7;  //Input Capture
    TIM1_TIE      |=     _S12_C7I;   //Enable Interrupt

    TIM1_TCTL3    |=    _S12_EDG7A;  //Capture on any rising
    TIM1_TCTL3    &=    ~_S12_EDG7B; //Flag Rising Edges

    TIM1_TFLG1    =     _S12_C7F;    //Clear Flag

    EnableInterrupts;                //Globally Enable Interrupts


    //Detmerining the frequency of the enemy robots power plant signal
    switch(GetColor(Self))
    {
        case TEAM_COLOR_RED:
            EnemyPowerPeriod = BLUE_POWER_PERIOD;
            printf("We are red\r\n");
            break;

        case TEAM_COLOR_BLUE:
            EnemyPowerPeriod = RED_POWER_PERIOD;
            printf("We are blue\r\n");
            break;

        default:
            EnemyPowerPeriod = BLUE_POWER_PERIOD;
            printf("Unknown Color\r\n");
            break;
    }

}

EventCheckers.c

// Event Checking functions for sample

#include "ES_Configure.h"
#include "ES_General.h"
#include "ES_Events.h"
#include "ES_PostList.h"
#include "EventCheckers.h"
#include "Const.h"
#include "MasterMachine.h"
#include <stdio.h>

// This include will pull in all of the headers from the service modules
// providing the prototypes for all of the post functions
#include "ES_ServiceHeaders.h"

boolean CheckBallIn(void)
{
    static unsigned char CurrentPinState = 0;
    static unsigned char LastPinState = 255;
    boolean ReturnVal = False;

    // Current pin state (1 for high, 0 for low)
    CurrentPinState = ((BALL_ENTER_HOPPER_PORT & BALL_ENTER_HOPPER_BIT) != 0);

    // First time through
    if (LastPinState == 255)
    {
        LastPinState = CurrentPinState;
        return False;
    }

    // If we see a change in pin state
    if (CurrentPinState != LastPinState)
    {
        // If the pin is high
        if (CurrentPinState != 0)
        {
            ES_Event PostEvent;
            PostEvent.EventType = EV_BALL_DROPPED;

            ReturnVal = PostMasterMachine(PostEvent);
        }
    }

    LastPinState = CurrentPinState;

    return ReturnVal;
}

boolean CheckBallOut(void)
{
    static unsigned char CurrentPinState = 0;
    static unsigned char LastPinState = 255;
    boolean ReturnVal = False;

    // Current pin state (1 for high, 0 for low)
    CurrentPinState = ((BALL_SHOT_PORT & BALL_SHOT_BIT) != 0);

    // First time through
    if (LastPinState == 255)
    {
        LastPinState = CurrentPinState;
        return False;
    }

    // If we see a change in pin state
    if (CurrentPinState != LastPinState)
    {
        // If the pin is high
        if (CurrentPinState != 0)
        {
            ES_Event PostEvent;
            PostEvent.EventType = EV_BALL_SHOT;

            ReturnVal = PostMasterMachine(PostEvent);
        }
    }

    LastPinState = CurrentPinState;

    return ReturnVal;
}

boolean CheckBumpSensor(void)
{
    static unsigned char CurrentPinState = 0;
    static unsigned char LastPinState = 255;
    boolean ReturnVal = False;

    // Current pin state (1 for high, 0 for low)
    CurrentPinState = ((BUMP_SENSOR_PORT & BUMP_SENSOR_BIT) != 0);

    // First time through
    if (LastPinState == 255)
    {
        LastPinState = CurrentPinState;
        return False;
    }

    // If we see a change in pin state
    if (CurrentPinState != LastPinState)
    {
        // If the pin is high
        if (CurrentPinState != 0)
        {
            ES_Event PostEvent;
            PostEvent.EventType = EV_BUMP_DETECTED;

            ReturnVal = PostMasterMachine(PostEvent);
        }
    }

    LastPinState = CurrentPinState;

    return ReturnVal;
}

FACDriveService.c

/****************************************************************************
 Module
   FACDriveService.c

 Description
   This module implements FAC controlled drive for the robot

****************************************************************************/
/*----------------------------- Include Files -----------------------------*/
/* include header files for this state machine as well as any machines at the
   next lower level in the hierarchy that are sub-machines to this machine
*/
#include "ES_Configure.h"
#include "ES_Framework.h"
#include "FACDriveService.h"
#include "DriveMotorService.h"
#include "QueryFAC.h"
#include "Const.h"
#include "MasterMachine.h"
#include "PositionLib.h"
#include "stdio.h"
/*----------------------------- Module Defines ----------------------------*/
#define UPDATE_DRIVE_TIME     400 //In milliseconds
#define UPDATE_TURN_TIME     150 //In milliseconds

#define ANGLE_TOLERANCE       1  //In degrees 
#define DISTANCE_TOLERANCE    .5  //In inches
#define MIN_TURN_ANGLE        8  //In degrees


/*---------------------------- Module Functions ---------------------------*/
void UpdatePosition(void);
boolean PositionIsAcceptable(void);
void UpdateDutyCycle(void);
boolean IsValidPosition(float x, float y, int r);

/*---------------------------- Module Variables ---------------------------*/
// everybody needs a state variable, you may need others as well.
// type of state variable should match htat of enum in header file
static FACDriveState_t CurrentState;

// with the introduction of Gen2, we need a module level Priority var as well
static uint8_t MyPriority;

static float selfX = 0;
static float selfY = 96;
static int selfRotation = 90;

static float destinationX;
static float destinationY;

static unsigned char DutyCycle_R;
static unsigned char DutyCycle_L;

/*------------------------------ Module Code ------------------------------*/
/****************************************************************************
 Function
     InitFACDriveService

 Parameters
     uint8_t : the priorty of this service

 Returns
     boolean, False if error in initialization, True otherwise

 Description
     Saves away the priority, sets up the initial transition and does any
     other required initialization for this state machine
****************************************************************************/
boolean InitFACDriveService ( uint8_t Priority )
{
    ES_Event ThisEvent;
    MyPriority = Priority;

    //Initially put in off state
    CurrentState = FAC_Drive_Off;

    // post the initial transition event
    ThisEvent.EventType = ES_INIT;
    if (ES_PostToService( MyPriority, ThisEvent) == True)
    {
        return True;
    }
    else
    {
        return False;
    }
}

/****************************************************************************
 Function
     PostFACDriveService

 Parameters
     EF_Event ThisEvent , the event to post to the queue

 Returns
     boolean False if the Enqueue operation failed, True otherwise

 Description
     Posts an event to this state machine's queue
****************************************************************************/
boolean PostFACDriveService( ES_Event ThisEvent )
{
    return ES_PostToService( MyPriority, ThisEvent);
}

/****************************************************************************
 Function
    RunFACDriveService

 Parameters
   ES_Event : the event to process

 Returns
   ES_Event, ES_NO_EVENT if no error ES_ERROR otherwise

 Description
   Runs the FACDriveService state machine
****************************************************************************/
ES_Event RunFACDriveService( ES_Event ThisEvent )
{
    ES_Event ReturnEvent;
    ReturnEvent.EventType = ES_NO_EVENT; // assume no errors

    switch ( CurrentState )
    {
            /*------------------STATE: FAC_Drive_Off---------------*/
        case FAC_Drive_Off:
            //Do Nothing
            break;


            /*------------------STATE: DriveForward_Calibrate---------------*/
        case DriveForward_Calibrate:
            if (ThisEvent.EventType == EV_CHANGE_FAC_DRIVE_STATE)
            {
                StopDrive();
            }
            UpdatePosition();

            if (PositionIsAcceptable())
            {
                ES_Event ThisEvent;
                ThisEvent.EventType = EV_MOVE_COMPLETE;

                ThisEvent.EventParam = FAC_MOTOR_MOVE_POST_PARAM;
                PostMasterMachine(ThisEvent);
                CurrentState = FAC_Drive_Off;
            }
            else
            {
                ES_Event ThisEvent;
                ThisEvent.EventType = EV_CHANGE_FAC_DRIVE_STATE;
                CurrentState = DriveForward_Turn;
                PostFACDriveService(ThisEvent);
            }
            break;



            /*------------------STATE: DriveForward_Turn---------------*/
        case DriveForward_Turn:

            switch(ThisEvent.EventType)
            {
                case EV_CHANGE_FAC_DRIVE_STATE:
                {
                    int turnAngle;

                    if (!IsValidPosition(selfX, selfY, selfRotation))
                    {
                        turnAngle = 0;
                    }
                    else
                    {
                        turnAngle = CalculateDeltaAngle(selfX, selfY, selfRotation, destinationX, destinationY);
                    }


                    if (turnAngle > MIN_TURN_ANGLE || turnAngle < -MIN_TURN_ANGLE)
                    {
                        if (turnAngle < 0)
                        {
                            Turn(-turnAngle, CW);
                        }
                        else
                        {
                            Turn(turnAngle, CCW);
                        }
                    }
                    else
                    {
                        ES_Event ThisEvent;
                        ThisEvent.EventType = EV_CHANGE_FAC_DRIVE_STATE;

                        CurrentState = DriveForward_Drive;
                        PostFACDriveService(ThisEvent);
                    }

                    break;
                }


                case EV_MOVE_COMPLETE:
                    if (ThisEvent.EventParam == DRIVE_MOTOR_MOVE_POST_PARAM)
                    {
                        ES_Event ThisEvent;
                        ThisEvent.EventType = EV_CHANGE_FAC_DRIVE_STATE;

                        CurrentState = DriveForward_Drive;
                        PostFACDriveService(ThisEvent);
                    }
            }
            break;

            /*------------------STATE: DriveForward_Drive---------------*/
        case DriveForward_Drive:


            switch(ThisEvent.EventType)
            {
                case EV_CHANGE_FAC_DRIVE_STATE:
                    UpdatePosition();
                    //DriveForward(CalculateDistance(selfX, selfY, destinationX, destinationY));
                    UpdateDutyCycle();

                    DriveManual(LEFT, CW, DutyCycle_L);
                    DriveManual(RIGHT, CW, DutyCycle_R);

                    ES_Timer_InitTimer(FAC_DRIVE_TIMER, UPDATE_DRIVE_TIME);
                    break;

                case ES_TIMEOUT:
                    if (ThisEvent.EventParam == FAC_DRIVE_TIMER)
                    {
                        ES_Event ThisEvent;
                        ThisEvent.EventType = EV_CHANGE_FAC_DRIVE_STATE;

                        CurrentState = DriveForward_Calibrate;
                        PostFACDriveService(ThisEvent);
                    }
                    break;
            }
            break;

            /*------------------STATE: DriveBackward_Calibrate---------------*/
        case DriveBackward_Calibrate:
            if (ThisEvent.EventType == EV_CHANGE_FAC_DRIVE_STATE)
            {
                StopDrive();
            }
            UpdatePosition();

            printf("Current Position x = %f, y=%f \n\r", selfX, selfY);

            if (PositionIsAcceptable())
            {
                ES_Event ThisEvent;
                ThisEvent.EventType = EV_MOVE_COMPLETE;

                printf("Posted Move Complete from FAC Drive Backward \n\r");

                ThisEvent.EventParam = FAC_MOTOR_MOVE_POST_PARAM;
                PostMasterMachine(ThisEvent);
                CurrentState = FAC_Drive_Off;
            }
            else
            {
                ES_Event ThisEvent;
                ThisEvent.EventType = EV_CHANGE_FAC_DRIVE_STATE;
                CurrentState = DriveBackward_Turn;
                PostFACDriveService(ThisEvent);
            }
            break;

            /*------------------STATE: DriveBackward_Turn---------------*/
        case DriveBackward_Turn:
            switch(ThisEvent.EventType)
            {
                case EV_CHANGE_FAC_DRIVE_STATE:
                {
                    int turnAngle;

                    if (!IsValidPosition(selfX, selfY, selfRotation))
                    {
                        turnAngle = 0;
                    }
                    else
                    {
                        turnAngle = CalculateDeltaAngle(selfX, selfY, selfRotation, destinationX, destinationY);
                        turnAngle = turnAngle + 180;
                        if (turnAngle > 180)
                        {
                            turnAngle -= 360;
                        }
                    }

                    if (turnAngle > MIN_TURN_ANGLE || turnAngle < -MIN_TURN_ANGLE)
                    {
                        if (turnAngle < 0)
                        {
                            Turn(-turnAngle, CW);
                        }
                        else
                        {
                            Turn(turnAngle, CCW);
                        }
                    }
                    else
                    {
                        ES_Event ThisEvent;
                        ThisEvent.EventType = EV_CHANGE_FAC_DRIVE_STATE;

                        CurrentState = DriveBackward_Drive;
                        PostFACDriveService(ThisEvent);
                    }
                    break;
                }


                case EV_MOVE_COMPLETE:
                    if (ThisEvent.EventParam == DRIVE_MOTOR_MOVE_POST_PARAM)
                    {
                        ES_Event ThisEvent;
                        ThisEvent.EventType = EV_CHANGE_FAC_DRIVE_STATE;

                        CurrentState = DriveBackward_Drive;
                        PostFACDriveService(ThisEvent);
                    }
                    break;
            }
            break;


            /*------------------STATE: DriveBackward_Drive---------------*/
        case DriveBackward_Drive:
            switch(ThisEvent.EventType)
            {
                case EV_CHANGE_FAC_DRIVE_STATE:
                    UpdatePosition();
                    //DriveBackward(CalculateDistance(selfX, selfY, destinationX, destinationY));
                    UpdateDutyCycle();

                    DriveManual(LEFT, CCW, DutyCycle_L);
                    DriveManual(RIGHT, CCW, DutyCycle_R);

                    ES_Timer_InitTimer(FAC_DRIVE_TIMER, UPDATE_DRIVE_TIME);
                    break;

                case ES_TIMEOUT:
                    if (ThisEvent.EventParam == FAC_DRIVE_TIMER)
                    {
                        ES_Event ThisEvent;
                        ThisEvent.EventType = EV_CHANGE_FAC_DRIVE_STATE;

                        CurrentState = DriveBackward_Calibrate;
                        PostFACDriveService(ThisEvent);
                    }
                    break;
            }
            break;


            /*------------------STATE: Turn_Calibrate---------------*/
        case Turn_Calibrate:
            if (ThisEvent.EventType == EV_CHANGE_FAC_DRIVE_STATE)
            {
                static unsigned char Count = 0;
                static unsigned long LastTimestamp = 0;

                if (LastTimestamp == 0)
                {
                    LastTimestamp = GetTimestamp();
                }

                StopDrive();
                UpdatePosition();

                if (PositionIsAcceptable())
                {
                    if (GetTimestamp() == LastTimestamp)
                    {
                        ES_Event ThisEvent;
                        ThisEvent.EventType = EV_CHANGE_FAC_DRIVE_STATE;
                        CurrentState = Turn_Calibrate;
                        PostFACDriveService(ThisEvent);
                        break;
                    }
                    Count++;
                    if (Count % 2 == 1)
                    {
                        ES_Event ThisEvent;
                        ThisEvent.EventType = EV_CHANGE_FAC_DRIVE_STATE;
                        CurrentState = Turn_Drive;
                        PostFACDriveService(ThisEvent);
                        break;
                    }
                    else
                    {
                        ES_Event ThisEvent;
                        ThisEvent.EventType = EV_MOVE_COMPLETE;

                        printf("Posted Move Complete from FAC Turn \n\r");
                        printf("Self rotation: %u\r\n", selfRotation);

                        ThisEvent.EventParam = FAC_MOTOR_MOVE_POST_PARAM;
                        PostMasterMachine(ThisEvent);
                        CurrentState = FAC_Drive_Off;
                    }
                }
                else
                {
                    ES_Event ThisEvent;
                    ThisEvent.EventType = EV_CHANGE_FAC_DRIVE_STATE;
                    CurrentState = Turn_Drive;
                    PostFACDriveService(ThisEvent);
                }
            }
            break;

            /*------------------STATE: Turn_Drive---------------*/
        case Turn_Drive:
            switch(ThisEvent.EventType)
            {
                case EV_CHANGE_FAC_DRIVE_STATE:
                {
                    UpdateDutyCycle();

                    ES_Timer_InitTimer(FAC_DRIVE_TIMER, UPDATE_TURN_TIME);
                    break;
                }

                case ES_TIMEOUT:
                {
                    if (ThisEvent.EventParam == FAC_DRIVE_TIMER)
                    {
                        ES_Event ThisEvent;
                        ThisEvent.EventType = EV_CHANGE_FAC_DRIVE_STATE;

                        CurrentState = Turn_Calibrate;
                        PostFACDriveService(ThisEvent);
                    }
                    break;
                }
                break;
            }
    }
    return ReturnEvent;
}

/****************************************************************************
 Function
     QueryFACDriveService

 Parameters
     None

 Returns
     FACDriveState_t The current state of the Template state machine

 Description
     returns the current state of the FAC Drive State Machine
****************************************************************************/
FACDriveState_t QueryFACDriveService ( void )
{
    return(CurrentState);
}

/****************************************************************************
 Function
     FAC_DriveStop

 Parameters
     None

 Returns
     None

 Description
     Has the robot stop moving
****************************************************************************/
void FAC_DriveStop(void)
{
    CurrentState = FAC_Drive_Off;
    DriveManual(LEFT, CW, 0);
    DriveManual(RIGHT, CW, 0);
}


/****************************************************************************
 Function
     FAC_Turn

 Parameters
     float x, float y

 Returns
     None

 Description
     Has the robot turn towards the point x,y
****************************************************************************/
void FAC_Turn(float x, float y)
{
    ES_Event ThisEvent;
    ThisEvent.EventType = EV_CHANGE_FAC_DRIVE_STATE;

    selfX = 0;
    selfY = 96;
    selfRotation = 90;

    UpdatePosition();
    destinationX = x;
    destinationY = y;

    DutyCycle_L = 90;
    DutyCycle_R = 90;

    CurrentState = Turn_Calibrate;

    RunFACDriveService(ThisEvent);
}


/****************************************************************************
 Function
     FAC_DriveForward

 Parameters
     float x, float y

 Returns
     None

 Description
     Has the robot turn towards the point x,y and then drive forward to it.
****************************************************************************/
void FAC_DriveForward(float x, float y)
{

    ES_Event ThisEvent;
    ThisEvent.EventType = EV_CHANGE_FAC_DRIVE_STATE;

    selfX = 0;
    selfY = 96;
    selfRotation = 90;

    UpdatePosition();
    destinationX = x;
    destinationY = y;

    DutyCycle_L = 90;
    DutyCycle_R = 90;

    CurrentState = DriveForward_Calibrate;

    RunFACDriveService(ThisEvent);
}




/****************************************************************************
 Function
     FAC_DriveBackward

 Parameters
     float x, float y

 Returns
     None

 Description
     Has the robot turn its back to the point x,y and then drive backward
     to it.
****************************************************************************/
void FAC_DriveBackward(float x, float y)
{
    ES_Event ThisEvent;
    ThisEvent.EventType = EV_CHANGE_FAC_DRIVE_STATE;

    selfX = 0;
    selfY = 96;
    selfRotation = 90;

    UpdatePosition();
    destinationX = x;
    destinationY = y;

    DutyCycle_L = 90;
    DutyCycle_R = 90;

    CurrentState = DriveBackward_Calibrate;

    RunFACDriveService(ThisEvent);
}



/***************************************************************************
 private functions
 ***************************************************************************/

/****************************************************************************
Function
    updatePosition

Parameters
    None

Returns
    None

Description
    Updates the robots internal position variables based on the FAC
    reading.
****************************************************************************/
void UpdatePosition(void)
{
    if (IsValidPosition(GetX(Self), GetY(Self), GetRotation(Self)))
    {
        selfX = GetX(Self);
        selfY = GetY(Self);
        selfRotation = GetRotation(Self);
    }
}


/****************************************************************************
Function
    PositionIsAcceptable

Parameters
    None

Returns
    None

Description
    Compares the position of the robot and the destination and returns true
    if they are close enough
****************************************************************************/
boolean PositionIsAcceptable(void)
{
    switch(CurrentState)
    {
        case Turn_Calibrate:
            return (CalculateDeltaAngle(selfX, selfY, selfRotation, destinationX, destinationY) <= ANGLE_TOLERANCE
                    && CalculateDeltaAngle(selfX, selfY, selfRotation, destinationX, destinationY) >= -ANGLE_TOLERANCE);
            break;

        case DriveForward_Calibrate:
        case DriveBackward_Calibrate:
        {
            float deltaX;
            float deltaY;

            deltaX = selfX - destinationX;
            if (deltaX < 0)
            {
                deltaX = -deltaX;
            }

            deltaY = selfY - destinationY;
            if (deltaY < 0)
            {
                deltaY = -deltaY;
            }

            return ( (deltaX <= DISTANCE_TOLERANCE) && (deltaY <= DISTANCE_TOLERANCE));
            break;
        }

    }
}

/****************************************************************************
Function
    UpdateDutyCycle

Parameters
    None

Returns
    None

Description
    Slows the robot down as it gets closer to the target
****************************************************************************/
void UpdateDutyCycle(void)
{

    switch(CurrentState)
    {
        case DriveForward_Drive:
        case DriveBackward_Drive:
        {
            float deltaX;
            float deltaY;
            float maxDelta;

            deltaX = selfX - destinationX;
            if (deltaX < 0)
            {
                deltaX = -deltaX;
            }

            deltaY = selfY - destinationY;
            if (deltaY < 0)
            {
                deltaY = -deltaY;
            }


            maxDelta = deltaX;
            if (maxDelta < deltaY)
            {
                maxDelta = deltaY;
            }

            if (maxDelta > 15)
            {
                DutyCycle_L = 100;
                DutyCycle_R = 100;
            }
            else if (maxDelta > 10)
            {
                DutyCycle_L = 80;
                DutyCycle_R = 80;
            }
            else if (maxDelta > 8)
            {
                DutyCycle_L = 60;
                DutyCycle_R = 60;
            }
            else if (maxDelta > 4)
            {
                DutyCycle_L = 55;
                DutyCycle_R = 55;
            }
            else if (maxDelta > 0)
            {
                DutyCycle_L = 50;
                DutyCycle_R = 50;
            }
            else
            {
                DutyCycle_L = 0;
                DutyCycle_R = 0;
            }

            break;
        }


        case Turn_Drive:
        {
            int turnAngle;
            Direction_t direction = CCW;

            turnAngle = CalculateDeltaAngle(selfX, selfY, selfRotation, destinationX, destinationY);

            if (turnAngle < 0)
            {
                turnAngle = -turnAngle;
                direction = CW;
            }

            if (turnAngle > 60)
            {
                DutyCycle_L = 70;
                DutyCycle_R = 70;
            }
            else if (turnAngle > 30)
            {
                DutyCycle_L = 60;
                DutyCycle_R = 60;
            }
            else if (turnAngle > 10)
            {
                DutyCycle_L = 50;
                DutyCycle_R = 50;
            }
            else if (turnAngle > 0)
            {
                DutyCycle_L = 50;
                DutyCycle_R = 50;
            }
            else
            {
                DutyCycle_L = 0;
                DutyCycle_R = 0;
            }


            if (direction == CW)
            {
                DriveManual(LEFT, CW, DutyCycle_L);
                DriveManual(RIGHT, CCW, DutyCycle_R);
            }
            else
            {
                DriveManual(LEFT, CCW, DutyCycle_L);
                DriveManual(RIGHT, CW, DutyCycle_R);
            }
            break;
        }
    }

}

boolean IsValidPosition(float x, float y, int r)
{
    if (x != 0 && y != 96 && r != 90)
    {
        return True;
    }
    return False;
}

FiducialCover.c

/****************************************************************************
 Module
   FiducialCover.c

 Description
   This Module implements the fiducial control.
****************************************************************************/
/*----------------------------- Include Files -----------------------------*/
#include "ES_Configure.h"
#include "ES_Framework.h"
#include "Const.h"
#include "PWM.h"
#include <stdio.h>
#include "Servo.h"


#include "FiducialCover.h"

/*----------------------------- Module Defines ----------------------------*/
#define COVER 1000
#define UNCOVER 2000

/*---------------------------- Module Variables ---------------------------*/

void InitFiducialCover(void)
{
    // Already Initialized in Dispenser Control
    // Servo_Init();
    Servo_Enable(FIDUCIAL_SERVO_CHANNEL);
    Servo_Set_Pulse_Width(FIDUCIAL_SERVO_CHANNEL, UNCOVER);
}


void CoverFiducial(void)
{
    Servo_Set_Pulse_Width(FIDUCIAL_SERVO_CHANNEL, COVER);
}


void UncoverFiducial(void)
{
    Servo_Set_Pulse_Width(FIDUCIAL_SERVO_CHANNEL, UNCOVER);
}

FiducialVisibility.c

/****************************************************************************
 Module
   FiducialVisibility.c

 Description
   This is a file for implementing hiding and showing the fiducial.

****************************************************************************/
/*----------------------------- Include Files -----------------------------*/
// Basic includes for a program using the Events and Services Framework
#include "ES_Configure.h"
#include "ES_Framework.h"
#include "FiducialCover.h"

/* include header files for this state machine as well as any machines at the
   next lower level in the hierarchy that are sub-machines to this machine
*/
#include "FiducialVisibility.h"
#include "Gameplay.h"
#include <stdio.h>


/*----------------------------- Module Defines ----------------------------*/
// define constants for the states for this machine
// and any other local defines

/*---------------------------- Module Functions ---------------------------*/
/* prototypes for private functions for this machine, things like during
   functions, entry & exit functions.They should be functions relevant to the
   behavior of this state machine
*/
static ES_Event DuringVisible( ES_Event Event);
static ES_Event DuringInvisible( ES_Event Event);

/*---------------------------- Module Variables ---------------------------*/
// everybody needs a state variable, you may need others as well
static FiducialVisibilityState_t CurrentState;

/*------------------------------ Module Code ------------------------------*/
/****************************************************************************
 Function
    RunFiducialVisibility

 Parameters
   ES_Event: the event to process

 Returns
   ES_Event: an event to return

****************************************************************************/
// make recursive call warning into info
#pragma MESSAGE INFORMATION C1855

ES_Event RunFiducialVisibility( ES_Event CurrentEvent )
{
    unsigned char MakeTransition = False;/* are we making a state transition? */
    FiducialVisibilityState_t NextState = CurrentState;
    ES_Event EntryEventKind = { ES_ENTRY, 0 };// default to normal entry to new state
    ES_Event ReturnEvent = CurrentEvent; // assume we are not consuming event

    switch ( CurrentState )
    {
        case VISIBLE :
        {
            // Execute During function for this state. ES_ENTRY & ES_EXIT are
            // processed here allow the lower level state machines to re-map
            // or consume the event
            CurrentEvent = DuringVisible(CurrentEvent);
            //process any events
            if ( CurrentEvent.EventType != ES_NO_EVENT ) //If an event is active
            {
                switch (CurrentEvent.EventType)
                {
                    case EV_OUT_OF_BALLS :
                        NextState = INVISIBLE;//Decide what the next state will be
                        MakeTransition = True; //mark that we are taking a transition
                        EntryEventKind.EventType = ES_ENTRY;
                        ReturnEvent = CurrentEvent;
                        break;
                }
            }
            break;
        }
        case INVISIBLE :
        {
            // Execute During function for this state. ES_ENTRY & ES_EXIT are
            // processed here allow the lower level state machines to re-map
            // or consume the event
            CurrentEvent = DuringInvisible(CurrentEvent);
            //process any events
            if ( CurrentEvent.EventType != ES_NO_EVENT ) //If an event is active
            {
                switch (CurrentEvent.EventType)
                {
                    case EV_IN_POSITION_TO_SHOOT :
                        if (QueryGameplay() == MOVING_TO_SHOOT || QueryGameplay() == SHOOTING)
                        {
                            NextState = VISIBLE;//Decide what the next state will be
                            MakeTransition = True; //mark that we are taking a transition
                            // if transitioning to a state with history change kind of entry
                            EntryEventKind.EventType = ES_ENTRY;
                            ReturnEvent = CurrentEvent;
                        }
                        break;
                }
            }
            break;
        }
    }
    // If we are making a state transition
    if (MakeTransition == True)
    {
        // Execute exit function for current state
        CurrentEvent.EventType = ES_EXIT;
        RunFiducialVisibility(CurrentEvent);

        CurrentState = NextState; //Modify state variable

        // Execute entry function for new state
        RunFiducialVisibility(EntryEventKind);
    }
    return(ReturnEvent);
}
/****************************************************************************
 Function
     StartFiducialVisibility

 Parameters
     None

 Returns
     None

 Description
     Does any required initialization for this state machine

****************************************************************************/
void StartFiducialVisibility ( ES_Event CurrentEvent )
{
    // local variable to get debugger to display the value of CurrentEvent
    ES_Event LocalEvent = CurrentEvent;
    // to implement entry to a history state or directly to a substate
    // you can modify the initialization of the CurrentState variable
    // otherwise just start in the entry state every time the state machine
    // is started
    if ( ES_ENTRY_HISTORY != CurrentEvent.EventType )
    {
        CurrentState = VISIBLE;
    }
    // call the entry function (if any) for the ENTRY_STATE
    RunFiducialVisibility(CurrentEvent);
}

/****************************************************************************
 Function
     QueryFiducialVisibility

 Parameters
     None

 Returns
     FiducialVisibilityState_t The current state of the state machine

****************************************************************************/
FiducialVisibilityState_t QueryFiducialVisibility ( void )
{
    return(CurrentState);
}

/***************************************************************************
 private functions
 ***************************************************************************/

static ES_Event DuringVisible( ES_Event Event)
{
    ES_Event ReturnEvent = Event; // assmes no re-mapping or comsumption

    // process ES_ENTRY, ES_ENTRY_HISTORY & ES_EXIT events
    if ( (Event.EventType == ES_ENTRY) ||
            (Event.EventType == ES_ENTRY_HISTORY) )
    {
        // implement any entry actions required for this state machine

        // Show Fiducial
        UncoverFiducial();

        // after that start any lower level machines that run in this state
    }
    else if ( Event.EventType == ES_EXIT )
    {
        // on exit, give the lower levels a chance to clean up first

        // now do any local exit functionality

        // Hide fiducial
        //CoverFiducial();

    }
    else
        // do the 'during' function for this state
    {
        // run any lower level state machine

        // do any activity that is repeated as long as we are in this state
    }
    // return either Event, if you don't want to allow the lower level machine
    // to remap the current event, or ReturnEvent if you do want to allow it.
    return(ReturnEvent);
}


static ES_Event DuringInvisible( ES_Event Event)
{
    ES_Event ReturnEvent = Event; // assmes no re-mapping or comsumption


    // process ES_ENTRY, ES_ENTRY_HISTORY & ES_EXIT events
    if ( (Event.EventType == ES_ENTRY) ||
            (Event.EventType == ES_ENTRY_HISTORY) )
    {
        // implement any entry actions required for this state machine

        //No entry action

        //No lower level sm to enter
    }
    else if ( Event.EventType == ES_EXIT )
    {
        //No lower level sm to exit

        //No exit action

    }
    else
        // do the 'during' function for this state
    {
        // run any lower level state machine

        // do any activity that is repeated as long as we are in this state
    }
    // return either Event, if you don't want to allow the lower level machine
    // to remap the current event, or ReturnEvent if you do want to allow it.
    return(ReturnEvent);
}

GameMaster.c

/****************************************************************************
 Module
   GameMaster.c

 Revision
   1.0

 Description
   This is a template file for implementing the game state

****************************************************************************/
/*----------------------------- Include Files -----------------------------*/
// Basic includes for a program using the Events and Services Framework
#include "ES_Configure.h"
#include "ES_Framework.h"
#include "Const.h"

/* include header files for this state machine as well as any machines at the
   next lower level in the hierarchy that are sub-machines to this machine
*/
#include "GameMaster.h"

#include "Gameplay.h"
#include "FiducialVisibility.h"

/*----------------------------- Module Defines ----------------------------*/
// define constants for the states for this machine
// and any other local defines

/*---------------------------- Module Functions ---------------------------*/
/* prototypes for private functions for this machine, things like during
   functions, entry & exit functions.They should be functions relevant to the
   behavior of this state machine
*/
static ES_Event DuringWaiting( ES_Event Event);
static ES_Event DuringPlaying( ES_Event Event);

/*---------------------------- Module Variables ---------------------------*/
// everybody needs a state variable, you may need others as well
static GameMasterState_t CurrentState;

/*------------------------------ Module Code ------------------------------*/
/****************************************************************************
 Function
    RunGameMaster

 Parameters
   ES_Event: the event to process

 Returns
   ES_Event: an event to return

****************************************************************************/
// make recursive call warning into info
#pragma MESSAGE INFORMATION C1855

ES_Event RunGameMaster( ES_Event CurrentEvent )
{
    unsigned char MakeTransition = False;/* are we making a state transition? */
    GameMasterState_t NextState = CurrentState;
    ES_Event EntryEventKind = { ES_ENTRY, 0 };// default to normal entry to new state
    ES_Event ReturnEvent = CurrentEvent; // assume we are not consuming event

    switch ( CurrentState )
    {
        case WAITING :
        {
            // Execute During function for this state. ES_ENTRY & ES_EXIT are
            // processed here allow the lower level state machines to re-map
            // or consume the event
            CurrentEvent = DuringWaiting(CurrentEvent);
            //process any events
            if ( CurrentEvent.EventType != ES_NO_EVENT ) //If an event is active
            {
                switch (CurrentEvent.EventType)
                {
                    case EV_GAME_START :

                        NextState = PLAYING;//Decide what the next state will be

                        MakeTransition = True; //mark that we are taking a transition
                        // if transitioning to a state with history change kind of entry
                        EntryEventKind.EventType = ES_ENTRY;
                        // optionally, consume or re-map this event for the upper
                        // level state machine
                        ReturnEvent = CurrentEvent;
                        break;

                }
            }
            break;
        }
        case PLAYING :
        {
            // Execute During function for this state. ES_ENTRY & ES_EXIT are
            // processed here allow the lower level state machines to re-map
            // or consume the event
            CurrentEvent = DuringPlaying(CurrentEvent);
            //process any events
            if ( CurrentEvent.EventType != ES_NO_EVENT ) //If an event is active
            {
                switch (CurrentEvent.EventType)
                {
                    case EV_GAME_OVER :

                        NextState = WAITING;//Decide what the next state will be

                        MakeTransition = True; //mark that we are taking a transition
                        // if transitioning to a state with history change kind of entry
                        EntryEventKind.EventType = ES_ENTRY;
                        // optionally, consume or re-map this event for the upper
                        // level state machine
                        ReturnEvent = CurrentEvent;
                        break;

                }
            }
            break;
        }
    }
    //   If we are making a state transition
    if (MakeTransition == True)
    {
        //   Execute exit function for current state
        CurrentEvent.EventType = ES_EXIT;
        RunGameMaster(CurrentEvent);

        CurrentState = NextState; //Modify state variable

        //   Execute entry function for new state

        RunGameMaster(EntryEventKind);
    }
    return(ReturnEvent);
}
/****************************************************************************
 Function
     StartGameMaster

 Parameters
     None

 Returns
     None

 Description
     Does any required initialization for this state machine
 Notes

 Author
     J. Edward Carryer, 2/18/99, 10:38AM
****************************************************************************/
void StartGameMaster ( ES_Event CurrentEvent )
{
    // local variable to get debugger to display the value of CurrentEvent
    ES_Event LocalEvent = CurrentEvent;
    // to implement entry to a history state or directly to a substate
    // you can modify the initialization of the CurrentState variable
    // otherwise just start in the entry state every time the state machine
    // is started
    if ( ES_ENTRY_HISTORY != CurrentEvent.EventType )
    {
        CurrentState = WAITING;
    }

    // Set LED game on to output
    LED_GAME_ON_IO |= LED_GAME_ON_BIT;

    // call the entry function (if any) for the ENTRY_STATE
    RunGameMaster(CurrentEvent);
}

/****************************************************************************
 Function
     QueryGameMaster

 Parameters
     None

 Returns
     GameMasterState_t The current state of the Game master state machine

 Description
     returns the current state of the Game master state machine
 Notes

 Author
     J. Edward Carryer, 2/11/05, 10:38AM
****************************************************************************/
GameMasterState_t QueryGameMaster ( void )
{
    return(CurrentState);
}

/***************************************************************************
 private functions
 ***************************************************************************/

static ES_Event DuringWaiting( ES_Event Event)
{
    ES_Event ReturnEvent = Event; // assmes no re-mapping or comsumption

    // process ES_ENTRY, ES_ENTRY_HISTORY & ES_EXIT events
    if ( (Event.EventType == ES_ENTRY) ||
            (Event.EventType == ES_ENTRY_HISTORY) )
    {
        // implement any entry actions required for this state machine
        // Turn LED Game On light off
        LED_GAME_ON_PORT &= ~LED_GAME_ON_BIT;

        // after that start any lower level machines that run in this state


    }
    else if ( Event.EventType == ES_EXIT )
    {
        // on exit, give the lower levels a chance to clean up first


        // now do any local exit functionality
        // Turn LED Game On light on
        LED_GAME_ON_PORT |= LED_GAME_ON_BIT;
    }
    else
        // do the 'during' function for this state
    {
        // run any lower level state machine


        // do any activity that is repeated as long as we are in this state
    }
    // return either Event, if you don't want to allow the lower level machine
    // to remap the current event, or ReturnEvent if you do want to allow it.
    return(ReturnEvent);
}

static ES_Event DuringPlaying( ES_Event Event)
{
    ES_Event ReturnEvent = Event; // assmes no re-mapping or comsumption

    // process ES_ENTRY, ES_ENTRY_HISTORY & ES_EXIT events
    if ( (Event.EventType == ES_ENTRY) ||
            (Event.EventType == ES_ENTRY_HISTORY) )
    {
        // implement any entry actions required for this state machine

        // after that start any lower level machines that run in this state


        StartFiducialVisibility(Event);
        StartGameplay(Event);
    }
    else if ( Event.EventType == ES_EXIT )
    {
        // on exit, give the lower levels a chance to clean up first

        RunFiducialVisibility(Event);
        RunGameplay(Event);

        // now do any local exit functionality
    }
    else
        // do the 'during' function for this state
    {
        // run any lower level state machine

        RunFiducialVisibility(Event);
        ReturnEvent = RunGameplay(Event);

        // do any activity that is repeated as long as we are in this state
    }
    // return either Event, if you don't want to allow the lower level machine
    // to remap the current event, or ReturnEvent if you do want to allow it.
    return(ReturnEvent);
}

Gameplay.c

/****************************************************************************
 Module
   Gameplay.c

 Description
   This is a file for the states of our game (strategy)
****************************************************************************/
/*----------------------------- Include Files -----------------------------*/
// Basic includes for a program using the Events and Services Framework
#include "ES_Configure.h"
#include "ES_Framework.h"
#include "Const.h"

/* include header files for this state machine as well as any machines at the
   next lower level in the hierarchy that are sub-machines to this machine
*/
#include "MasterMachine.h"
#include "Gameplay.h"
#include "Shooting.h"
#include "MovingToDepot.h"
#include "RetrievingBalls.h"
#include "TapeSensors.h"
#include "ShooterMotorService.h"
#include "DriveMotorService.h"
#include <stdio.h>
#include "QueryFAC.h"
#include "FACDriveService.h"

#include "DispenserControl.h"

/*----------------------------- Module Defines ----------------------------*/

/*---------------------------- Module Functions ---------------------------*/
/* prototypes for private functions for this machine, things like during
   functions, entry & exit functions.They should be functions relevant to the
   behavior of this state machine
*/
static ES_Event DuringShooting( ES_Event Event);
static ES_Event DuringRetrievingBalls( ES_Event Event);
static ES_Event DuringMovingToShoot( ES_Event Event);
static ES_Event DuringMovingToDepot( ES_Event Event);
static ES_Event DuringDrivingForwardGameplay( ES_Event Event);
static ES_Event DuringDroppingExtraBalls( ES_Event Event);

/*---------------------------- Module Variables ---------------------------*/
// everybody needs a state variable, you may need others as well
static GameplayState_t CurrentState;

/*------------------------------ Module Code ------------------------------*/
/****************************************************************************
 Function
    RunGameplay

 Parameters
   ES_Event: the event to process

 Returns
   ES_Event: an event to return

****************************************************************************/
// make recursive call warning into info
#pragma MESSAGE INFORMATION C1855

ES_Event RunGameplay( ES_Event CurrentEvent )
{
    unsigned char MakeTransition = False;/* are we making a state transition? */
    GameplayState_t NextState = CurrentState;
    ES_Event EntryEventKind = { ES_ENTRY, 0 };// default to normal entry to new state
    ES_Event ReturnEvent = CurrentEvent; // assume we are not consuming event

    switch ( CurrentState )
    {
        case SHOOTING :
        {
            // Execute During function for this state. ES_ENTRY & ES_EXIT are
            // processed here allow the lower level state machines to re-map
            // or consume the event
            CurrentEvent = DuringShooting(CurrentEvent);
            //process any events
            if ( CurrentEvent.EventType != ES_NO_EVENT ) //If an event is active
            {
                switch (CurrentEvent.EventType)
                {
                    case EV_OUT_OF_BALLS :

                        NextState = MOVING_TO_DEPOT;//Decide what the next state will be

                        MakeTransition = True; //mark that we are taking a transition
                        // if transitioning to a state with history change kind of entry
                        EntryEventKind.EventType = ES_ENTRY;
                        // optionally, consume or re-map this event for the upper
                        // level state machine
                        ReturnEvent = CurrentEvent;
                        break;
                }
            }
            break;
        }
        case MOVING_TO_DEPOT :
        {
            // Execute During function for this state. ES_ENTRY & ES_EXIT are
            // processed here allow the lower level state machines to re-map
            // or consume the event
            CurrentEvent = DuringMovingToDepot(CurrentEvent);
            //process any events
            if ( CurrentEvent.EventType != ES_NO_EVENT ) //If an event is active
            {
                switch (CurrentEvent.EventType)
                {
                    case EV_BUMP_DETECTED :

                        FAC_DriveStop();
                        NextState = RETRIEVING_BALLS;//Decide what the next state will be

                        MakeTransition = True; //mark that we are taking a transition
                        // if transitioning to a state with history change kind of entry
                        EntryEventKind.EventType = ES_ENTRY;
                        // optionally, consume or re-map this event for the upper
                        // level state machine
                        ReturnEvent = CurrentEvent;
                        break;
                }
            }
            break;
        }
        case DRIVING_FORWARD_GAMEPLAY:
        {
            CurrentEvent = DuringDrivingForwardGameplay(CurrentEvent);
            switch(CurrentEvent.EventType)
            {
                case EV_MOVE_COMPLETE:
                {
                    if (CurrentEvent.EventParam == DRIVE_MOTOR_MOVE_POST_PARAM)
                    {

                        NextState = DROPPING_EXTRA_BALLS;//Decide what the next state will be

                        MakeTransition = True; //mark that we are taking a transition
                        // if transitioning to a state with history change kind of entry
                        EntryEventKind.EventType = ES_ENTRY;
                    }
                    // optionally, consume or re-map this event for the upper
                    // level state machine
                    ReturnEvent = CurrentEvent;
                    break;
                }
            }
            break;
        }
        case DROPPING_EXTRA_BALLS:
        {
            CurrentEvent = DuringDroppingExtraBalls(CurrentEvent);
            switch (CurrentEvent.EventType)
            {
                case ES_TIMEOUT:
                {
                    if (CurrentEvent.EventParam == BALL_SHOT_TIMER)
                    {
                        static unsigned char counter = 0;

                        if (counter++ >= MAX_BALLS)
                        {
                            TurnShooterOff();
                            NextState = MOVING_TO_DEPOT;

                            MakeTransition = True; //mark that we are taking a transition
                            counter = 0;
                        }
                        else
                        {
                            DropBall();
                            NextState = DROPPING_EXTRA_BALLS;//Decide what the next state will be

                            MakeTransition = True; //mark that we are taking a transition
                        }
                        // if transitioning to a state with history change kind of entry
                        EntryEventKind.EventType = ES_ENTRY;
                    }
                    // optionally, consume or re-map this event for the upper
                    // level state machine
                    ReturnEvent = CurrentEvent;
                    break;
                }
            }
            break;
        }
        case RETRIEVING_BALLS :
        {
            // Execute During function for this state. ES_ENTRY & ES_EXIT are
            // processed here allow the lower level state machines to re-map
            // or consume the event
            CurrentEvent = DuringRetrievingBalls(CurrentEvent);
            //process any events
            if ( CurrentEvent.EventType != ES_NO_EVENT ) //If an event is active
            {
                switch (CurrentEvent.EventType)
                {
                    case EV_MAX_BALLS_ACQUIRED :

                        NextState = MOVING_TO_SHOOT;//Decide what the next state will be

                        MakeTransition = True; //mark that we are taking a transition
                        // if transitioning to a state with history change kind of entry
                        EntryEventKind.EventType = ES_ENTRY;
                        // optionally, consume or re-map this event for the upper
                        // level state machine
                        ReturnEvent = CurrentEvent;
                        break;

                    case ES_TIMEOUT:
                    {
                        if (CurrentEvent.EventParam == AT_DEPOT_TIMER)
                        {
                            DriveForward(20);

                            NextState = DRIVING_FORWARD_GAMEPLAY;//Decide what the next state will be

                            MakeTransition = True; //mark that we are taking a transition
                            // if transitioning to a state with history change kind of entry
                            EntryEventKind.EventType = ES_ENTRY;
                            // optionally, consume or re-map this event for the upper
                            // level state machine
                            ReturnEvent = CurrentEvent;
                            break;
                        }
                    }
                }
            }
            break;
        }
        case MOVING_TO_SHOOT :
        {
            // Execute During function for this state. ES_ENTRY & ES_EXIT are
            // processed here allow the lower level state machines to re-map
            // or consume the event

            CurrentEvent = DuringMovingToShoot(CurrentEvent);

            //process any events
            if ( CurrentEvent.EventType != ES_NO_EVENT ) //If an event is active
            {
                switch (CurrentEvent.EventType)
                {
                    case EV_MOVE_COMPLETE:
                    {

                        NextState = SHOOTING;//Decide what the next state will be

                        MakeTransition = True; //mark that we are taking a transition
                        // if transitioning to a state with history change kind of entry
                        EntryEventKind.EventType = ES_ENTRY;
                        // optionally, consume or re-map this event for the upper
                        // level state machine
                        ReturnEvent = CurrentEvent;
                        break;
                    }
                }
            }
            break;
        }

    }
    //   If we are making a state transition
    if (MakeTransition == True)
    {
        //   Execute exit function for current state
        CurrentEvent.EventType = ES_EXIT;
        RunGameplay(CurrentEvent);

        CurrentState = NextState; //Modify state variable

        //   Execute entry function for new state

        RunGameplay(EntryEventKind);
    }
    return(ReturnEvent);
}
/****************************************************************************
 Function
     StartGameplay

 Parameters
     None

 Returns
     None

 Description
     Does any required initialization for this state machine

****************************************************************************/
void StartGameplay ( ES_Event CurrentEvent )
{
    // local variable to get debugger to display the value of CurrentEvent
    ES_Event LocalEvent = CurrentEvent;
    // to implement entry to a history state or directly to a substate
    // you can modify the initialization of the CurrentState variable
    // otherwise just start in the entry state every time the state machine
    // is started
    if ( ES_ENTRY_HISTORY != CurrentEvent.EventType )
    {
        CurrentState = SHOOTING;

    }

    // Turn querying ball light off
    LED_QUERY_BALL_PORT &= ~LED_QUERY_BALL_BIT;
    IR_QUERY_BALL_PORT &= ~IR_QUERY_BALL_BIT;

    // call the entry function (if any) for the ENTRY_STATE
    RunGameplay(CurrentEvent);
}

/****************************************************************************
 Function
     QueryGameplay

 Parameters
     None

 Returns
     GameplayState_t The current state of the state machine

 Description
     returns the current state of the state machine

****************************************************************************/
GameplayState_t QueryGameplay ( void )
{
    return(CurrentState);
}

/***************************************************************************
 private functions
 ***************************************************************************/

static ES_Event DuringShooting( ES_Event Event)
{
    ES_Event ReturnEvent = Event; // assmes no re-mapping or comsumption

    // process ES_ENTRY, ES_ENTRY_HISTORY & ES_EXIT events
    if ( (Event.EventType == ES_ENTRY) ||
            (Event.EventType == ES_ENTRY_HISTORY) )
    {
        // implement any entry actions required for this state machine

        // Initialize power plant detector
        InitPowerPlantDetector();

        // after that start any lower level machines that run in this state

        StartShooting(Event);
    }
    else if ( Event.EventType == ES_EXIT )
    {
        // on exit, give the lower levels a chance to clean up first

        RunShooting(Event);

        // now do any local exit functionality

        // Turn off shooting wheel
        TurnShooterOff();

    }
    else
        // do the 'during' function for this state
    {
        // run any lower level state machine

        ReturnEvent = RunShooting(Event);

        // do any activity that is repeated as long as we are in this state
    }
    // return either Event, if you don't want to allow the lower level machine
    // to remap the current event, or ReturnEvent if you do want to allow it.
    return(ReturnEvent);
}

static ES_Event DuringMovingToDepot( ES_Event Event)
{
    ES_Event ReturnEvent = Event; // assmes no re-mapping or comsumption

    // process ES_ENTRY, ES_ENTRY_HISTORY & ES_EXIT events
    if ( (Event.EventType == ES_ENTRY) ||
            (Event.EventType == ES_ENTRY_HISTORY) )
    {
        // implement any entry actions required for this state machine

        // after that start any lower level machines that run in this state

        StartMovingToDepot(Event);
    }
    else if ( Event.EventType == ES_EXIT )
    {
        // on exit, give the lower levels a chance to clean up first

        RunMovingToDepot(Event);

        // now do any local exit functionality
        // Bump detected, so we are at the supply depot, so stop driving
        StopDrive();

        // We have no balls, so make room for first one
        MakeSpaceForNewBall();
    }
    else
        // do the 'during' function for this state
    {
        // run any lower level state machine

        ReturnEvent = RunMovingToDepot(Event);

        // do any activity that is repeated as long as we are in this state
    }
    // return either Event, if you don't want to allow the lower level machine
    // to remap the current event, or ReturnEvent if you do want to allow it.
    return(ReturnEvent);
}

static ES_Event DuringRetrievingBalls( ES_Event Event)
{
    ES_Event ReturnEvent = Event; // assmes no re-mapping or comsumption

    // process ES_ENTRY, ES_ENTRY_HISTORY & ES_EXIT events
    if ( (Event.EventType == ES_ENTRY) ||
            (Event.EventType == ES_ENTRY_HISTORY) )
    {
        // implement any entry actions required for this state machine
        // Start a timer to timeout if we don't get balls
        ES_Timer_InitTimer(AT_DEPOT_TIMER, AT_DEPOT_TIMEOUT);

        // after that start any lower level machines that run in this state

        StartRetrievingBalls(Event);
    }
    else if ( Event.EventType == ES_EXIT )
    {
        // on exit, give the lower levels a chance to clean up first

        RunRetrievingBalls(Event);

        // now do any local exit functionality
        // Stop our ball retrieving timer
        ES_Timer_StopTimer(AT_DEPOT_TIMER);

        // Turn on shooting wheel
        TurnShooterOn();

    }
    else
        // do the 'during' function for this state
    {
        // run any lower level state machine

        ReturnEvent = RunRetrievingBalls(Event);

        // do any activity that is repeated as long as we are in this state
    }
    // return either Event, if you don't want to allow the lower level machine
    // to remap the current event, or ReturnEvent if you do want to allow it.
    return(ReturnEvent);
}

static ES_Event DuringMovingToShoot( ES_Event Event)
{
    ES_Event ReturnEvent = Event; // assmes no re-mapping or comsumption

    // process ES_ENTRY, ES_ENTRY_HISTORY & ES_EXIT events
    if ( (Event.EventType == ES_ENTRY) ||
            (Event.EventType == ES_ENTRY_HISTORY) )
    {
        // implement any entry actions required for this state machine

        DriveForward(20);

        // after that start any lower level machines that run in this state

    }
    else if ( Event.EventType == ES_EXIT )
    {
        // on exit, give the lower levels a chance to clean up first

        // now do any local exit functionality

    }
    else
        // do the 'during' function for this state
    {
        // run any lower level state machine

        // do any activity that is repeated as long as we are in this state
    }
    // return either Event, if you don't want to allow the lower level machine
    // to remap the current event, or ReturnEvent if you do want to allow it.
    return(ReturnEvent);
}

static ES_Event DuringDrivingForwardGameplay( ES_Event Event)
{
    ES_Event ReturnEvent = Event; // assmes no re-mapping or comsumption

    // process ES_ENTRY, ES_ENTRY_HISTORY & ES_EXIT events
    if ( (Event.EventType == ES_ENTRY) ||
            (Event.EventType == ES_ENTRY_HISTORY) )
    {
        // implement any entry actions required for this state machine

        // after that start any lower level machines that run in this state

    }
    else if ( Event.EventType == ES_EXIT )
    {
        // on exit, give the lower levels a chance to clean up first

        // now do any local exit functionality
        TurnShooterOn();

    }
    else
        // do the 'during' function for this state
    {
        // run any lower level state machine

        // do any activity that is repeated as long as we are in this state
    }
    // return either Event, if you don't want to allow the lower level machine
    // to remap the current event, or ReturnEvent if you do want to allow it.
    return(ReturnEvent);
}

static ES_Event DuringDroppingExtraBalls( ES_Event Event)
{
    ES_Event ReturnEvent = Event; // assmes no re-mapping or comsumption

    // process ES_ENTRY, ES_ENTRY_HISTORY & ES_EXIT events
    if ( (Event.EventType == ES_ENTRY) ||
            (Event.EventType == ES_ENTRY_HISTORY) )
    {
        // implement any entry actions required for this state machine
        ES_Timer_InitTimer(BALL_SHOT_TIMER, BALL_SHOT_TIMEOUT);

        // after that start any lower level machines that run in this state

    }
    else if ( Event.EventType == ES_EXIT )
    {
        // on exit, give the lower levels a chance to clean up first

        // now do any local exit functionality

    }
    else
        // do the 'during' function for this state
    {
        // run any lower level state machine

        // do any activity that is repeated as long as we are in this state
    }
    // return either Event, if you don't want to allow the lower level machine
    // to remap the current event, or ReturnEvent if you do want to allow it.
    return(ReturnEvent);
}

KeyMapper.c

/****************************************************************************
 Module
   KeyMapper.c

 Description
   This service maps keystrokes to events.
****************************************************************************/
/*----------------------------- Include Files -----------------------------*/
#include <stdio.h>
#include <ctype.h>
#include "ES_Configure.h"
#include "ES_Framework.h"
#include "KeyMapper.h"
#include "MasterMachine.h"
#include "GameMaster.h"
#include "FiducialVisibility.h"
#include "Gameplay.h"
#include "QueryFAC.h"
#include "Shooting.h"
#include "MovingToDepot.h"
#include "RetrievingBalls.h"
#include "ES_Timers.h"

#include "DriveMotorService.h"
#include "ShooterMotorService.h"
#include "DispenserControl.h"


/*----------------------------- Module Defines ----------------------------*/
#define KEY_MAP_TIME 1000

/*---------------------------- Module Functions ---------------------------*/
void PrintGameStates(void);


/*---------------------------- Module Variables ---------------------------*/
// with the introduction of Gen2, we need a module level Priority variable
static uint8_t MyPriority;

/*------------------------------ Module Code ------------------------------*/
/****************************************************************************
 Function
     InitKeyMapper

 Parameters
     uint8_t : the priorty of this service

 Returns
     boolean, False if error in initialization, True otherwise

 Description
     Saves away the priority, and does any
     other required initialization for this service

****************************************************************************/
boolean InitKeyMapper ( uint8_t Priority )
{
    MyPriority = Priority;

    ES_Timer_InitTimer(KEY_MAP_TIMER, KEY_MAP_TIME);

    return True;
}

/****************************************************************************
 Function
     PostKeyMapper

 Parameters
     ES_Event ThisEvent ,the event to post to the queue

 Returns
     boolean False if the Enqueue operation failed, True otherwise

 Description
     Posts an event to this state machine's queue

****************************************************************************/
boolean PostKeyMapper( ES_Event ThisEvent )
{
    return ES_PostToService( MyPriority, ThisEvent);
}


/*******************************************bbaba*********************************
 Function
    RunKeyMapper

 Parameters
   ES_Event : the event to process

 Returns
   ES_Event, ES_NO_EVENT if no error ES_ERROR otherwise

 Description
   Maps keyboard keys to events to be posted

****************************************************************************/
ES_Event RunKeyMapper( ES_Event ThisEvent )
{
    ES_Event ReturnEvent;
    ReturnEvent.EventType = ES_NO_EVENT; // assume no errors

    if ( ThisEvent.EventType == ES_TIMEOUT && ThisEvent.EventParam == KEY_MAP_TIMER)
    {
        //PrintGameStates();
        ES_Timer_InitTimer(KEY_MAP_TIMER, KEY_MAP_TIME);

    }


    if ( ThisEvent.EventType == ES_NEW_KEY) // there was a key pressed
    {
        switch ( toupper(ThisEvent.EventParam))   //converts event parameter to upper case
        {
            case 'A':
            {
                MakeSpaceForNewBall();
                break;
            }
            case 'B':
            {
                DropBall();
                break;
            }
            case 'C':
            {
                DriveManual(RIGHT, CW, 100);
                break;
            }
            case 'D':
            {
                DriveManual(RIGHT, CCW, 100);
                break;
            }
            case 'E':
            {
                SetShooterDistance(1000);
                break;
            }
            case 'F':
            {
                SetShooterDistance(2000);
                break;
            }
            case 'G':
            {
                SetShooterDistance(3000);
                break;
            }
            case 'H':
            {
                SetShooterDistance(4000);
                break;
            }
            case 'I':
            {
                SetShooterDistance(5000);
                break;
            }
            case 'J':
            {
                TurnShooterOn();
                break;
            }
            case 'K':
            {
                TurnShooterOff();
                break;
            }
            case 'L':
            {
                ES_Event TempEvent;
                TempEvent.EventType = EV_BALL_DROPPED;
                PostMasterMachine(TempEvent);
                break;
            }
            case 'M':
            {
                ES_Event TempEvent;
                TempEvent.EventType = EV_BUMP_DETECTED;
                PostMasterMachine(TempEvent);
                break;
            }
            case 'N':
            {
                ES_Event TempEvent;
                TempEvent.EventType = EV_GAME_START;
                PostMasterMachine(TempEvent);
                break;
            }
            case 'O':
            {
                ES_Event TempEvent;
                TempEvent.EventType = EV_GAME_OVER;
                PostMasterMachine(TempEvent);
                break;
            }
            case 'P':
            {
                ES_Event TempEvent;
                TempEvent.EventType = EV_MOVE_COMPLETE;
                PostMasterMachine(TempEvent);
                break;
            }
            case 'Q':
            {
                ES_Event TempEvent;
                TempEvent.EventType = EV_OUT_OF_BALLS;
                PostMasterMachine(TempEvent);
                break;
            }
            case 'R':
            {
                ES_Event TempEvent;
                TempEvent.EventType = EV_MAX_BALLS_ACQUIRED;
                PostMasterMachine(TempEvent);
                break;
            }
            case 'S':
            {
                ES_Event TempEvent;
                TempEvent.EventType = EV_IN_POSITION_TO_SHOOT;
                PostMasterMachine(TempEvent);
                break;
            }
            case 'T':
            {
                ES_Event TempEvent;
                TempEvent.EventType = EV_POWER_PLANT_DETECTED;
                PostMasterMachine(TempEvent);
                break;
            }
            case 'U':
            {
                ES_Event TempEvent;
                TempEvent.EventType = EV_OUTPUT_COMPARE;
                PostMasterMachine(TempEvent);
                break;
            }
            case 'V':
            {
                ES_Event TempEvent;
                TempEvent.EventType = ES_TIMEOUT;
                TempEvent.EventParam = FAC_TIMER;
                PostMasterMachine(TempEvent);
                break;
            }
            case 'W':
            {
                ES_Event TempEvent;
                TempEvent.EventType = ES_TIMEOUT;
                TempEvent.EventParam = DRIVE_AD_TIMER;
                PostMasterMachine(TempEvent);
                break;
            }
            case 'X':
            {
                ES_Event TempEvent;
                TempEvent.EventType = ES_TIMEOUT;
                TempEvent.EventParam = BALL_TIMER;
                PostMasterMachine(TempEvent);
                break;
            }
            case 'Y':
            {
                ES_Event TempEvent;
                TempEvent.EventType = EV_BALL_SHOT;
                PostMasterMachine(TempEvent);
                break;
            }
            case 'Z':
            {
                ES_Event TempEvent;
                TempEvent.EventType = EV_OUT_OF_BALLS;
                PostMasterMachine(TempEvent);
                break;
            }

        }

    }

    return ReturnEvent;
}


void PrintGameStates(void)
{

    static GameMasterState_t GameMasterState;
    static GameplayState_t GameplayState;
    static QueryFAC_State_t QueryFACState;
    static FiducialVisibilityState_t FiducialState;
    static ShootingState_t ShootingState;
    static MovingToDepotState_t MovingToDepotState;
    static RetrievingBallsState_t RetrievingBallsState;
    static Drive_State_t DriveState;
    static unsigned long int firstCall = 0;

    while(firstCall++ == 0)
    {
        printf("GameMaster \tGamePlay \t\tFACState \tFiducial \tShooting \t\tMovingToDepot \t\tRetrieveBalls \t\tDriveState\n\r");
    }


    GameMasterState = QueryGameMaster();
    switch(GameMasterState)
    {
        case WAITING:
            printf("Waiting \t");
            break;
        case PLAYING:
            printf("Playing \t");
            break;
    }

    GameplayState = QueryGameplay();
    switch(GameplayState)
    {
        case SHOOTING:
            printf("Shooting \t\t");
            break;
        case RETRIEVING_BALLS:
            printf("Retrieving Balls \t");
            break;
        case MOVING_TO_SHOOT:
            printf("Moving to Shoot \t");
            break;
        case MOVING_TO_DEPOT:
            printf("Moving to Depot \t");
            break;
    }


    QueryFACState = QueryQueryFAC();
    switch(QueryFACState)
    {
        case FAC_WAITING:
            printf("Waiting \t");
            break;
        case FAC_QUERYING:
            printf("Querying \t");
            break;
    }

    FiducialState = QueryFiducialVisibility();
    switch(FiducialState)
    {
        case VISIBLE:
            printf("Visible \t");
            break;
        case INVISIBLE:
            printf("Invisible \t");
            break;
    }


    ShootingState = QueryShooting();
    switch(ShootingState)
    {
        case ALIGNING:
            printf("Aligning \t\t");
            break;
        case WAITING_FOR_DISPENSER:
            printf("Waiting to Dispenser \t");
            break;
    }


    MovingToDepotState = QueryMovingToDepot();
    switch(MovingToDepotState)
    {
        case TURNING_TOWARDS_CENTER_TAPE:
            printf("Turning towards Center \t");
            break;
        case MOVING_TOWARDS_CENTER_TAPE:
            printf("Moving towards Center \t");
            break;
        case TURNING_TOWARDS_DEPOT:
            printf("Turning toward depot \t");
            break;
        case MOVING_BACKWARD:
            printf("Moving Backward \t");
            break;
    }


    RetrievingBallsState = QueryRetrievingBalls();
    switch(RetrievingBallsState)
    {
        case PULSING:
            printf("Pulsing \t\t");
            break;
        case WAITING_FOR_BALL:
            printf("Waiting for Ball \t");
            break;
    }

    DriveState = QueryDriveMotorService();
    switch(DriveState)
    {
        case TURNING_CW:
            printf("Turning CW \t\t");
            break;
        case TURNING_CCW:
            printf("Turning CCW \t\t");
            break;
        case DRIVING_FORWARD:
            printf("Driving Forward \t");
            break;
        case DRIVING_BACKWARD:
            printf("Driving Backward \t");
            break;
        case FINDING_POWER_CW:
            printf("Finding Power CW \t");
            break;
        case FINDING_POWER_CCW:
            printf("Finding Power CCW \t");
            break;
        case MANUALLY_DRIVEN:
            printf("Manually Driven \t");
            break;
    }

    printf("\r");


}

MasterMachine.c

/****************************************************************************
 Module
   MasterMachine.c

 Description
   This is a template for the top level master machine

****************************************************************************/
/*----------------------------- Include Files -----------------------------*/
/* include header files for this state machine as well as any machines at the
   next lower level in the hierarchy that are sub-machines to this machine
*/
#include "ES_Configure.h"
#include "ES_Framework.h"
#include "MasterMachine.h"
#include "Const.h"
#include "GameMaster.h"
#include "QueryFAC.h"
#include "ShooterMotorService.h"
#include "DriveMotorService.h"
#include "FACDriveService.h"
#include "DispenserControl.h"
#include "FiducialCover.h"
#include "Servo.h"
#include "TapeSensors.h"
#include <stdio.h>

/*----------------------------- Module Defines ----------------------------*/

/*---------------------------- Module Functions ---------------------------*/
static ES_Event DuringMaster( ES_Event Event);

/*---------------------------- Module Variables ---------------------------*/
// everybody needs a state variable, though if the top level state machine
// is just a single state container for orthogonal regions, you could get
// away without it
static MasterMachineState_t CurrentState;
// with the introduction of Gen2, we need a module level Priority var as well
static uint8_t MyPriority;

static unsigned char GameTimerTimeouts = 0;

/*------------------------------ Module Code ------------------------------*/
/****************************************************************************
 Function
     InitMasterMachine

 Parameters
     uint8_t Priority
         Priority of the queue

 Returns
     boolean
         False if error in initialization
         True otherwise

 Description
     Saves away the priority,  and starts
     the top level state machine

****************************************************************************/
boolean InitMasterMachine ( uint8_t Priority )
{
    ES_Event ThisEvent;

    MyPriority = Priority;  // save our priority

    //Initialize All Helper Modules Not In State Machine
    InitShooterMotorService();
    InitDispenserControl();
    InitFiducialCover();
    InitTapeSensors();

    // Set LED lines to outputs
    LED_QUERY_BALL_IO |= LED_QUERY_BALL_BIT;
    IR_QUERY_BALL_IO |= IR_QUERY_BALL_BIT;

    ThisEvent.EventType = ES_ENTRY;

    // Start the Master State machine
    StartMasterMachine( ThisEvent );

    return True;
}

/****************************************************************************
 Function
     PostMasterMachine

 Parameters
     ES_Event ThisEvent
         The event to post to the queue

 Returns
     boolean
         False if the post operation failed
         True otherwise

 Description
     Posts an event to this state machine's queue

****************************************************************************/
boolean PostMasterMachine( ES_Event ThisEvent )
{
    return ES_PostToService( MyPriority, ThisEvent);
}

/****************************************************************************
 Function
    RunMasterMachine

 Parameters
   ES_Event CurrentEvent
       The event to process

 Returns
   ES_Event
       An event to return

 Description
   the run function for the top level state machine

****************************************************************************/
// make recursive call warning into info
#pragma MESSAGE INFORMATION C1855

ES_Event RunMasterMachine( ES_Event CurrentEvent )
{

    // start by passing events to the lower level machines
    // this is the 'during' function for this machine
    CurrentEvent = DuringMaster(CurrentEvent);

    // there is only 1 state at this level and it has 2 concurrent regions
    // running the QueryFAC & GameMaster state machines

    if (CurrentEvent.EventType == EV_GAME_OVER)
    {
        FAC_DriveStop();
        TurnShooterOff();
    }

    // in the absence of an error the top level state machine should
    // always return ES_NO_EVENT
    CurrentEvent.EventType = ES_NO_EVENT;
    return(CurrentEvent);
}
/****************************************************************************
 Function
     StartMasterMachine

 Parameters
     ES_Event CurrentEvent

 Returns
     nothing

 Description
     Does any required initialization for this state machine
****************************************************************************/
void StartMasterMachine ( ES_Event CurrentEvent )
{
    // local variable to get debugger to display the value of CurrentEvent
    volatile ES_Event LocalEvent = CurrentEvent;

    CurrentState = RUNNING;

    // now we need to let the Run function init the lower level state machines
    // use LocalEvent to keep the compiler from complaining about unused var
    RunMasterMachine(LocalEvent);
    return;
}


/***************************************************************************
 private functions
 ***************************************************************************/

static ES_Event DuringMaster( ES_Event Event )
{
    // process ES_ENTRY & ES_EXIT events
    if ( Event.EventType == ES_ENTRY)
    {
        // implement any entry actions required for this state machine

        // after that start any lower level machines that run in this state
        StartGameMaster(Event);
        StartQueryFAC(Event);
    }
    else if ( Event.EventType == ES_EXIT)
    {
        // on exit, give the lower levels a chance to clean up first
        RunGameMaster(Event);
        RunQueryFAC(Event);

        // do any exit actions
    }
    else
        // do the 'during' function for this state
    {
        // Remap game timer timeouts to no event or game over
        switch (Event.EventType)
        {
            case ES_TIMEOUT:
            {
                if (Event.EventParam == GAME_TIMER)
                {
                    // Need to reset timer to reach 2 minutes
                    if (GameTimerTimeouts == 0)
                    {
                        ES_Timer_InitTimer(GAME_TIMER, 51652);
                        Event.EventType = ES_NO_EVENT;
                        GameTimerTimeouts++;
                    }
                    else if (GameTimerTimeouts == 1)
                    {
                        Event.EventType = EV_GAME_OVER;
                        GameTimerTimeouts = 0;
                    }
                }
                break;
            }
            case EV_GAME_START:
            {
                // Start 2 min timer for game play
                ES_Timer_InitTimer(GAME_TIMER, 65535);
                break;
            }
        }
        // run any lower level state machine
        RunGameMaster(Event);
        RunQueryFAC(Event);
    }
    return(Event);
}

MovingToDepot.c

/****************************************************************************
 Module
   MovingToDepot.c

 Description
   This is a file for implementing moving to depot
****************************************************************************/
/*----------------------------- Include Files -----------------------------*/
// Basic includes for a program using the Events and Services Framework
#include "ES_Configure.h"
#include "ES_Framework.h"
#include "Const.h"
#include "TapeSensors.h"
#include "QueryFAC.h"
#include "DriveMotorService.h"
#include "PositionLib.h"
#include "MasterMachine.h"
#include "TapeSensors.h"
#include <math.h>
#include <stdio.h>
#include "FACDriveService.h"

/* include header files for this state machine as well as any machines at the
   next lower level in the hierarchy that are sub-machines to this machine
*/
#include "MovingToDepot.h"

/*----------------------------- Module Defines ----------------------------*/
#define RED_CENTER_X    20 //inches  
#define RED_CENTER_Y    48

#define BLUE_CENTER_X   73
#define BLUE_CENTER_Y   48

/*---------------------------- Module Functions ---------------------------*/
ES_Event DuringTurningTowardsTape(ES_Event Event);
ES_Event DuringTapeSensing(ES_Event Event);
ES_Event DuringTurningToDepot(ES_Event Event);
ES_Event DuringBackingToDepot(ES_Event Event);

/*---------------------------- Module Variables ---------------------------*/
// everybody needs a state variable, you may need others as well
static MovingToDepotState_t CurrentState;
static unsigned int DesiredX;
static unsigned int DesiredY;

/*------------------------------ Module Code ------------------------------*/
/****************************************************************************
 Function
    RunMovingToDepot

 Parameters
   ES_Event: the event to process

 Returns
   ES_Event: an event to return

 Description
   Runs through the state machine of moving to the depot.
****************************************************************************/
// make recursive call warning into info
#pragma MESSAGE INFORMATION C1855

ES_Event RunMovingToDepot( ES_Event CurrentEvent )
{
    unsigned char MakeTransition = False;/* are we making a state transition? */
    MovingToDepotState_t NextState = CurrentState;
    ES_Event EntryEventKind = { ES_ENTRY, 0 };// default to normal entry to new state
    ES_Event ReturnEvent = CurrentEvent; // assume we are not consuming event


    switch ( CurrentState )
    {
        case TURNING_TOWARDS_TAPE:
            // Execute During function for this state. ES_ENTRY & ES_EXIT are
            // processed here allow the lower level state machines to re-map
            // or consume the event
            CurrentEvent = DuringTurningTowardsTape(CurrentEvent);
            //process any events
            if ( CurrentEvent.EventType != ES_NO_EVENT ) //If an event is active
            {
                switch (CurrentEvent.EventType)
                {
                    case EV_MOVE_COMPLETE :

                        if (CurrentEvent.EventParam == FAC_MOTOR_MOVE_POST_PARAM)
                        {

                            NextState = TAPE_SENSING;//Decide what the next state will be

                            MakeTransition = True; //mark that we are taking a transition
                            // if transitioning to a state with history change kind of entry
                            EntryEventKind.EventType = ES_ENTRY;
                            // optionally, consume or re-map this event for the upper
                            // level state machine
                        }

                        ReturnEvent = CurrentEvent;
                        break;
                }
            }
            break;

        case TAPE_SENSING:
            // Execute During function for this state. ES_ENTRY & ES_EXIT are
            // processed here allow the lower level state machines to re-map
            // or consume the event
            CurrentEvent = DuringTapeSensing(CurrentEvent);
            //process any events
            if ( CurrentEvent.EventType != ES_NO_EVENT ) //If an event is active
            {
                switch (CurrentEvent.EventType)
                {
                    case EV_MOVE_COMPLETE :
                        if (CurrentEvent.EventParam == FAC_MOTOR_MOVE_POST_PARAM)
                        {

                            NextState = TURNING_TO_DEPOT;//Decide what the next state will be

                            MakeTransition = True; //mark that we are taking a transition
                            // if transitioning to a state with history change kind of entry
                            EntryEventKind.EventType = ES_ENTRY;
                        }

                        // optionally, consume or re-map this event for the upper
                        // level state machine
                        ReturnEvent = CurrentEvent;
                        break;
                    case ES_TIMEOUT:
                        if (CurrentEvent.EventParam == DRIVE_AD_TIMER)
                        {
                            //Feedback into itself to restart the timer and recalibrate the drive
                            NextState = TAPE_SENSING;//Decide what the next state will be

                            MakeTransition = True; //mark that we are taking a transition
                            // if transitioning to a state with history change kind of entry
                            EntryEventKind.EventType = ES_ENTRY;
                            // optionally, consume or re-map this event for the upper
                            // level state machine
                            ReturnEvent = CurrentEvent;
                        }
                        break;
                }
            }
            break;

        case TURNING_TO_DEPOT:
            // Execute During function for this state. ES_ENTRY & ES_EXIT are
            // processed here allow the lower level state machines to re-map
            // or consume the event
            CurrentEvent = DuringTurningToDepot(CurrentEvent);
            //process any events
            if ( CurrentEvent.EventType != ES_NO_EVENT ) //If an event is active
            {
                switch (CurrentEvent.EventType)
                {
                    case EV_MOVE_COMPLETE :
                        if (CurrentEvent.EventParam == FAC_MOTOR_MOVE_POST_PARAM)
                        {

                            NextState = BACKING_TO_DEPOT;//Decide what the next state will be

                            MakeTransition = True; //mark that we are taking a transition
                            // if transitioning to a state with history change kind of entry
                            EntryEventKind.EventType = ES_ENTRY;
                            // optionally, consume or re-map this event for the upper
                            // level state machine
                        }

                        ReturnEvent = CurrentEvent;
                        break;
                }
            }
            break;

        case BACKING_TO_DEPOT:
            // Execute During function for this state. ES_ENTRY & ES_EXIT are
            // processed here allow the lower level state machines to re-map
            // or consume the event
            CurrentEvent = DuringBackingToDepot(CurrentEvent);
            //process any events
            if ( CurrentEvent.EventType != ES_NO_EVENT ) //If an event is active
            {
                switch (CurrentEvent.EventType)
                {
                }
            }
            break;
    }


    //   If we are making a state transition
    if (MakeTransition == True)
    {
        //   Execute exit function for current state
        CurrentEvent.EventType = ES_EXIT;
        RunMovingToDepot(CurrentEvent);

        CurrentState = NextState; //Modify state variable

        //   Execute entry function for new state

        RunMovingToDepot(EntryEventKind);
    }
    return(ReturnEvent);
}
/****************************************************************************
 Function
     StartMovingToDepot

 Parameters
     None

 Returns
     None

 Description
     Does any required initialization for this Moving To Depot State Machine
****************************************************************************/
void StartMovingToDepot ( ES_Event CurrentEvent )
{
    // local variable to get debugger to display the value of CurrentEvent
    ES_Event LocalEvent = CurrentEvent;

    // to implement entry to a history state or directly to a substate
    // you can modify the initialization of the CurrentState variable
    // otherwise just start in the entry state every time the state machine
    // is started
    if ( ES_ENTRY_HISTORY != CurrentEvent.EventType )
    {
        CurrentState = TAPE_SENSING;
    }
    // call the entry function (if any) for the ENTRY_STATE

    switch (GetColor(Self))
    {
        case TEAM_COLOR_RED:
            DesiredX = RED_CENTER_X;
            DesiredY = RED_CENTER_Y;
            break;

        case TEAM_COLOR_BLUE:
            DesiredX = BLUE_CENTER_X;
            DesiredY = BLUE_CENTER_Y;
            break;
        default:

            break;
    }

    RunMovingToDepot(CurrentEvent);
}

/****************************************************************************
 Function
     QueryMovingToDepot

 Parameters
     None

 Returns
     MovingToDepotState_t The current state of the Moving to Depot State Machine

 Description
     returns the current state of the moving to depot state machine
****************************************************************************/
MovingToDepotState_t QueryMovingToDepot ( void )
{
    return(CurrentState);
}

/***************************************************************************
 private functions
 ***************************************************************************/

ES_Event DuringTurningTowardsTape(ES_Event Event)
{
    ES_Event ReturnEvent = Event; // assmes no re-mapping or comsumption

    // process ES_ENTRY, ES_ENTRY_HISTORY & ES_EXIT events
    if ( (Event.EventType == ES_ENTRY) ||
            (Event.EventType == ES_ENTRY_HISTORY) )
    {

        FAC_Turn(DesiredX, DesiredY);

        //No Lower Level State Machines to start
    }
    else if ( Event.EventType == ES_EXIT )
    {
        //No Lower Level State Machine to Exit

        //No exit actions in this state
    }
    else
        // do the 'during' function for this state
    {
        //No Lower Level State machine to run

        //All action taken in entry
    }
    // return either Event, if you don't want to allow the lower level machine
    // to remap the current event, or ReturnEvent if you do want to allow it.
    return(ReturnEvent);
}


ES_Event DuringTapeSensing(ES_Event Event)
{
    ES_Event ReturnEvent = Event; // assmes no re-mapping or comsumption

    // process ES_ENTRY, ES_ENTRY_HISTORY & ES_EXIT events
    if ( (Event.EventType == ES_ENTRY) ||
            (Event.EventType == ES_ENTRY_HISTORY) )
    {

        FAC_DriveBackward(DesiredX, DesiredY);

        //No Lower Level State Machines to start
    }
    else if ( Event.EventType == ES_EXIT )
    {
        //No Lower Level State Machine to Exit

        //No exit actions in this state
    }
    else
        // do the 'during' function for this state
    {
        //No Lower Level State machine to run

        //All action taken in entry
    }
    // return either Event, if you don't want to allow the lower level machine
    // to remap the current event, or ReturnEvent if you do want to allow it.
    return(ReturnEvent);
}


ES_Event DuringTurningToDepot(ES_Event Event)
{
    ES_Event ReturnEvent = Event; // assmes no re-mapping or comsumption

    // process ES_ENTRY, ES_ENTRY_HISTORY & ES_EXIT events
    if ( (Event.EventType == ES_ENTRY) ||
            (Event.EventType == ES_ENTRY_HISTORY) )
    {

        //FAC_Turn(96-DesiredX, DesiredY);
        FAC_Turn(96 - DesiredX, DesiredY - 1.5 * ((float)DesiredY - GetY(Self)));

        //No Lower Level State Machines to start
    }
    else if ( Event.EventType == ES_EXIT )
    {
        //No Lower Level State Machine to Exit

        //No exit actions in this state
    }
    else
        // do the 'during' function for this state
    {
        //No Lower Level State machine to run

        //All action taken in entry
    }
    // return either Event, if you don't want to allow the lower level machine
    // to remap the current event, or ReturnEvent if you do want to allow it.
    return(ReturnEvent);
}


ES_Event DuringBackingToDepot(ES_Event Event)
{
    ES_Event ReturnEvent = Event; // assmes no re-mapping or comsumption

    // process ES_ENTRY, ES_ENTRY_HISTORY & ES_EXIT events
    if ( (Event.EventType == ES_ENTRY) ||
            (Event.EventType == ES_ENTRY_HISTORY) )
    {
        // Drive backward more than it needs to until it hits bump sensor
        DriveManual(LEFT, CCW, 100);
        DriveManual(RIGHT, CCW, 100);

        //No Lower Level State Machines to start
    }
    else if ( Event.EventType == ES_EXIT )
    {
        //No Lower Level State Machine to Exit

        // Exit actions in this state
        //StopDrive();

    }
    else
        // do the 'during' function for this state
    {
        //No Lower Level State machine to run

        //All action taken in entry
    }
    // return either Event, if you don't want to allow the lower level machine
    // to remap the current event, or ReturnEvent if you do want to allow it.
    return(ReturnEvent);
}

PositionLib.c

/****************************************************************************
 Module
     PositionLib.c

 Description
     This is a module to provide position helper functions

     Note: coordinate system flipped so that
         (0,0) is in bottom left and 0 degrees is pointing right
         ie, standard math coordinates/notation

****************************************************************************/

#include <math.h>

#define PI 3.1415926

/****************************************************************************
 Function
    CalculateAngle

 Parameters
    float x_self
    float y_self
    float x_other
    float y_other

 Returns
    signed int
        Angle other object makes from 0 with self

****************************************************************************/
signed int CalculateAngle(float x_self, float y_self, float x_other, float y_other)
{
    return (signed int)(atan2(y_other - y_self, x_other - x_self) * 180 / PI);
}

/****************************************************************************
 Function
    CalculateDeltaAngle

 Parameters
    float x_self
    float y_self
    float x_other
    float y_other

 Returns
    signed int
        Angle from self to other object

****************************************************************************/
signed int CalculateDeltaAngle(float x_self, float y_self, unsigned int angle_self, float x_other, float y_other)
{
    signed int Angle = (signed int)(atan2(y_other - y_self, x_other - x_self) * 180 / PI);
    Angle -= angle_self;
    if (Angle < -180)
    {
        Angle += 360;
    }
    else if (Angle > 180)
    {
        Angle -= 360;
    }
    return Angle;
}

/****************************************************************************
 Function
    CalculateDistance

 Parameters
    float x_self
    float y_self
    float x_other
    float y_other

 Returns
    float
        Distance between the two points

****************************************************************************/
float CalculateDistance(float x_self, float y_self, float x_other, float y_other)
{
    return (float)sqrt((x_self - x_other) * (x_self - x_other) + (y_self - y_other) * (y_self - y_other));
}

/****************************************************************************
 Function
    CalculatePowerPlantDistance

 Parameters
    float x_self
    float y_self
    unsigned int angle_self

 Returns
    float
        Distance to the power plant
        Assumes facing power plant already

****************************************************************************/
float CalculatePowerPlantDistance(float x_self, float y_self, unsigned int angle_self)
{
    float deltaX = 0;
    float deltaY = 0;

    if (x_self < 48)
    {
        deltaX = 96 - x_self;
    }
    else
    {
        deltaX = x_self;
    }

    deltaY = deltaX * tan(angle_self * (float)PI / 180);

    return (float)sqrt(deltaX*deltaX + deltaY*deltaY);
}

PWM.c

#undef TEST
/****************************************************************************
 Module
     PWM.c

 Description
     This is a module to do PWM on any channel for the C32

     This gives 1% resolution for each channel

****************************************************************************/

/*----------------------------- Include Files -----------------------------*/
#include <hidef.h>
#include <mc9s12e128.h>
#include <S12e128bits.h>
#include <Bin_Const.h>
#include "PWM.h"
#include <stdio.h>

/*----------------------------- Module Defines ----------------------------*/

/*------------------------------ Module Types -----------------------------*/

/*---------------------------- Module Functions ---------------------------*/

/*---------------------------- Module Variables ---------------------------*/

static unsigned char* const DutyRegisters[] =
{&PWMDTY0, &PWMDTY1, &PWMDTY2, &PWMDTY3, &PWMDTY4, &PWMDTY5};

static unsigned char* const PeriodRegisters[] =
{&PWMPER0, &PWMPER1, &PWMPER2, &PWMPER3, &PWMPER4, &PWMPER5};

static unsigned char* const ScaleClockRegisters[] =
{&PWMSCLA, &PWMSCLB};

static unsigned char PrescaleSelectBits[2][3] =
{
    {_S12_PCKA0, _S12_PCKA1, _S12_PCKA2},
    {_S12_PCKB0, _S12_PCKB1, _S12_PCKB2}
};

static unsigned char PWME_Bits[] =
{_S12_PWME0, _S12_PWME1, _S12_PWME2, _S12_PWME3, _S12_PWME4, _S12_PWME5};

static unsigned char PWMPOL_Bits[] =
{_S12_PPOL0, _S12_PPOL1, _S12_PPOL2, _S12_PPOL3, _S12_PPOL4, _S12_PPOL5};

static unsigned char PWMCLK_Bits[] =
{_S12_PCLK0, _S12_PCLK1, _S12_PCLK2, _S12_PCLK3, _S12_PCLK4, _S12_PCLK5};

static unsigned char MODRR_Bits[] =
{
    _S12_MODRR0, _S12_MODRR1, _S12_MODRR2,
    _S12_MODRR3, _S12_MODRR4, _S12_MODRR5
};

/*------------------------------ Module Code ------------------------------*/
/****************************************************************************
Function
    PWM_Init

Parameters
    unsigned char Channel

Returns
    PWM_Status_t
        PWM_OK if initialized properly
        PWM_ERROR otherwise

Description
    Initializes the PWM hardware subsystem to 500Hz, 1% resolution, 0% DC
    for the given channel

****************************************************************************/
PWM_Status_t PWM_Init(unsigned char Channel)
{
    PWM_Status_t ReturnStatus = PWM_OK;

    if (Channel >= MAX_CHANNELS)
    {
        return PWM_ERROR;
    }

    // Set PWM initially high so duty cycle is active cycle
    PWMPOL |= PWMPOL_Bits[Channel];
    // Set pin to PWM
    MODRR |= MODRR_Bits[Channel];
    // Enable PWM line
    PWME |= PWME_Bits[Channel];

    // Set period to 100
    *PeriodRegisters[Channel] = 100;

    // Set frequency to 500Hz
    ReturnStatus |= PWM_Set_Frequency(Channel, 500);

    // Set duty cycle to 0
    ReturnStatus |= PWM_Set_Duty_Cycle(Channel, 0);

    return ReturnStatus;
}

/****************************************************************************
Function
    PWM_Set_Frequency

Parameters
    unsigned char Channel
    unsigned long Frequency

Returns
    PWM_Status_t
        PWM_OK if frequency was changed
        PWM_ERROR otherwise

Description
    Changes the frequency for the group of PWM channels
    ** Channels 0,1,4,5 and channels 2,3 are tied together **
    ** Changing one channel changes the frequency for the others **

****************************************************************************/
PWM_Status_t PWM_Set_Frequency(unsigned char Channel, unsigned long Frequency)
{
    unsigned char i;
    static unsigned long MAX_FREQUENCY = 240000;

    unsigned char Group = ((Channel & BIT1HI) != 0);
    unsigned char ChannelsInGroup[2][4] = {{0, 1, 4, 5}, {2, 2, 3, 3}};
    unsigned char Prescaler = 1;
    unsigned int Scaler = 1;

    if (Channel >= MAX_CHANNELS)
    {
        return PWM_ERROR;
    }

    // Prescaler*(2*Scaler)*Freq = 240000
    //     Prescaler*Scaler*Freq = 120000
    // Prescaler*Freq = 240000

    // Check if frequency is possible
    if (Frequency < 4 || Frequency > MAX_FREQUENCY ||
            (MAX_FREQUENCY / Frequency)*Frequency != MAX_FREQUENCY)
    {
        return PWM_ERROR;
    }
    Scaler = (unsigned char)((MAX_FREQUENCY / 2) / Frequency);
    if (Scaler == 0)
    {
        // Use prescaled clock only
        for (i = 0; i < sizeof(ChannelsInGroup[Group]) / sizeof(ChannelsInGroup[Group][0]); i++)
        {
            PWMCLK &= ~PWMCLK_Bits[ChannelsInGroup[Group][i]];
        }

        // Set prescale clock to 1
        PWMPRCLK &= ~(PrescaleSelectBits[Group][0] |
                      PrescaleSelectBits[Group][1] | PrescaleSelectBits[Group][2]);
    }
    else
    {
        int i;

        // Calculate prescaler and scaler
        Scaler = (unsigned int)((MAX_FREQUENCY / 2) / Frequency);

        while (Scaler > 256)
        {
            Prescaler *= 2;
            Scaler /= 2;
        }
        if (Prescaler * Scaler * Frequency != MAX_FREQUENCY / 2)
        {
            return PWM_ERROR;
        }

        // Account for special case if Scaler is 256
        if (Scaler == 256)
        {
            Scaler = 0;
        }

        // Use scaled clock
        for (i = 0; i < sizeof(ChannelsInGroup[Group]) / sizeof(ChannelsInGroup[Group][0]); i++)
        {
            PWMCLK |= PWMCLK_Bits[ChannelsInGroup[Group][i]];
        }
        // Set prescale clock
        if (Prescaler >= 16)
        {
            PWMPRCLK |= PrescaleSelectBits[Group][2];
            Prescaler /= 16;
        }
        else
        {
            PWMPRCLK &= ~PrescaleSelectBits[Group][2];
        }
        if (Prescaler >= 4)
        {
            PWMPRCLK |= PrescaleSelectBits[Group][1];
            Prescaler /= 4;
        }
        else
        {
            PWMPRCLK &= ~PrescaleSelectBits[Group][1];
        }
        if (Prescaler >= 2)
        {
            PWMPRCLK |= PrescaleSelectBits[Group][0];
        }
        else
        {
            PWMPRCLK &= ~PrescaleSelectBits[Group][0];
        }
        // Set scale clock
        *ScaleClockRegisters[Group] = (unsigned char)Scaler;
    }

    return PWM_OK;
}

/****************************************************************************
Function
    PWM_Set_Duty_Cycle

Parameters
    unsigned char Channel
    unsigned char DC

Returns
    PWM_Status_t
        PWM_OK if duty cycle was changed
        PWM_ERROR otherwise

Description
    Changes the duty cycle of the given channel

****************************************************************************/
PWM_Status_t PWM_Set_Duty_Cycle(unsigned char Channel, unsigned char DC)
{
    if (Channel >= MAX_CHANNELS || DC > 100)
    {
        return PWM_ERROR;
    }

    *DutyRegisters[Channel] = DC;
    //printf("Duty Cycle = %d \n\r",DC);

    return PWM_OK;
}


/****************************************************************************
Function
    PWM_Set_Polarity

Parameters
    unsigned char Channel
    PWM_Polarity_t Polarity
        If Polarity is high, active period is duty cycle

Returns
    PWM_Status_t
        PWM_OK if polarity was set
        PWM_ERROR otherwise

Description
    Changes the duty cycle of the given channel

****************************************************************************/
PWM_Status_t PWM_Set_Polarity(unsigned char Channel, PWM_Polarity_t Polarity)
{
    if (Channel >= MAX_CHANNELS)
    {
        return PWM_ERROR;
    }

    if (Polarity == PWM_POLARITY_TOGGLE)
    {
        Polarity = (PWMPOL_Bits[Channel] == 0);
    }
    switch (Polarity)
    {
        case (PWM_POLARITY_LOW):
        {
            PWMPOL &= ~PWMPOL_Bits[Channel];
            break;
        }
        case (PWM_POLARITY_HIGH):
        {
            PWMPOL |= PWMPOL_Bits[Channel];
            break;
        }
    }

    return PWM_OK;
}

/****************************************************************************
Function
    PWM_Release

Parameters
    unsigned char Channel

Returns
    PWM_Status_t
        PWM_OK if channel was released
        PWM_ERROR otherwise

Description
    Releases the channel from the PWM subsystem

****************************************************************************/
PWM_Status_t PWM_Release(unsigned char Channel)
{
    if (Channel >= MAX_CHANNELS)
    {
        return PWM_ERROR;
    }

    // Release the pin from PWM
    MODRR &= ~MODRR_Bits[Channel];
    // Disable PWM
    PWME &= ~PWME_Bits[Channel];

    return PWM_OK;
}

/*---------------------------- Testing module -----------------------------*/

#ifdef TEST

#include <stdio.h>
#include <ME218_C32.h>
#include "PWM.h"

void main(void)
{
    unsigned char i;
    unsigned char f;
    unsigned char DutyCycle;
    unsigned long Frequency[] = {100, 1000, 10000};
    unsigned char TestChannel = 0;

    printf("Testing PWM Library\r\n");

    // Enable timer
    TSCR1 = _S12_TEN;
    // Set prescaler of 128 (349.53ms rollover)
    TSCR2 |= _S12_PR2 | _S12_PR1 | _S12_PR0;

    printf("Initializing (0 = OK): %u\r\n", PWM_Init(TestChannel));

    for (f = 0; f < sizeof(Frequency) / sizeof(Frequency[0]); f++)
    {
        for (DutyCycle = 0; DutyCycle <= 100; DutyCycle += 10)
        {
            printf("Frequency of %luHz, Duty Cycle of %u%%\r\n", Frequency[f], DutyCycle);
            printf("Setting frequency (0 = OK): %u\r\n", PWM_Set_Frequency(TestChannel, Frequency[f]));
            printf("Setting duty cycle (0 = OK): %u\r\n", PWM_Set_Duty_Cycle(TestChannel, DutyCycle));
            // Wait ~5 seconds
            for (i = 0; i < 16; i++)
            {
                while((TFLG2 & _S12_TOF) == 0)
                {
                }
                TFLG2 = _S12_TOF;
            }
        }
    }

    PWM_Release(TestChannel);


    /* Test PWM
    PWM_Init(0);
    PWM_Set_Frequency(0, 5000);
    PWM_Set_Duty_Cycle(0, 50);
    PWM_Init(2);
    PWM_Set_Duty_Cycle(2, 0);
    */

    /*
    while (1)
    {
       static unsigned short potValue = 0;
       static char dutyCycle;
       static long temp = 0;

       ADS12_Init("AAAAAAAA");
       potValue = ADS12_ReadADPin(7);
       dutyCycle = 30 + (unsigned char)((unsigned long)70*potValue/1024);

       printf("%u\r\n", potValue);

       PWM_Set_Duty_Cycle(0, dutyCycle);

       while (temp++ % 10000 != 0)
       {
          //
       }
    }
    */

}
#endif

/*------------------------------ End of file ------------------------------*/

QueryFAC.c

/****************************************************************************
 Module
     QueryFAC.c

 Description
     This is a module to query the FAC and store the return values

     Note: no error checking is done on ship number
         values must be between 3 and 39, inclusive
     Note: no error checking is done on team number
         values must be between 3 and 19, inclusive
     Note: coordinate system flipped so that
         (0,0) is in bottom left and 0 degrees is pointing right
         ie, standard math coordinates/notation

****************************************************************************/
/*----------------------------- Include Files -----------------------------*/
// Basic includes for a program using the Events and Services Framework
#include "ES_Configure.h"
#include "ES_Framework.h"
#include <hidef.h>
#include <mc9s12e128.h>
#include <S12e128bits.h>
#include <Bin_Const.h>
#include "S12eVec.h"
#include "Const.h"
#include "MasterMachine.h"
#include "PositionLib.h"
#include <stdio.h>

/* include header files for this state machine as well as any machines at the
   next lower level in the hierarchy that are sub-machines to this machine
*/
#include "QueryFAC.h"

/*----------------------------- Module Defines ----------------------------*/
// define constants for the states for this machine
// and any other local defines

/*---------------------------- Module Functions ---------------------------*/
/* prototypes for private functions for this machine, things like during
   functions, entry & exit functions.They should be functions relevant to the
   behavior of this state machine
*/
static ES_Event DuringWaiting(ES_Event Event);
static ES_Event DuringQuerying(ES_Event Event);

void interrupt _Vec_spi FAC_New_Byte_Resp(void);

/*---------------------------- Module Variables ---------------------------*/
// everybody needs a state variable, you may need others as well
static QueryFAC_State_t CurrentState;

static unsigned char CommandCounter = 2;
static unsigned char ByteCounter = 0;
static unsigned char isGameOn = 0;
static unsigned char OpponentNumber = 0;
static unsigned char LastKnownX[20];
static unsigned char LastKnownY[20];
static unsigned char LastKnownR[20];
static unsigned char X[40];
static unsigned char Y[40];
static unsigned char R[40];
static unsigned char PowerPlantUnderRepair_Red = 0;
static unsigned char PowerPlantUnderRepair_Blue = 0;
static unsigned char NumTimesPowerPlantUnderRepair_Red = 0;
static unsigned char NumTimesPowerPlantUnderRepair_Blue = 0;
static float InchesPerTick = 0.375;
static float DegreesPerTick = 1.40625;
static unsigned long DataCount = 0;

// Hack to wait to send game start event
static unsigned char NeedToPostStart = 0;

/*------------------------------ Module Code ------------------------------*/
/****************************************************************************
 Function
    RunQueryFAC

 Parameters
    ES_Event CurrentEvent
        The event to process

 Returns
    ES_Event
        An event to return

****************************************************************************/
// make recursive call warning into info
#pragma MESSAGE INFORMATION C1855

ES_Event RunQueryFAC(ES_Event CurrentEvent)
{
    unsigned char MakeTransition = False;/* are we making a state transition? */
    QueryFAC_State_t NextState = CurrentState;
    ES_Event EntryEventKind = {ES_ENTRY, 0};// default to normal entry to new state
    ES_Event ReturnEvent = CurrentEvent; // assume we are not consuming event

    switch (CurrentState)
    {
        case FAC_WAITING:
        {
            // Execute During function (and/or process entry and exit functions)
            CurrentEvent = DuringWaiting(CurrentEvent);
            // Process any event
            switch (CurrentEvent.EventType)
            {
                case ES_TIMEOUT:
                {
                    switch (CurrentEvent.EventParam)
                    {
                        case FAC_TIMER:
                        {
                            // Define next state
                            NextState = FAC_QUERYING;
                            // Set that transition is being made
                            MakeTransition = True;
                            // Set entry type to normal entry
                            EntryEventKind.EventType = ES_ENTRY;
                            // Set return event
                            ReturnEvent = CurrentEvent;

                            DataCount++;

                            break;
                        }
                    }
                }
            }
            break;
        }
        case FAC_QUERYING:
        {
            // Execute During function (and/or process entry and exit functions)
            CurrentEvent = DuringQuerying(CurrentEvent);
            // Process any event
            switch (CurrentEvent.EventType)
            {
                case ES_TIMEOUT:
                {
                    switch (CurrentEvent.EventParam)
                    {
                        case FAC_TIMER:
                        {
                            // If command counter >= 40, done querying
                            if (CommandCounter >= 40)
                            {
                                // Post game start if needed
                                if (NeedToPostStart)
                                {
                                    ES_Event ThisEvent;

                                    // Post game start event to master
                                    ThisEvent.EventType = EV_GAME_START;
                                    PostMasterMachine(ThisEvent);

                                    NeedToPostStart = 0;
                                }

                                // Reset command counter
                                CommandCounter = 2;
                                // Reset Byte
                                ByteCounter = 0;
                                // Define next state
                                NextState = FAC_WAITING;
                                // Set that transition is being made
                                MakeTransition = True;
                                // Set entry type to normal entry
                                EntryEventKind.EventType = ES_ENTRY;
                                // Set return event
                                ReturnEvent = CurrentEvent;
                                break;
                            }
                            else
                            {
                                // Send next byte
                                if (ByteCounter == 0)
                                {
                                    // Set SS line low
                                    SS_LINE_PORT &= ~SS_LINE_BIT;

                                    if (CommandCounter == 2)
                                    {
                                        // Send status of game command
                                        if(SPISR & _S12_SPTEF)
                                        {
                                            SPIDR = 0x3F; // Game status command
                                        }
                                    }
                                    else
                                    {
                                        // Send ship information command
                                        if(SPISR & _S12_SPTEF)
                                        {
                                            SPIDR = (0xC0 | CommandCounter);
                                        }
                                    }
                                }
                                else
                                {
                                    // Send 0 to FAC
                                    if(SPISR & _S12_SPTEF)
                                    {
                                        SPIDR = 0x00;
                                    }
                                }
                            }
                        }
                    }
                }
            }
            break;
        }
    }
    // If we are making a state transition
    if (MakeTransition == True)
    {
        // Execute exit function for current state
        CurrentEvent.EventType = ES_EXIT;
        RunQueryFAC(CurrentEvent);

        // Change to next state
        CurrentState = NextState;

        // Execute entry function for new state
        RunQueryFAC(EntryEventKind);
    }
    return(ReturnEvent);
}

/****************************************************************************
 Function
     StartQueryFAC

 Parameters
     None

 Returns
     None

 Description
     Does any required initialization for this state machine

****************************************************************************/
void StartQueryFAC(ES_Event CurrentEvent)
{
    // local variable to get debugger to display the value of CurrentEvent
    ES_Event LocalEvent = CurrentEvent;

    // Set the current state if there is no history
    if ( ES_ENTRY_HISTORY != CurrentEvent.EventType )
    {
        CurrentState = FAC_WAITING;
    }

    // Initialize SPI system
    SPIBR   |= (_S12_SPR2 | _S12_SPR1 | _S12_SPR0);
    SPIBR   |= (_S12_SPR1 | _S12_SPR0);

    //Putting communication transfer in mode 3 (idle high, 2nd pulse)
    SPICR1  |= _S12_CPOL;
    SPICR1  |= _S12_CPHA;

    //Master
    SPICR1  |= _S12_MSTR;

    // Set SS line to be set manually
    SPICR2  &= ~_S12_MODFEN;
    SPICR1  &= ~_S12_SSOE;
    SS_LINE_IO |= SS_LINE_BIT;

    // Enable interrupt for incoming byte
    SPICR1  |= _S12_SPIE;
    EnableInterrupts;

    //SPI Enable
    SPICR1 |= _S12_SPE;

    // call the entry function (if any) for the ENTRY_STATE
    RunQueryFAC(CurrentEvent);
}

/****************************************************************************
 Function
     QueryQueryFAC

 Parameters
     None

 Returns
     QueryFAC_State_t
         The current state of the state machine

****************************************************************************/
QueryFAC_State_t QueryQueryFAC(void)
{
    return(CurrentState);
}

/****************************************************************************
Function
    ResetData

Parameters
    None

Returns
    None

Description
    Resets all data back to original non-playing state

****************************************************************************/
void ResetData(void)
{
    unsigned char i;

    isGameOn = 0;
    OpponentNumber = 0;
    PowerPlantUnderRepair_Red = 0;
    PowerPlantUnderRepair_Blue = 0;
    NumTimesPowerPlantUnderRepair_Red = 0;
    NumTimesPowerPlantUnderRepair_Blue = 0;
    for (i = 0; i < 20; i++)
    {
        LastKnownX[i] = 0;
        LastKnownY[i] = 0;
        LastKnownR[i] = 0;
    }
    for (i = 0; i < 40; i++)
    {
        X[i] = 0;
        Y[i] = 0;
        R[i] = 0;
    }
}

/*------------------------------- Accessors -------------------------------*/
/****************************************************************************
Function
    IsGameOn

Parameters
    None

Returns
    unsigned char : If the game is running

Description
    Gets the team/robot number of the opponent

****************************************************************************/
unsigned char IsGameOn(void)
{
    return isGameOn;
}

/****************************************************************************
Function
    GetOpponentNumber

Parameters
    None

Returns
    unsigned char : The team number of the opponent's robot

Description
    Gets the team/robot number of the opponent

****************************************************************************/
unsigned char GetOpponentNumber(void)
{
    // Figure out opponent if not saved yet
    if (OpponentNumber < 3)
    {
        unsigned char i;

        // Calculate which bot is on field
        for (i = 3; i <= 19; i++)
        {
            if (i == Self)
            {
                continue;
            }
            // Robot is not visible if X, Y, R are all 0
            if (LastKnownX[i] != 0 || LastKnownY[i] != 0 || LastKnownR[i] != 0)
            {
                OpponentNumber = i;
                break;
            }
        }
    }
    return OpponentNumber;
}

/****************************************************************************
Function
    GetColor

Parameters
    unsigned char TeamNumber

Returns
    Robot_Color_t : Color of the team

Description
    Color is none if the robot isn't on the field

****************************************************************************/
Robot_Color_t GetColor(unsigned char TeamNumber)
{

    if (LastKnownX[TeamNumber] == 0 && LastKnownY[TeamNumber] == 0 && LastKnownR[TeamNumber] == 0)
    {
        return TEAM_COLOR_NONE;
    }
    if (LastKnownX[TeamNumber] < 128)
    {
        return TEAM_COLOR_RED;
    }
    else
    {
        return TEAM_COLOR_BLUE;
    }
}

/****************************************************************************
Function
    GetScore

Parameters
    unsigned char TeamNumber

Returns
    signed int : Number of points we are leading by

Description
    Each ship is worth FLEET_SHIP_VALUE, Each power plant hit is worth POWER_PLANT_VALUE

****************************************************************************/
signed int GetScoreDifferential(void)
{
    unsigned char i;

    // Score is Red Team - Blue Team
    signed int Score = 0;

    for (i = 20; i < 30; i++)
    {
        if (X[i] == 0 && Y[i] == 0 && R[i] == 0)
        {
            Score -= FLEET_SHIP_VALUE;
        }
    }
    for (i = 30; i < 40; i++)
    {
        if (X[i] == 0 && Y[i] == 0 && R[i] == 0)
        {
            Score += FLEET_SHIP_VALUE;
        }
    }
    Score += POWER_PLANT_VALUE * NumTimesPowerPlantUnderRepair_Blue;
    Score -= POWER_PLANT_VALUE * NumTimesPowerPlantUnderRepair_Red;

    if (GetColor(Self) == TEAM_COLOR_RED)
    {
        return Score;
    }
    else if (GetColor(Self) == TEAM_COLOR_BLUE)
    {
        return -Score;
    }
    else
    {
        // Why do we not have a color?
    }
}

/****************************************************************************
Function
    GetNearestOpponentShip

Parameters
    None

Returns
    unsigned char : Number of ship that is closest
        0 if there is no ship left to hit

Description
    Calculates closest opponent ship based on y coordinate
    that wasn't returned on the last call
    i.e. if ship 20 was returned on previous call, it won't
        be returned this call

****************************************************************************/
unsigned char GetNearestOpponentShip(void)
{
    unsigned char i;
    unsigned char First = 0;
    //unsigned char NearestY = 255;
    unsigned char NearestR = 255;
    unsigned char NearestShip = 0;
    static unsigned char LastShipReturned = 0;

    if (GetColor(Self) == TEAM_COLOR_RED)
    {
        First = 30;
    }
    else if (GetColor(Self) == TEAM_COLOR_BLUE)
    {
        First = 20;
    }
    else
    {
        // Why do we not have a color?
        //return 0;
        First = 20;
    }

    for (i = First; i < First + 10; i++)
    {
        // Don't check last returned ship
        if (i == LastShipReturned)
        {
            continue;
        }
        // If ship is on field
        if (X[i] != 0 && Y[i] != 0)
        {
            // Calculate absolute value of angle from self
            signed int deltaR = CalculateDeltaAngle(GetX(Self), GetY(Self), GetRotation(Self), GetX(i), GetY(i));
            if (deltaR < 0)
            {
                deltaR = -deltaR;
            }

            // If angle is closer than previously seen, record it
            if (deltaR < NearestR)
            {
                NearestR = (unsigned char)deltaR;
                NearestShip = i;
            }
        }
    }

    // If there is no ship and last ship is still on board
    if (NearestShip == 0 && X[LastShipReturned] != 0 && Y[LastShipReturned] != 0)
    {
        NearestShip = LastShipReturned;
    }

    LastShipReturned = NearestShip;

    return NearestShip;
}

/****************************************************************************
Function
    GetTimestamp

Parameters
    None

Returns
    unsigned long : "Time" of newest data FAC has received

****************************************************************************/
unsigned long GetTimestamp(void)
{
    return DataCount;
}

/****************************************************************************
Function
    GetX

Parameters
    unsigned char ShipNumber

Returns
    float : X coordinate in inches
        Top left corner is 0

****************************************************************************/
float GetX(unsigned char ShipNumber)
{
    return X[ShipNumber] * InchesPerTick;
}

/****************************************************************************
Function
    GetXRaw

Parameters
    unsigned char ShipNumber

Returns
    unsigned char : X coordinate in raw value from FAC
        Top left corner is 0

****************************************************************************/
unsigned char GetXRaw(unsigned char ShipNumber)
{
    return X[ShipNumber];
}

/****************************************************************************
Function
    GetLastKnownX

Parameters
    unsigned char TeamNumber

Returns
    float : X coordinate in inches
        Bottom left corner is 0

****************************************************************************/
float GetLastKnownX(unsigned char TeamNumber)
{
    return LastKnownX[TeamNumber] * InchesPerTick;
}

/****************************************************************************
Function
    GetLastKnownXRaw

Parameters
    unsigned char TeamNumber

Returns
    unsigned char : X coordinate in raw value from FAC
        Bottom left corner is 0

****************************************************************************/
unsigned char GetLastKnownXRaw(unsigned char TeamNumber)
{
    return LastKnownX[TeamNumber];
}

/****************************************************************************
Function
    GetY

Parameters
    unsigned char ShipNumber

Returns
    float : Y coordinate in inches
        Bottom left corner is 0

****************************************************************************/
float GetY(unsigned char ShipNumber)
{
    return (255 - Y[ShipNumber]) * InchesPerTick;
}

/****************************************************************************
Function
    GetYRaw

Parameters
    unsigned char ShipNumber

Returns
    unsigned char : Y coordinate in raw value from FAC
        Bottom left corner is 0

****************************************************************************/
unsigned char GetYRaw(unsigned char ShipNumber)
{
    return (255 - Y[ShipNumber]);
}

/****************************************************************************
Function
    GetLastKnownY

Parameters
    unsigned char TeamNumber

Returns
    float : Y coordinate in inches
        Bottom left corner is 0

****************************************************************************/
float GetLastKnownY(unsigned char TeamNumber)
{
    return (255 - LastKnownY[TeamNumber]) * InchesPerTick;
}

/****************************************************************************
Function
    GetLastKnownYRaw

Parameters
    unsigned char TeamNumber

Returns
    unsigned char : Y coordinate in raw value from FAC
        Bottom left corner is 0

****************************************************************************/
unsigned char GetLastKnownYRaw(unsigned char TeamNumber)
{
    return (255 - LastKnownY[TeamNumber]);
}

/****************************************************************************
Function
    GetRotation

Parameters
    unsigned char ShipNumber

Returns
    unsigned int : Degrees the ship is facing
        Pointing right is 0

****************************************************************************/
unsigned int GetRotation(unsigned char ShipNumber)
{
    signed int Degrees = (signed int)((float)R[ShipNumber] * DegreesPerTick + 90);
    if (Degrees < 0)
    {
        Degrees += 360;
    }
    if (Degrees > 360)
    {
        Degrees -= 360;
    }
    return (unsigned int)(Degrees);
}

/****************************************************************************
Function
    GetRotationRaw

Parameters
    unsigned char ShipNumber

Returns
    unsigned char : Rotation angle in raw value from FAC
        Pointing right is 0

****************************************************************************/
unsigned char GetRotationRaw(unsigned char ShipNumber)
{
    signed int Ticks = (signed int)R[ShipNumber] + 64;
    if (Ticks < 0)
    {
        Ticks += 255;
    }
    if (Ticks > 255)
    {
        Ticks -= 255;
    }
    return (unsigned char)(Ticks);
}

/****************************************************************************
Function
    GetLastKnownRotation

Parameters
    unsigned char TeamNumber

Returns
    unsigned int : Degrees the ship is facing
        Pointing right is 0

****************************************************************************/
unsigned int GetLastKnownRotation(unsigned char TeamNumber)
{
    signed int Degrees = (signed int)((float)LastKnownR[TeamNumber] * DegreesPerTick + 90);
    if (Degrees < 0)
    {
        Degrees += 360;
    }
    if (Degrees > 360)
    {
        Degrees -= 360;
    }
    return (unsigned int)(Degrees);
}

/****************************************************************************
Function
    GetLastKnownRotationRaw

Parameters
    unsigned char TeamNumber

Returns
    unsigned char : Rotation angle in raw value from FAC
        Pointing right is 0

****************************************************************************/
unsigned char GetLastKnownRotationRaw(unsigned char TeamNumber)
{
    signed int Ticks = (signed int)LastKnownR[TeamNumber] + 64;
    if (Ticks < 0)
    {
        Ticks += 255;
    }
    if (Ticks > 255)
    {
        Ticks -= 255;
    }
    return (unsigned char)(Ticks);
}






/***************************************************************************
 private functions
 ***************************************************************************/

static ES_Event DuringWaiting(ES_Event Event)
{
    // Set return event as event being passed in
    ES_Event ReturnEvent = Event;

    // process ES_ENTRY, ES_ENTRY_HISTORY & ES_EXIT events
    if ((Event.EventType == ES_ENTRY) ||
            (Event.EventType == ES_ENTRY_HISTORY))
    {
        // Do entry actions of this state
        // If game isn't running, go directly back to querying
        if (!isGameOn)
        {
            // Start timer to shortest time possible
            ES_Timer_InitTimer(FAC_TIMER, 1);
        }
        else
        {
            // Start timer
            ES_Timer_InitTimer(FAC_TIMER, TimeToWaitBetweenQuerying);
        }

        // Call entry actions of any lower state machines
    }
    else if (Event.EventType == ES_EXIT)
    {
        // Call exit actions of any lower state machines

        // Do exit actions of this state
    }
    // Execute this during this state
    else
    {
        // Run any lower level state machine and set to ReturnEvent

        // Do any activity that is repeated as long as we are in this state
    }
    return(ReturnEvent);
}

static ES_Event DuringQuerying(ES_Event Event)
{
    // Set return event as event being passed in
    ES_Event ReturnEvent = Event;

    // process ES_ENTRY, ES_ENTRY_HISTORY & ES_EXIT events
    if ((Event.EventType == ES_ENTRY) ||
            (Event.EventType == ES_ENTRY_HISTORY))
    {
        ES_Event TempEvent;

        // Do entry actions
        // Send first byte
        TempEvent.EventType = ES_TIMEOUT;
        TempEvent.EventParam = FAC_TIMER;
        RunQueryFAC(TempEvent);

        // Call entry actions of any lower state machines
    }
    else if (Event.EventType == ES_EXIT)
    {
        // Call exit actions of any lower state machines

        // Do exit actions of this state
    }
    // Execute this during this state
    else
    {
        // Run any lower level state machine and set to ReturnEvent

        // Do any activity that is repeated as long as we are in this state
    }
    return(ReturnEvent);
}

/*
 * Interrupt function for receiving byte
 */
void interrupt _Vec_spi FAC_New_Byte_Resp(void)
{
    static unsigned char Incoming = 0;
    // Read byte
    if (SPISR & _S12_SPIF)
    {
        Incoming = SPIDR;
    }
    // Store byte
    // If CommandCounter is 2
    if (CommandCounter == 2)
    {
        // Store status of game
        if (ByteCounter == 4)
        {
            // Update red power plant status
            if ((Incoming & BIT7HI) != 0)
            {
                if (PowerPlantUnderRepair_Red == 0)
                {
                    NumTimesPowerPlantUnderRepair_Red++;
                }
                PowerPlantUnderRepair_Red = 1;
            }
            else
            {
                PowerPlantUnderRepair_Red = 0;
            }
            // Update blue power plant status
            if ((Incoming & BIT6HI) != 0)
            {
                if (PowerPlantUnderRepair_Blue == 0)
                {
                    NumTimesPowerPlantUnderRepair_Blue++;
                }
                PowerPlantUnderRepair_Blue = 1;
            }
            else
            {
                PowerPlantUnderRepair_Blue = 0;
            }
            // Update game on/off status
            if (isGameOn)
            {
                if ((Incoming & 0x0F) == 0 || (Incoming & 0x0F) == 0x0B || (Incoming & 0x0F) == 0x0C)
                {
                    ES_Event ThisEvent;

                    // Post game over event to master
                    ThisEvent.EventType = EV_GAME_OVER;
                    ThisEvent.EventParam = 0;
                    if ( (((Incoming & BIT0HI) != 0) && GetColor(Self) == TEAM_COLOR_RED) ||
                            (((Incoming & BIT2HI) != 0) && GetColor(Self) == TEAM_COLOR_BLUE) )
                    {
                        ThisEvent.EventParam = 1;
                    }
                    PostMasterMachine(ThisEvent);

                    isGameOn = 0;

                    //ResetData();
                }
            }
            else
            {
                if ((Incoming & 0x0F) == 0x0A)
                {
                    NeedToPostStart = 1;
                    isGameOn = 1;
                }
            }
        }
    }
    else
    {
        // Store ship information
        switch (ByteCounter)
        {
            case 2:
            {
                // Store X
                X[CommandCounter] = Incoming;
                if (X[CommandCounter] > 0)
                {
                    LastKnownX[CommandCounter] = X[CommandCounter];
                }
                break;
            }
            case 3:
            {
                // Store Y
                Y[CommandCounter] = Incoming;
                if (Y[CommandCounter] > 0)
                {
                    LastKnownY[CommandCounter] = Y[CommandCounter];
                }
                break;
            }
            case 4:
            {
                // Store R
                R[CommandCounter] = Incoming;
                if (R[CommandCounter] > 0)
                {
                    LastKnownR[CommandCounter] = R[CommandCounter];
                }
                break;
            }
        }
    }
    // Increment byte counter
    ByteCounter++;
    // Increment command counter based on byte counter
    if (ByteCounter % 5 == 0)
    {
        ByteCounter = 0;
        // Only increment counter if game is running, want to keep checking status while game is stopped
        if (isGameOn)
        {
            // Increment counter until we get to a team that is on the field or ship that is still in play
            CommandCounter++;
        }

        // Raise SS line
        SS_LINE_PORT |= SS_LINE_BIT;

        // Start timer to wait at end of 5-byte transfer
        ES_Timer_InitTimer(FAC_TIMER, 4);
    }
    else
    {
        ES_Event TempEvent;
        TempEvent.EventType = ES_TIMEOUT;
        TempEvent.EventParam = FAC_TIMER;
        RunQueryFAC(TempEvent);
    }
}

RetrievingBalls.c

/****************************************************************************
 Module
   RetrievingBalls.c

 Description
   This is the file for Ball Retrieval
****************************************************************************/
/*----------------------------- Include Files -----------------------------*/
// Basic includes for a program using the Events and Services Framework
#include "ES_Configure.h"
#include "ES_Framework.h"
#include "Const.h"
#include <hidef.h>
#include <mc9s12e128.h>
#include <S12e128bits.h>
#include <Bin_Const.h>
#include "S12eVec.h"
#include "Const.h"
#include "PWM.h"
#include <stdio.h>
#include <math.h>
#include "MasterMachine.h"
#include "DispenserControl.h"
#include "RetrievingBalls.h"


/*----------------------------- Module Defines ----------------------------*/
#define _MS_OC                   *3000
#define PULSE_ON_PERIOD          10_MS_OC
#define PULSE_OFF_PERIOD         30_MS_OC
#define NUM_PULSES               10
#define WAIT_FOR_BALL_TIME       3000 // Milliseconds

/*---------------------------- Module Functions ---------------------------*/
/* prototypes for private functions for this machine, things like during
   functions, entry & exit functions.They should be functions relevant to the
   behavior of this state machine
*/
static ES_Event DuringPulsing( ES_Event Event);
static ES_Event DuringWaitingForBall( ES_Event Event);
void InitPulseOutputCompare(void);

/*---------------------------- Module Variables ---------------------------*/
// everybody needs a state variable, you may need others as well
static RetrievingBallsState_t CurrentState;
static int numBalls;
static unsigned char NeedToTurnServo = 0;
static unsigned char PulseSent = 0;
/*------------------------------ Module Code ------------------------------*/
/****************************************************************************
 Function
    RunRetrievingBalls

 Parameters
   ES_Event: the event to process

 Returns
   ES_Event: an event to return

****************************************************************************/
// make recursive call warning into info
#pragma MESSAGE INFORMATION C1855

ES_Event RunRetrievingBalls( ES_Event CurrentEvent )
{
    unsigned char MakeTransition = False;/* are we making a state transition? */
    RetrievingBallsState_t NextState = CurrentState;
    ES_Event EntryEventKind = { ES_ENTRY, 0 };// default to normal entry to new state
    ES_Event ReturnEvent = CurrentEvent; // assume we are not consuming event

    switch ( CurrentState )
    {
        case PULSING :

            // Execute During function for this state. ES_ENTRY & ES_EXIT are
            // processed here allow the lower level state machines to re-map
            // or consume the event
            CurrentEvent = DuringPulsing(CurrentEvent);

            //process any events
            if ( CurrentEvent.EventType != ES_NO_EVENT ) //If an event is active
            {
                switch (CurrentEvent.EventType)
                {
                    case EV_OUTPUT_COMPARE :
                    {
                        static int pulseCounter = 0;

                        //If NUM_PULSES pulses have been sent
                        if (pulseCounter == 3 * NUM_PULSES)
                        {
                            NextState = WAITING_FOR_BALL;
                            MakeTransition = True;
                            pulseCounter = 0;

                            if (NeedToTurnServo)
                            {
                                // Move servo to make room for new ball
                                MakeSpaceForNewBall();

                                NeedToTurnServo = 0;
                            }

                            PulseSent = 1;

                        }
                        else
                        {
                            // Set timer to do half period for off time
                            // Off time is longer than rollover
                            if (pulseCounter % 3 == 0)
                            {
                                TIM0_TC7 =  TIM0_TCNT + PULSE_ON_PERIOD;
                                IR_QUERY_BALL_PORT |=  IR_QUERY_BALL_BIT;

                            }
                            else
                            {
                                TIM0_TC7 =  TIM0_TCNT + (unsigned int)((unsigned long)PULSE_OFF_PERIOD / 2);
                                IR_QUERY_BALL_PORT &=  ~IR_QUERY_BALL_BIT;

                            }

                            NextState = PULSING;
                            MakeTransition = False;
                            pulseCounter++;
                        }


                        ReturnEvent = CurrentEvent;
                        break;
                    }
                }
            }
            break;


        case WAITING_FOR_BALL :
            // Execute During function for this state. ES_ENTRY & ES_EXIT are
            // processed here allow the lower level state machines to re-map
            // or consume the event
            CurrentEvent = DuringWaitingForBall(CurrentEvent);

            //process any events
            if ( CurrentEvent.EventType != ES_NO_EVENT ) //If an event is active
            {
                switch (CurrentEvent.EventType)
                {
                    case EV_BALL_DROPPED:

                        // If pulse wasn't sent, we have duplicate event
                        // Don't do anything (break out)
                        if (!PulseSent)
                        {
                            break;
                        }
                        else
                        {
                            // Clear flag for if pulse was sent
                            PulseSent = 0;
                        }

                        // Reset AT_DEPOT_TIMER to say we got a ball
                        ES_Timer_InitTimer(AT_DEPOT_TIMER, AT_DEPOT_TIMEOUT);

                        numBalls++;

                        if (numBalls >= MAX_BALLS)
                        {
                            ES_Event ThisEvent;
                            ThisEvent.EventType = EV_MAX_BALLS_ACQUIRED;
                            PostMasterMachine(ThisEvent);

                        }
                        else
                        {
                            //NextState = PULSING;
                            //MakeTransition = True;
                            //ReturnEvent = CurrentEvent;

                            NeedToTurnServo = 1;
                        }
                        break;

                    case ES_TIMEOUT:
                        if (CurrentEvent.EventParam == BALL_TIMER)
                        {
                            NextState = PULSING;
                            MakeTransition = True;

                            ReturnEvent = CurrentEvent;
                            break;
                        }
                        break;
                }
            }
            break;

    }
    //   If we are making a state transition
    if (MakeTransition == True)
    {
        //   Execute exit function for current state
        CurrentEvent.EventType = ES_EXIT;
        RunRetrievingBalls(CurrentEvent);

        CurrentState = NextState; //Modify state variable

        //   Execute entry function for new state
        //   this defaults to ES_ENTRY
        RunRetrievingBalls(EntryEventKind);
    }
    return(ReturnEvent);
}
/****************************************************************************
 Function
     StartRetrievingBalls

 Parameters
     None

 Returns
     None

 Description
     Does any required initialization for this state machine
****************************************************************************/
void StartRetrievingBalls ( ES_Event CurrentEvent )
{
    // local variable to get debugger to display the value of CurrentEvent
    ES_Event LocalEvent = CurrentEvent;
    // to implement entry to a history state or directly to a substate
    // you can modify the initialization of the CurrentState variable
    // otherwise just start in the entry state every time the state machine
    // is started
    if ( ES_ENTRY_HISTORY != CurrentEvent.EventType )
    {
        CurrentState = PULSING;
    }

    InitPulseOutputCompare();

    //Set numBalls
    numBalls = 0;


    RunRetrievingBalls(CurrentEvent);

}

/****************************************************************************
 Function
     QueryRetrievingBalls

 Parameters
     None

 Returns
     RetrievingBallsState_t The current state of the RetrievingBalls State Machine

 Description
     returns the current state of the RetrievingBalls State Machine
****************************************************************************/
RetrievingBallsState_t QueryRetrievingBalls ( void )
{
    return(CurrentState);
}

unsigned char GetNumBallsLoaded(void)
{
    return numBalls;
}

/***************************************************************************
 private functions
 ***************************************************************************/

static ES_Event DuringPulsing( ES_Event Event)
{
    ES_Event ReturnEvent = Event; // assmes no re-mapping or comsumption

    // process ES_ENTRY, ES_ENTRY_HISTORY & ES_EXIT events
    if ( (Event.EventType == ES_ENTRY) ||
            (Event.EventType == ES_ENTRY_HISTORY) )
    {

        //Reset Output Compare
        TIM0_TC7 =  TIM0_TCNT + PULSE_ON_PERIOD;

        //Entry Action : Turn LED's IO pins to outputs
        LED_QUERY_BALL_IO |= LED_QUERY_BALL_BIT;
        IR_QUERY_BALL_IO |=  IR_QUERY_BALL_BIT;
        //Turn on Marker LED
        LED_QUERY_BALL_PORT |= LED_QUERY_BALL_BIT;

        //No Lower SM
    }
    else if ( Event.EventType == ES_EXIT )
    {
        //No Lower SM

        //Exit actions: Turn off Marker LED
        LED_QUERY_BALL_PORT &= ~LED_QUERY_BALL_BIT;
    }
    else
    {
        //No Lower SM to run
    }
    return(ReturnEvent);
}

static ES_Event DuringWaitingForBall( ES_Event Event)
{
    ES_Event ReturnEvent = Event; // assmes no re-mapping or comsumption

    // process ES_ENTRY, ES_ENTRY_HISTORY & ES_EXIT events
    if ( (Event.EventType == ES_ENTRY) ||
            (Event.EventType == ES_ENTRY_HISTORY) )
    {
        //No entry actions
        ES_Timer_InitTimer(BALL_TIMER, WAIT_FOR_BALL_TIME);
        //No Lower SM
    }
    else if ( Event.EventType == ES_EXIT )
    {
        //No Lower SM
        //No exit actions
    }
    else
    {
        //No Lower SM to run
    }
    return(ReturnEvent);
}


/****************************************************************************
 Function
     InitPulseOutputCompare

 Parameters
     None

 Returns
     None

 Description
     Initializes the pulse output compare for ball querying
     Uses Output Compare on TIM0
****************************************************************************/
void InitPulseOutputCompare(void)
{
    /*----Initialize PI output compare on Timer 0, Timer 7, Port T2------*/

    TIM0_TSCR1 =     _S12_TEN;            //Turning on Timer

    // Set timer prescaler to divide by 8 (21.85ms rollover)
    // Note: This timer is shared by the servo
    /*---------------------------------------------------
       TIM0 Timing Details
       Dividing 24MHz by 8, 3MHz clock, 1 ms = 3000 ticks
    ----------------------------------------------------*/
    TIM0_TSCR2 |= (_S12_PR1 | _S12_PR0);


    TIM0_TIOS  |=    _S12_IOS7;              //Output Compare
    TIM0_TCTL1 &=    ~(_S12_OL7 | _S12_OM7); //No Pin Conected

    //Not OC on until enter the pulse state
    //TIM0_TC7   =     TIM0_TCNT + PULSE_ON_PERIOD;

    TIM0_TFLG1 =     _S12_C7F;               //Clear flag
    TIM0_TIE   |=    _S12_C7I;               //Enable local interrupt
    EnableInterrupts;

}


/****************************************************************************
 Function
     PulseLengthInterrupt

 Parameters
     None

 Returns
     None

 Description
     Output Compare to Toggle the IR pin for Ball Querying
****************************************************************************/
void interrupt _Vec_tim0ch7 PulseLengthInterrupt(void)
{
    ES_Event ThisEvent;
    ThisEvent.EventType = EV_OUTPUT_COMPARE;

    TIM0_TFLG1 =     _S12_C7F;

    RunRetrievingBalls(ThisEvent);
}

Servo.c

#undef TEST
/****************************************************************************
 Module
     Servo.c

 Description
     This is a module to send the pulse to servos on Port T (pins 0-7)
     Uses timer 0, channel 4 on E128

****************************************************************************/

/*----------------------------- Include Files -----------------------------*/
#include <hidef.h>
#include <mc9s12e128.h>
#include <S12e128bits.h>
#include <Bin_Const.h>
#include "S12eVec.h"
#include "Servo.h"
#include <stdio.h>
#include "DriveMotorService.h"
#include "ShooterMotorService.h"

/*----------------------------- Module Defines ----------------------------*/

#define _us 3

/*------------------------------ Module Types -----------------------------*/

/*---------------------------- Module Functions ---------------------------*/
void interrupt _Vec_tim0ch4 Servo_Timer_Resp(void);
void interrupt _Vec_tim0ovf Servo_Timer_Overflow_Resp(void);

/*---------------------------- Module Variables ---------------------------*/

static unsigned char Bits[8] = {BIT0HI, BIT1HI, BIT2HI, BIT3HI, BIT4HI, BIT5HI, BIT6HI, BIT7HI};
static unsigned char Enabled[SERVO_MAX_CHANNELS] = {0, 0, 0, 0, 0, 0, 0, 0};
static unsigned int Widths[SERVO_MAX_CHANNELS] = {1500, 1500, 1500, 1500, 1500, 1500, 1500, 1500};
static unsigned char Counter = 0;

/*------------------------------ Module Code ------------------------------*/
/****************************************************************************
Function
    Servo_Init

Parameters
    unsigned char Channel

Returns
    Servo_Status_t
        Servo_OK if initialized properly
        Servo_ERROR otherwise

Description
    Initializes the timer system

****************************************************************************/
Servo_Status_t Servo_Init(void)
{
    unsigned char i;

    // Set timer prescaler to divide by 8 (21.85ms rollover)
    TIM0_TSCR2 |= (_S12_PR1 | _S12_PR0);
    // Enable timer overflow interrupt
    TIM0_TSCR2 |= _S12_TOI;
    // Enable timer
    TIM0_TSCR1 |= _S12_TEN;
    // Set channel as output compare
    TIM0_TIOS |= _S12_IOS4;
    // Enable interrupt for channel
    TIM0_TIE |= _S12_C4I;
    // Clear flag
    TIM0_TFLG1 = _S12_C4F;

    // Globally enable interrupts
    EnableInterrupts;

    // Default all widths to 1500us
    for (i = 0; i < SERVO_MAX_CHANNELS; i++)
    {
        Widths[i] = 1500;
    }

    return SERVO_OK;
}

/****************************************************************************
Function
    Servo_Enable

Parameters
    unsigned char Channel

Returns
    Servo_Status_t
        Servo_OK if initialized properly
        Servo_ERROR otherwise

Description
    Enables a servo on the given channel

****************************************************************************/
Servo_Status_t Servo_Enable(unsigned char Channel)
{
    if (Channel >= SERVO_MAX_CHANNELS)
    {
        return SERVO_ERROR;
    }

    // Enable channel
    Enabled[Channel] = 1;

    // Set servo pin as output
    DDRT |= Bits[Channel];

    // Set width to center
    Servo_Set_Pulse_Width(Channel, 1500);

    printf("Channel %u turned on\r\n", Channel);

    return SERVO_OK;
}

/****************************************************************************
Function
    Servo_Set_Pulse_Width

Parameters
    unsigned char Channel
    unsigned int Width
        New pulse width in microseconds (typically 1000-2000)

Returns
    Servo_Status_t
        Servo_OK if changed successfully
        Servo_ERROR otherwise

Description
    Changes the width of the pulse of the given channel

****************************************************************************/
Servo_Status_t Servo_Set_Pulse_Width(unsigned char Channel, unsigned int Width)
{
    if (Channel >= SERVO_MAX_CHANNELS || Width == 0)
    {
        return SERVO_ERROR;
    }

    Widths[Channel] = Width;

    printf("Channel %u pulse width %u\r\n", Channel, Width);

    return SERVO_OK;
}

/****************************************************************************
Function
    Servo_Release

Parameters
    unsigned char Channel

Returns
    Servo_Status_t
        Servo_OK if channel was released
        Servo_ERROR otherwise

Description
    Releases the channel from being updated with a servo pulse

****************************************************************************/
Servo_Status_t Servo_Release(unsigned char Channel)
{
    if (Channel >= SERVO_MAX_CHANNELS)
    {
        return SERVO_ERROR;
    }

    Enabled[Channel] = 0;

    return SERVO_OK;
}


/*--------------------------- Private Functions ---------------------------*/
/*
 * Interrupt function for output compare to change servo pulse
 */
void interrupt _Vec_tim0ch4 Servo_Timer_Resp(void)
{
    // Clear flag
    TIM0_TFLG1 = _S12_C4F;

    if (Enabled[Counter])
    {
        // Set line low
        PTT &= ~Bits[Counter];
        //printf("%uL\r\n",Counter);
    }

    // Increment servo counter
    Counter++;

    if (Counter < SERVO_MAX_CHANNELS)
    {
        if (Enabled[Counter])
        {
            // Set line high
            PTT |= Bits[Counter];
            //printf("%uH\r\n",Counter);
        }

        // Set output compare for next time
        TIM0_TC4 = TIM0_TCNT + Widths[Counter] * _us;
    }
}

/*
 * Interrupt function for timer overflow
 */
void interrupt _Vec_tim0ovf Servo_Timer_Overflow_Resp(void)
{
    // Clear flag
    TIM0_TFLG2 = _S12_TOF;

    // Reset servo counter
    Counter = 0;

    // Set output compare to first width
    TIM0_TC4 = TIM0_TCNT + Widths[Counter] * _us;

    if (Enabled[Counter])
    {
        // Raise line for first servo
        PTT |= Bits[Counter];
        //printf("%uH\r\n",Counter);
    }

    //Increments the Output Compare overflow on the shooter PI control
    IncrementShooterOCOverflow();

}


/*---------------------------- Testing module -----------------------------*/

#ifdef TEST


void main(void)
{
    /*
    // Test servo library
    printf("%u\r\n",Servo_Init());
    //printf("%u\r\n",Servo_Enable(0));
    printf("%u\r\n",Servo_Enable(1));
    printf("%u\r\n",Servo_Set_Pulse_Width(1, 1800));
    while (1)
    {
        //
    }
    */

}
#endif

/*------------------------------ End of file ------------------------------*/

ShooterMotorService.c

/****************************************************************************
 Module
   ShooterMotorService.c

****************************************************************************/
/*----------------------------- Include Files -----------------------------*/

#include "ES_Configure.h"
#include "ES_Framework.h"
#include <hidef.h>
#include <mc9s12e128.h>
#include <S12e128bits.h>
#include <Bin_Const.h>
#include "S12eVec.h"
#include "Const.h"
#include "PWM.h"
#include <stdio.h>

#include "ShooterMotorService.h"
/*----------------------------- Module Defines ----------------------------*/

#define _MS_OC                   *3000
#define PERIOD                   (20_MS_OC)

#define ENCODER_TICKS_PER_REV    1
#define ENCODER_CLK_TICKS_PER_MS 1500  //Assuming Divide by 16 clock on Encoders
#define MSEC_PER_MIN             60000

//PI Control Constants
#define KP                       0.050
#define KI                       0.005

//PWM Defines
#define SHOOTER_PWM_FREQ         5000

//At 100% Duty Cycle, 5V
// Motor runs at 5217 RPM
#define MAX_RPM                  10000
#define DEFAULT_RPM              2100


/*---------------------------- Module Functions ---------------------------*/
void interrupt _Vec_tim0ch6 ShooterControlInterrupt(void);
void interrupt _Vec_tim1ch6 EncoderResponseInterrupt(void);
unsigned int CalculateRPM(void);

/*---------------------------- Module Variables ---------------------------*/
// with the introduction of Gen2, we need a module level Priority variable
static int DESIRED_RPM;
static int MEASURED_RPM;

//Overflows
static unsigned int numOverFlowsEncoders = 0;
static unsigned int numOverFlowsOC = 0;

static unsigned int encoderLoopCounter = 0;
static unsigned long lastPeriod;


/*------------------------------ Module Code ------------------------------*/
/****************************************************************************
 Function
     InitShooterMotorService

 Parameters
     None

 Returns
     None

 Description
     Saves away the priority.
     Initializes the output compare system for PI control
     Initializes input capture for encoder
     Initializes PWM drive on motor
****************************************************************************/
void InitShooterMotorService (void)
{
    /*----Initialize PI output compare on Timer 0, Timer 6, Port T2------*/

    TIM0_TSCR1 =     _S12_TEN;            //Turning on Timer

    // Set timer prescaler to divide by 8 (21.85ms rollover)
    // Note: This timer is shared by the servo
    /*---------------------------------------------------
       TIM0 Timing Details
       Dividing 24MHz by 8, 3MHz clock, 1 ms = 3000 ticks
    ----------------------------------------------------*/
    TIM0_TSCR2 |= (_S12_PR1 | _S12_PR0);


    TIM0_TIOS  |=    _S12_IOS6;              //Output Compare
    TIM0_TCTL1 &=    ~(_S12_OL6 | _S12_OM6); //No Pin Conected
    TIM0_TC6   =     TIM0_TCNT + PERIOD;     //Set first capture
    TIM0_TFLG1 =     _S12_C6F;               //Clear flag
    TIM0_TIE   |=    _S12_C6I;               //Enable local interrupt
    EnableInterrupts;


    /*-----Initialize Encoder Input Capture on Timer 1, Timer 6, Port T6----*/
    TIM1_TSCR1    =     _S12_TEN;
    /*---------------------------------------------------
       TIM2 Timing Details
       Dividing 24MHz by 16, 1.5MHz clock, 1 ms = 1500 ticks
    ----------------------------------------------------*/
    TIM1_TSCR2    =     _S12_PR2;

    TIM1_TIOS     &=    ~_S12_IOS6;  //Input Capture
    TIM1_TIE      |=     _S12_C6I;   //Enable Interrupt

    TIM1_TCTL3    |=    _S12_EDG6A;  //Capture on any rising
    TIM1_TCTL3    &=    ~_S12_EDG6B; //Flag Rising Edges

    TIM1_TSCR2    |=    _S12_TOI;  // Enable timer overflow interrupt

    TIM1_TFLG1    =     _S12_C6F;    //Clear Flag
    EnableInterrupts;                //Globally Enable Interrupts


    /*----------Initialize PWM System for shooter Motor------------------*/
    PWM_Init(WHEEL_PWM_SHOOTER_CHANNEL);
    PWM_Set_Frequency(WHEEL_PWM_SHOOTER_CHANNEL, SHOOTER_PWM_FREQ);
    PWM_Set_Polarity(WHEEL_PWM_SHOOTER_CHANNEL, PWM_POLARITY_HIGH);
    PWM_Set_Duty_Cycle(WHEEL_PWM_SHOOTER_CHANNEL, 0);

}


/****************************************************************************
 Function
      SetShooterDistance

 Parameters
      float Distance

 Returns
      None

 Description
      Changes the RPM so that the ball can hit the specified input distance.
****************************************************************************/
void SetShooterDistance( float Distance)
{
    //Mapping from distance to RPM
    DESIRED_RPM = (int)((float)33.20753924 * Distance + 1304.59166);
}


/****************************************************************************
 Function
      TurnShooterOn

 Parameters
      None

 Returns
      None

 Description
      Gets the shooter motor spinning at a default duty cycle
****************************************************************************/
void TurnShooterOn(void)
{
    DESIRED_RPM = DEFAULT_RPM;
}

/****************************************************************************
 Function
      TurnShooterOff

 Parameters
      None

 Returns
      None

 Description
      Turns the shooter motor off
****************************************************************************/
void TurnShooterOff(void)
{
    DESIRED_RPM = 0;

}

/****************************************************************************
 Function
      IncrementEncoderOverflow

 Parameters
      None

 Returns
      None

 Description
      Increments the Encoder Overflow counter
****************************************************************************/
void IncrementShooterEncoderOverflow(void)
{
    numOverFlowsEncoders++;

}

/****************************************************************************
 Function
      IncrementOCOverflow

 Parameters
      None

 Returns
      None

 Description
      Increments the OC Overflow counter
****************************************************************************/
void IncrementShooterOCOverflow(void)
{
    numOverFlowsOC++;
}


/****************************************************************************
 Function
     ShooterControlInterrupt

 Parameters
     None

 Returns
     None

 Description
     Implements PI control on the shooter motor
****************************************************************************/
void interrupt _Vec_tim0ch6 ShooterControlInterrupt(void)
{
    static float SumError = 0;
    static float RPMError;
    static int counter = 0;
    static float DutyCycle;

    TIM0_TFLG1 =     _S12_C6F;
    TIM0_TC6   +=    PERIOD;
    MEASURED_RPM = CalculateRPM();
    EnableInterrupts;

    //If 20*20ms = 400ms has passed without a tick, setting the measured RPM = 0

    if (encoderLoopCounter++ > 20)
    {
        MEASURED_RPM = 0;
    }

    if (MEASURED_RPM >= 0 && MEASURED_RPM <= MAX_RPM)
    {

        RPMError    =  DESIRED_RPM - MEASURED_RPM;
        SumError    += RPMError;

        DutyCycle = (KI * SumError + KP * RPMError);
        if (DutyCycle > 100)
        {
            DutyCycle = 100;
            SumError -= RPMError;
        }
        if (DutyCycle < 0)
        {
            DutyCycle = 0;
            SumError -= RPMError;
        }

        PWM_Set_Duty_Cycle(WHEEL_PWM_SHOOTER_CHANNEL, (unsigned char)(DutyCycle));
    }


}


/****************************************************************************
Function
  EncoderResponseInterrupt

Parameters
  None

Returns
  None

Description
  Interrupt Function: Not to be called by code
  Calculates the last period between encoder ticks
****************************************************************************/
void interrupt _Vec_tim1ch6 EncoderResponseInterrupt(void)
{
    static unsigned long currentTime = 0;
    static unsigned long lastTime = 0;

    TIM1_TFLG1 =  _S12_C6F;   //Clear Flag

    currentTime = ((unsigned long)numOverFlowsEncoders << 16) + TIM1_TC6;

    // Ignore pulses that come too fast (1500 ticks / ms)
    if ((currentTime - lastTime) < 1500 * 5)
    {
        return;
    }

    lastPeriod = currentTime - lastTime;

    lastTime = currentTime;

    encoderLoopCounter = 0;
}


/****************************************************************************
Function
  CalculateRPM

Parameters
  None

Returns
  Returns the RPM based on the lastPeriod

Description
  Returns the RPM calculated value
****************************************************************************/
unsigned int CalculateRPM(void)
{
    //last period = #ticks in last period
    //rev/min = [(clock ticks /rev) * (sec/clock ticks) * (min/sec)]^-1
    unsigned long RPM;
    RPM = (unsigned long)(MSEC_PER_MIN) * ENCODER_CLK_TICKS_PER_MS / lastPeriod;
    return (unsigned int)RPM;

}

Shooting.c

/****************************************************************************
 Module
   Shooting.c

 Description
   This is a file for implementing the Shooting state machine.
****************************************************************************/


/*----------------------------- Include Files -----------------------------*/
// Basic includes for a program using the Events and Services Framework
#include "ES_Configure.h"
#include "ES_Framework.h"
#include "Const.h"
#include "QueryFAC.h"
#include "DriveMotorService.h"
#include "ShooterMotorService.h"
#include "MasterMachine.h"
#include "PositionLib.h"
#include "DispenserControl.h"
#include "ShootingStrategy.h"
#include <stdio.h>

/* include header files for this state machine as well as any machines at the
   next lower level in the hierarchy that are sub-machines to this machine
*/
#include "Shooting.h"

/*----------------------------- Module Defines ----------------------------*/

/*---------------------------- Module Functions ---------------------------*/
static ES_Event DuringAligning( ES_Event Event);
static ES_Event DuringWaitingForDispenser( ES_Event Event);
void ExecuteShootingStrategy(void);

/*---------------------------- Module Variables ---------------------------*/
// everybody needs a state variable, you may need others as well
static ShootingState_t CurrentState;
static unsigned char numBalls;
static char nextOpponent;


/*------------------------------ Module Code ------------------------------*/
/****************************************************************************
 Function
    RunShooting

 Parameters
   ES_Event: the event to process

 Returns
   ES_Event: an event to return

 Description
   add your description here
****************************************************************************/
// make recursive call warning into info
#pragma MESSAGE INFORMATION C1855

ES_Event RunShooting( ES_Event CurrentEvent )
{
    unsigned char MakeTransition = False;/* are we making a state transition? */
    ShootingState_t NextState = CurrentState;
    ES_Event EntryEventKind = { ES_ENTRY, 0 };// default to normal entry to new state
    ES_Event ReturnEvent = CurrentEvent; // assume we are not consuming event

    switch ( CurrentState )
    {
        case ALIGNING :
            // Execute During function for this state. ES_ENTRY & ES_EXIT are
            // processed here allow the lower level state machines to re-map
            // or consume the event
            CurrentEvent = DuringAligning(CurrentEvent);
            //process any events
            if ( CurrentEvent.EventType != ES_NO_EVENT ) //If an event is active
            {
                switch (CurrentEvent.EventType)
                {
                        // Only called from key mapper event
                    case EV_POWER_PLANT_DETECTED:
                    {
                        StopDrive();

                        // Also do move complete action
                    }
                    case EV_MOVE_COMPLETE :
                    {
                        //Still need to set the shooter motor spin if we are looking for the enemy power plant
                        //This is because the distance calculation is based on the final angle of our bot which
                        //is only determined after the turn is complete.
                        Drive_State_t MotorDriveState = QueryDriveMotorService();
                        if (MotorDriveState == FINDING_POWER_CW || MotorDriveState == FINDING_POWER_CCW )
                        {
                            SetShooterDistance(CalculatePowerPlantDistance(GetX(Self), GetY(Self), GetRotation(Self)));

                        }
                        else
                        {
                            // Recalculate angle to turn (second time)
                            // Does form of control
                            //ExecuteShootingStrategy();
                        }

                        NextState = WAITING_FOR_DISPENSER;//Decide what the next state will be

                        MakeTransition = True; //mark that we are taking a transition
                        // if transitioning to a state with history change kind of entry
                        EntryEventKind.EventType = ES_ENTRY;
                        // optionally, consume or re-map this event for the upper
                        // level state machine
                        ReturnEvent = CurrentEvent;
                        break;
                    }

                }
            }
            break;

        case WAITING_FOR_DISPENSER:       // If current state is state one
            // Execute During function for this state. ES_ENTRY & ES_EXIT are
            // processed here allow the lower level state machines to re-map
            // or consume the event
            CurrentEvent = DuringWaitingForDispenser(CurrentEvent);
            //process any events
            if ( CurrentEvent.EventType != ES_NO_EVENT ) //If an event is active
            {
                switch (CurrentEvent.EventType)
                {
                    case EV_BALL_SHOT:

                        numBalls--;

                        NextState = ALIGNING;//Decide what the next state will be

                        MakeTransition = True; //mark that we are taking a transition
                        // if transitioning to a state with history change kind of entry
                        EntryEventKind.EventType = ES_ENTRY;
                        // optionally, consume or re-map this event for the upper
                        // level state machine
                        ReturnEvent = CurrentEvent;

                        if (numBalls <= 0)
                        {
                            ReturnEvent.EventType = EV_OUT_OF_BALLS;
                            PostMasterMachine(ReturnEvent);
                        }

                        break;
                }
            }
            break;
    }
    //   If we are making a state transition
    if (MakeTransition == True)
    {
        //   Execute exit function for current state
        CurrentEvent.EventType = ES_EXIT;
        RunShooting(CurrentEvent);

        CurrentState = NextState; //Modify state variable

        // Execute entry function for new state

        RunShooting(EntryEventKind);
    }
    return(ReturnEvent);
}
/****************************************************************************
 Function
     StartShooting

 Parameters
     None

 Returns
     None

 Description
     Does any required initialization for this state machine
****************************************************************************/
void StartShooting ( ES_Event CurrentEvent )
{
    // local variable to get debugger to display the value of CurrentEvent
    ES_Event LocalEvent = CurrentEvent;
    // to implement entry to a history state or directly to a substate
    // you can modify the initialization of the CurrentState variable
    // otherwise just start in the entry state every time the state machine
    // is started
    if ( ES_ENTRY_HISTORY != CurrentEvent.EventType )
    {
        CurrentState = ALIGNING;
    }
    // call the entry function (if any) for the ENTRY_STATE
    numBalls = MAX_BALLS;
    RunShooting(CurrentEvent);
}

/****************************************************************************
 Function
     QueryShooting

 Parameters
     None

 Returns
     ShootingState_t The current state of the Template state machine

 Description
     returns the current state of the Template state machine
****************************************************************************/
ShootingState_t QueryShooting ( void )
{
    return(CurrentState);
}

/****************************************************************************
 Function
     GetNumBallsLeft

 Parameters
     None

 Returns
     unsigned char : number of balls left in robot

****************************************************************************/
unsigned char GetNumBallsLeft(void)
{
    return numBalls;
}

/***************************************************************************
 private functions
 ***************************************************************************/

static ES_Event DuringAligning( ES_Event Event)
{
    ES_Event ReturnEvent = Event; // assmes no re-mapping or comsumption

    // process ES_ENTRY, ES_ENTRY_HISTORY & ES_EXIT events
    if ( (Event.EventType == ES_ENTRY) ||
            (Event.EventType == ES_ENTRY_HISTORY) )
    {

        //More Complex Strategy
        ExecuteShootingStrategy();

        //No Lower Level SM to enter
    }
    else if ( Event.EventType == ES_EXIT )
    {
        // No Lower Level SM to exit from

        // No Exit Action
    }
    else
        // do the 'during' function for this state
    {
        //No Lower Level SM to run

        // do any activity that is repeated as long as we are in this state
    }
    // return either Event, if you don't want to allow the lower level machine
    // to remap the current event, or ReturnEvent if you do want to allow it.
    return(ReturnEvent);
}


static ES_Event DuringWaitingForDispenser( ES_Event Event)
{
    ES_Event ReturnEvent = Event; // assmes no re-mapping or comsumption

    // process ES_ENTRY, ES_ENTRY_HISTORY & ES_EXIT events
    if ( (Event.EventType == ES_ENTRY) ||
            (Event.EventType == ES_ENTRY_HISTORY) )
    {

        //Entry Action: Shift Dispenser
        DropBall();

        // No Lower Level SM to start

    }
    else if ( Event.EventType == ES_EXIT )
    {
        //No Lower Level SM to Exit

        // now do any local exit functionality
    }
    else
        // do the 'during' function for this state
    {
        //No Lower Level SM

        // do any activity that is repeated as long as we are in this state
    }
    // return either Event, if you don't want to allow the lower level machine
    // to remap the current event, or ReturnEvent if you do want to allow it.
    return(ReturnEvent);
}

ShootingStrategy.c

/****************************************************************************
 Module
   ShootingStrategy.c

 Description
   This Module implements the shooting strategy
****************************************************************************/
/*----------------------------- Include Files -----------------------------*/
#include "ES_Configure.h"
#include "ES_Framework.h"
#include "Const.h"
#include "PWM.h"
#include <stdio.h>
#include "QueryFAC.h"
#include "PositionLib.h"
#include "DriveMotorService.h"
#include "ShooterMotorService.h"
#include "Shooting.h"

#include "ShootingStrategy.h"

/*----------------------------- Module Prototypes--------------------------*/
void updateOpponentPosition(void);
void updateEnemyBattleshipPosition(void);
void ShootAtEnemyPowerSupply(void);
void ShootAtEnemyBattleship(void);
void ShootAtFleetShip(void);


/*----------------------------- Module Defines ----------------------------*/
#define AVERAGE_DISTANCE_TO_POWER_PLANT 62.2494

/*---------------------------- Module Variables ---------------------------*/
static char nextOpponent;
static Point nextOpponentPosition;
static char enemyBattleship;
static Point enemyBattleshipPosition;
static int scoreDifferential;


/****************************************************************************
 Function
     ExecuteShootingStrategy

 Parameters
     None

 Returns
     None

 Description
     Has the ship execute a particular shooting strategy based on the current
     score differential and state of the game.
****************************************************************************/
void ExecuteShootingStrategy(void)
{
    //Getting location of next opponent fleet ship
    nextOpponent = GetNearestOpponentShip();
    updateOpponentPosition();

    //Getting location of enemy battleship
    enemyBattleship = GetOpponentNumber();
    updateEnemyBattleshipPosition();

    scoreDifferential = GetScoreDifferential();

    // Grading/checkoff code
    // Doesn't account for score or enemy
    if (GetNumBallsLeft() > 1 && nextOpponent != 0)
    {
        ShootAtFleetShip();
    }
    else
    {
        ShootAtEnemyPowerSupply();
    }
    return;

    // If we are winning by one full ship go after their main battleship, that
    // way if they shoot one of our ships just before we shoot them we still
    // win
    if (scoreDifferential >= FLEET_SHIP_VALUE)
    {
        ShootAtEnemyBattleship();
    }
    else (nextOpponent == 0)
    {
        if (GetNumBallsLeft() > 1 && nextOpponent != 0)
        {
            ShootAtFleetShip();
        }
        else
        {
            ShootAtEnemyPowerSupply();
        }
    }

}


/*---------------------------- Private Functions ---------------------------*/
void updateOpponentPosition(void)
{
    nextOpponentPosition.x = GetX(nextOpponent);
    nextOpponentPosition.y = GetY(nextOpponent);
}


void updateEnemyBattleshipPosition(void)
{
    enemyBattleshipPosition.x = GetLastKnownX(enemyBattleship);
    enemyBattleshipPosition.y = GetLastKnownY(enemyBattleship);
}


void ShootAtEnemyPowerSupply(void)
{
    SetShooterDistance(AVERAGE_DISTANCE_TO_POWER_PLANT);

    FindEnemyPowerPlant(CW);
}

void ShootAtFleetShip(void)
{
    unsigned long i = 0;
    signed int angle = CalculateDeltaAngle(GetX(Self), GetY(Self), GetRotation(Self), nextOpponentPosition.x, nextOpponentPosition.y);

    SetShooterDistance(CalculateDistance(GetX(Self), GetY(Self), nextOpponentPosition.x, nextOpponentPosition.y));

    if (angle >= 0)
    {
        Turn(angle, CCW);
    }
    else
    {
        Turn(-angle, CW);
    }

}

void ShootAtEnemyBattleship(void)
{
    // TODO: Beat the shit out of the other team
    signed int angle = CalculateDeltaAngle(GetX(Self), GetY(Self), GetRotation(Self), enemyBattleshipPosition.x, enemyBattleshipPosition.y);

    SetShooterDistance(CalculateDistance(GetX(Self), GetY(Self), enemyBattleshipPosition.x, enemyBattleshipPosition.y));

    if (angle >= 0)
    {
        Turn(angle, CCW);
    }
    else
    {
        Turn(-angle, CW);
    }

}

TapeSensors.c

/****************************************************************************
 Module
   TapeSensors.c

 Description
   This Module implements the tape sensor detection.
****************************************************************************/
/*----------------------------- Include Files -----------------------------*/
#include "ES_Configure.h"
#include "ES_Framework.h"
#include "Const.h"
#include "PWM.h"
#include <stdio.h>
#include "ADS12.h"
#include "DriveMotorService.h"
#include "QueryFAC.h"

#include "TapeSensors.h"

/*Note: Module Depends on the Tape Sensors Outputing as follows:
    ON BLACK TAPE - Output 2.5V ----> 512 AD Reading
    ON WHITE FLOOR- Output 0 V -----> 0 AD Reading
*/
/*----------------------------- Module Prototypes--------------------------*/
void readTapeSensors(void);

/*----------------------------- Module Defines ----------------------------*/

#define AD_THRESHOLD_CENTER_TAPE 100
#define AD_THRESHOLD_FRONT_TAPE 300

// Scale ~= (AD_THRESHOLD_FRONT_TAPE-AD_THRESHOLD_CENTER_TAPE)/MaxDropInPWMDC
#define AD_SCALE_FACTOR 8

/*---------------------------- Module Variables ---------------------------*/
static int leftADReading;
static int rightADReading;

/****************************************************************************
 Function
    InitTapeSensors

 Parameters
   None

 Returns
   None

 Description
   Initializes the tape sensors as analog inputs
****************************************************************************/
void InitTapeSensors(void)
{
    //Initialize AD Ports that Tape Sensors are on
    //Initializing PORT AD 0,1 as analog inputs
    ADS12_Init("OOOOOOAA");
}


/****************************************************************************
 Function
   MoveForwardOnTape

 Parameters
   None

 Returns
   None

 Description
   Reads the AD pins and changes the motor drive accordingly so that the
   robot goes forward along the tape.
****************************************************************************/
void MoveForwardOnTape(void)
{
    //Read both Tape sensors and modify rightDC and leftDC accordingly
    //to move forward following the tape
    readTapeSensors();

    if (leftADReading < AD_THRESHOLD_CENTER_TAPE && rightADReading < AD_THRESHOLD_CENTER_TAPE)
    {
        DriveManual(LEFT, CW, 100);
        DriveManual(RIGHT, CW, 100);

    }
    else if(leftADReading >= AD_THRESHOLD_CENTER_TAPE)
    {
        DriveManual(LEFT, CW, 100 - (leftADReading - AD_THRESHOLD_CENTER_TAPE) / AD_SCALE_FACTOR);
        DriveManual(RIGHT, CW, 100);


    }
    else if(rightADReading >= AD_THRESHOLD_CENTER_TAPE)
    {
        DriveManual(LEFT, CW, 100);
        DriveManual(RIGHT, CW, 100 - (rightADReading - AD_THRESHOLD_CENTER_TAPE) / AD_SCALE_FACTOR);

    }
}


/****************************************************************************
 Function
   MoveBackwardOnTape

 Parameters
   None

 Returns
   None

 Description
   Reads the AD pins and changes the motor drive accordingly so that the
   robot goes backward along the tape.
****************************************************************************/
void MoveBackwardOnTape(void)
{
    //Read both Tape sensors and modify rightDC and leftDC accordingly
    //to move backward following the tape
    readTapeSensors();

    if (leftADReading < AD_THRESHOLD_CENTER_TAPE && rightADReading < AD_THRESHOLD_CENTER_TAPE)
    {
        DriveManual(LEFT, CCW, 100);
        DriveManual(RIGHT, CCW, 100);

    }
    else if (leftADReading >= AD_THRESHOLD_CENTER_TAPE && rightADReading >= AD_THRESHOLD_CENTER_TAPE)
    {
        DriveManual(LEFT, CCW, 100);
        DriveManual(RIGHT, CCW, 100);

    }
    else if(leftADReading >= AD_THRESHOLD_CENTER_TAPE)
    {
        DriveManual(RIGHT, CCW, 100);
        DriveManual(LEFT, CCW, 100  - (leftADReading - AD_THRESHOLD_CENTER_TAPE) / AD_SCALE_FACTOR);

    }
    else if(rightADReading >= AD_THRESHOLD_CENTER_TAPE)
    {
        DriveManual(RIGHT, CCW, 100 - (rightADReading - AD_THRESHOLD_CENTER_TAPE) / AD_SCALE_FACTOR);
        DriveManual(LEFT, CCW, 100);

    }
}


/****************************************************************************
 Function
   AtFrontLine

 Parameters
   None

 Returns
   boolean

 Description
   Reads the AD pins and returns true if both are above a certain threshold
   indicating that the front line has been reached.
****************************************************************************/
boolean AtFrontLine(void)
{

    // Sanity check we passed the tape line (48 +- (8+4))
    if (GetX(Self) > 36 && GetX(Self) < 84)
    {

        return True;
    }

    //Returns true if both tape sensors are on the front line, false otherwise
    readTapeSensors();
    if (leftADReading > AD_THRESHOLD_FRONT_TAPE && rightADReading > AD_THRESHOLD_FRONT_TAPE)
    {
        return True;
    }
    return False;
}

/****************************************************************************
 Function
   NearDepot

 Parameters
   None

 Returns
   boolean

 Description
   Reads the AD pins and returns true if both are above a certain threshold
   indicating that the front line has been reached.
****************************************************************************/
boolean NearDepot(void)
{
    // Sanity check we passed the tape line (48 +- (8+4))
    if (GetX(Self) < 20 || GetX(Self) > 76)
    {
        return True;
    }
    return False;
}


/*---------------------------- Private Functions ---------------------------*/
/****************************************************************************
 Function
   readTapeSensors

 Parameters
   None

 Returns
   None

 Description
   Reads the AD pins and updates the module level variables leftADReading
   and rightADReading.
****************************************************************************/
void readTapeSensors(void)
{
    leftADReading = ADS12_ReadADPin(LEFT_TAPE_SENSOR_CHANNEL);
    rightADReading = ADS12_ReadADPin(RIGHT_TAPE_SENSOR_CHANNEL);
}

TestMain.c

#include <stdio.h>
#include "ES_Configure.h"
#include "ES_Framework.h"
#include "ES_Timers.h"
#include "PWM.h"
#include "ADS12.h"
#include <string.h>
#include <stdio.h>
#include "Servo.h"
#include "DriveMotorService.h"
#include "ShooterMotorService.h"
#include "Const.h"
#include "DispenserControl.h"
#include "FiducialCover.h"

#include "QueryFAC.h"
#include "PositionLib.h"


void main( void)
{
    ES_Return_t ErrorType;

    // initialize baud rate when downloading to E128
    SCI0BD = 39;

    // now initialize the Events and Services Framework and start it running
    ErrorType = ES_Initialize(ES_Timer_RATE_1MS);

    if ( ErrorType == Success )
    {

        ErrorType = ES_Run();

    }
    //if we got to here, there was an error
    switch (ErrorType)
    {
        case FailedPointer:
            puts("Failed on NULL pointer");
            break;
        case FailedInit:
            puts("Failed Initialization");
            break;
        default:
            puts("Other Failure");
            break;
    }
    for(;;)
        ;

};

/*------------------------------- Footnotes -------------------------------*/
/*------------------------------ End of file ------------------------------*/

Bill of Materials

Item Distributor Quantity Unit Unit Cost Total Cost
Mechanical Components and Sensors
Drive Motors SPDL 2 ea. $0.00 $0.00
Shooter Motor SPDL 1 ea. $3.00 $3.00
Vigor VTS-08B Servo SPDL 1 ea. $6.15 $6.15
Hextronik HXT900 Servo SPDL 1 ea. $3.60 $3.60
Wheels ServoCity 2 ea. $4.99 $9.98
Spider Coupler SPDL 2 ea. $3.75 $7.50
1/4" Clamping Hub ServoCity 2 ea. $7.99 $15.98
Axles McMaster-Carr 0.5 ft. $1.013 $0.51
Steel Shaft Collar McMaster-Carr 2 ea. $0.63 $1.26
Bronze Bearing McMaster-Carr 4 ea. $0.46 $1.84
M3 screws McMaster-Carr 6 ea. $0.032 $0.19
M3 washers McMaster-Carr 6 ea. $0.0158 $0.10
L-Brackets McMaster-Carr 10 ea. $0.37 $3.70
8-32 Nuts McMaster-Carr 20 ea. $0.015 $0.30
8-32 Washers McMaster-Carr 20 ea. $0.014 $0.29
8-32 5/8" Screws McMaster-Carr 20 ea. $0.05 $1.00
4-40 Nuts McMaster-Carr 21 ea. $0.029 $0.61
4-40 Washers McMaster-Carr 2 pk. $1.09 $2.18
4-40 1/2" Screws McMaster-Carr 69 ea. $0.03 $2.07
4-40 5/8" Screws McMaster-Carr 7 ea. $0.034 $0.24
4-40 3/4" Screws McMaster-Carr 6 ea. $0.035 $0.21
4-40 1-1/2" Screws McMaster-Carr 4 ea. $0.058 $0.24
Casters Pololu 2 ea. $2.99 $5.98
1" Female Standoff McMaster-Carr 4 ea. $0.45 $1.80
1-3/4" Female Standoff McMaster-Carr 8 ea. $0.62 $4.96
2-1/2" Female Standoff McMaster-Carr 8 ea. $0.67 $5.36
3" Female Standoff McMaster-Carr 4 ea. $0.70 $2.80
3-1/2" Female Standoff McMaster-Carr 6 ea. $0.74 $4.44
4" Female Standoff McMaster-Carr 4 ea. $0.77 $3.08
1/4" Male-Female Standoff McMaster-Carr 4 ea. $0.46 $1.84
Duron PRL 1.5 ea. $8.00 $12.00
High-gauge metal wire SPDL 0.5 ft. $0.10 $0.05
Masking tape Ace 0.5 ea. $3.80 $1.90
Zip tie Sparkfun 1 ea. $0.02 $0.02
Foam Core Office Depot 0.01 ea. $12.99 $0.13
Subtotal $105.31
Connectors and Fasteners
2 pin male Molex connector SPDL 10 ea. $0.10 $1.00
2 pin female Molex connector SPDL 12 ea. $0.10 $1.20
3 pin male Molex connector SPDL 38 ea. $0.10 $3.80
3 pin female Molex connector SPDL 33 ea. $0.10 $3.30
4 pin male Molex connector SPDL 15 ea. $0.10 $1.50
4 pin female Molex connector SPDL 14 ea. $0.10 $1.40
5 pin female Molex connector SPDL 1 ea. $0.10 $0.10
6 pin female Molex connector SPDL 1 ea. $0.10 $0.10
16-pin DIP socket SPDL 7 ea. $0.40 $2.80
20-pin DIP socket SPDL 1 ea. $0.50 $0.50
24-pin DIP socket SPDL 1 ea. $0.60 $0.60
Crimps SPDL 166 ea. $0.03 $4.98
Twisted pair wire SPDL 37.5 ft. $0.10 $3.75
Jumper wire Lab kit 200 ea. $0.00 $0.00
Heat shrink SPDL 2 ft. $0.25 $0.50
4 wire ribbon cable SPDL 3 ft. $0.25 $0.75
Terminal block Fry's 2 ea. $2.69 $5.38
Battery connector SPDL 2 ea. $2.05 $4.10
Subtotal $35.76
Electrical Components
Op-amp (LM324) SPDL 2 ea. $0.25 $0.50
Comparator (LM339) SPDL 2 ea. $0.25 $0.50
H-bridge (L293NE) Lab kit 1 ea. $0.00 $0.00
Schmitt Trigger (74HC14) SPDL 2 ea. $0.25 $0.25
H-bridge kit (LTE5206) SPDL 2 ea. $5.50 $11.00
Fast Recovery Diode (1N4935) Lab kit 4 ea. $0.00 $0.00
N-Channel MOSFET (2N7000) Lab kit 1 ea. $0.00 $0.00
Tape Sensors SPDL 5 ea. $2.00 $10.00
IR LED (LTE5208A) SPDL 8 ea. $0.35 $2.80
IR Phototransistor (LTR3208A) SPDL 3 ea. $0.50 $1.50
+5V 1A regulator SPDL 1 ea. $0.75 $0.75
+5V 3A regulator SPDL 1 ea. $0.75 $0.75
47 ohm resistor SPDL 1 ea. $0.01 $0.01
210 ohm resistor SPDL 3 ea. $0.01 $0.03
680 ohm resistor SPDL 8 ea. $0.01 $0.08
3.3K ohm resistor SPDL 6 ea. $0.01 $0.06
10K ohm resistor SPDL 2 ea. $0.01 $0.02
16K ohm resistor SPDL 1 ea. $0.01 $0.01
33K ohm resistor SPDL 4 ea. $0.01 $0.04
100K ohm resistor SPDL 14 ea. $0.01 $0.14
470K ohm resistor SPDL 5 ea. $0.01 $0.05
1M ohm resistor SPDL 3 ea. $0.01 $0.03
10M ohm resistor SPDL 3 ea. $0.01 $0.03
0.1 nF capacitor SPDL 6 ea. $0.10 $0.60
10 uF capacitor SPDL 4 ea. $0.10 $0.40
7.2V NiCd Battery SPDL 2 ea. $7.75 $15.50
Toggle Switch Room 36 1 ea. $1.25 $1.25
Fuse Holder Room 36 2 ea. $0.80 $1.60
1A Fuse Room 36 1 ea. $0.25 $0.25
3A Fuse Room 36 1 ea. $0.25 $0.25
Red LED SPDL 1 ea. $0.25 $0.25
Green LED SPDL 1 ea. $0.25 $0.25
Limit Switch SPDL 1 ea. $1.40 $1.40
Perfboard Room 36 5 ea. $1.50 $7.50
Heat Sink Room 36 2 ea. $0.25 $0.50
Subtotal $58.30
Miscellaneous
Freescale MC9S12E128 SPDL 1 ea. $0.00 $0.00
Forward Artillery Controller SPDL 1 ea. $0.00 $0.00
Subtotal $0.00
Grand Total
Total $199.37

Gems of Wisdom

  1. Simple and Efficient
    Fancy robots tend to be more complex and have more potential for bugs. Keep the design (and strategy) simple and easy to execute.
  2. Incremental Integration
    Integrating multiple systems at the same time can make finding errors more difficult. Integrate each system separately to make finding problems easier.
  3. Finish hardware and circuits first
    Finishing all building and circuits quickly will allow for more time debugging code and interfacing it with the actual system. There will be many issues that running on the hardware will create as compared to when events are artificially triggered (or simulated). Those that allowed several days for debugging the integrated system were more successful.
  4. Measure twice, cut once
    Create clear plans and time frames for hardware, circuit, and software designs before building anything. Having everyone on the same page before beginning is important and will expedite the process as the deadline approaches.
  5. Organization is the key to success:
    Keeping the listed items clean and organized will prevent many mistakes and save lots of time in the end.
    • Software: If the code is clearly commented and logically organized, it is much easier to read and therefore debug.
    • Circuits: Plan out how the circuits will be placed in your module frame. If this is not done, wires will be tangled, making circuit debugging a nightmare.
    • Components: Always buy spare components and organize them so that they are easily accessible.
    • Tools: The SPDL becomes a mess when the project is nearing its due date and finding the correct tool can take a frustratingly long amount of time. There is no point wasting this time each time you need something. Get your own set of whatever tools you need and keep them to yourself.
    • Personnel: Decomposing large problems into manageable pieces for individuals to work on is more efficient than having the entire group work on everything together.