Code Listings

Full Code Listing
Brain Module:
Header:
/****************************************************************************
Header file for the Brain module
****************************************************************************/
#ifndef Brain_H
#define Brain_H
// Event Definitions
#include "ES_Configure.h" /* gets us event definitions */
#include "ES_Types.h" /* gets bool type for returns */
// typedefs for the states
// State definitions for use with the query function
typedef enum { WaitingToStart, Round1, Recess1, Round2, Recess2, Round3,
Recess3, SuddenDeath
} RoundState_t ;
// Public Function Prototypes
bool InitBrain ( uint8_t Priority );
bool PostBrain( ES_Event ThisEvent );
ES_Event RunBrain( ES_Event ThisEvent );
RoundState_t QueryBrain ( void );
/* comm functions */
void setAllowedReload( bool redReloadStatus, bool darkReloadStatus );
void setGameScore( unsigned char RKscore, unsigned char DKscore );
#endif /* Brain_H */
Souce:
/****************************************************************************
Module
Brain.c
Revision
1.0.1
Description
Brain module is the master state machine 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 "Brain.h"
#include <stdio.h>
#include "EventCheckers.h"
#include <hidef.h>
/* common defines and macros */
#include <mc9s12e128.h>
/* derivative information */
#include <S12e128bits.h>
/* bit definitions */
#include <Bin_Const.h>
/* macros to allow specifying binary
constants */
#include <termio.h>
/* to get prototype fo kbhit() */
#include "s12evec.h"
/* E128 interrupt vectors */
#include <s12vec.h>
#include "DEFINES.h"
#include "navigation.h"
#include "Shooting.h"
#include "Jousting.h"
#include "BallDispensing.h"
#include "Comm.h"
/*----------------------------- Module Defines ----------------------------*/
// Brain inner current state defines
#define NOT_IN_ROUND 0
#define DRIVING_TO_GOAL 1
#define SHOOTING_BALLS 2
#define COLLECTING_BALLS 3
#define TURNING_TO_TAPE 4
#define DRIVING_TO_TAPE 5
#define TURNING_TO_WALL 6
#define DRIVING_TO_DISPENSER 7
#define ALIGNING_WITH_WALL 8
#define DRIVING_TO_KNIGHT 9
#define ATTACKING 10
#define ALIGN 11
#define DRIVING_FAST 12
/*---------------------------- Module Functions ---------------------------*/
/* prototypes for private functions for this machine.They should be functions
relevant to the behavior of this state machine
*/
// Command Functions
static void commandNavigation(unsigned char command);
static void commandShooting(unsigned char command);
static void commandJousting(unsigned char command);
static void commandBallDispensing(unsigned char command);
// Round specific state machines
static void Round1SM(ES_Event ThisEvent);
static void Recess1SM(ES_Event ThisEvent);
static void SuddenDeathSM(ES_Event ThisEvent);
/*---------------------------- 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 RoundState_t CurrentRound;
// with the introduction of Gen2, we need a module level Priority var as well
static uint8_t MyPriority;
static signed int score;
static unsigned char knightColor;
// JSR variables
static bool isWaitingForStart = false;
static bool isInMatch = false;
static bool isRecess = false;
static bool isEnteringSuddenDeath = false;
static bool isEndOfMatch = false;

static bool isUnhorsed = false;
static bool isAllowedReload = false;
static signed int score = 0;
/*------------------------------ Module Code ------------------------------*/
/****************************************************************************
Function
InitBrain
Parameters
uint8_t : the priorty of this service
Returns
bool, false if error in initialization, true otherwise
Description
Saves away the priority, sets up the Brain
****************************************************************************/
bool InitBrain( uint8_t Priority )
{
//
ES_Event ThisEvent;
MyPriority = Priority;
// Start first communication
ES_Timer_InitTimer(REQUEST_STATUS_TIMER, 500);
// put us into the Initial PseudoState
CurrentRound = WaitingToStart;
// Read input pin to get knight color
KNIGHT_SWITCH_LINE = INPUT;
knightColor = KNIGHT_SWITCH; // Red = 0, Dark = 1
printf("Knight Color : %d \r \n", knightColor);
// Set game match status LED to output
GAME_STATUS_LED_LINE = OUTPUT;
return true;
}
/****************************************************************************
Function
PostBrain
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 the Brain
****************************************************************************/
bool PostBrain( ES_Event ThisEvent )
{
return ES_PostToService( MyPriority, ThisEvent);
}
/****************************************************************************
Function
RunbRAIN
Parameters
ES_Event : the event to process
Returns
ES_Event, ES_NO_EVENT if no error ES_ERROR otherwise
Description
Runs the Brain state machine - a hierarchical state machine
****************************************************************************/
ES_Event RunBrain( ES_Event ThisEvent )
{
ES_Event ReturnEvent;
ES_Event StopEvent;
ES_Event TestCommand;
bool test = false;
ReturnEvent.EventType = ES_NO_EVENT; // assume no errors
StopEvent.EventType = ES_ROUND_OVER;
// Sample JSR
if ( ThisEvent.EventType == ES_TIMEOUT )
{
if (ThisEvent.EventParam == REQUEST_STATUS_TIMER)
{
// Post new command request to Comm SM
ES_Event CommEvent;
CommEvent.EventType = ES_SEND_COMMAND;
CommEvent.EventParam = COMMAND_GAME_STATUS;
PostComm( CommEvent );
//puts("posted command game status\r\n");
// Restart comm request timer
ES_Timer_InitTimer(REQUEST_SCORE_TIMER, REQUEST_SCORE_TIME);
return ReturnEvent;
}
if (ThisEvent.EventParam == REQUEST_SCORE_TIMER)
{
// Post new command request to Comm SM
ES_Event CommEvent;
CommEvent.EventType = ES_SEND_COMMAND;
CommEvent.EventParam = COMMAND_GAME_SCORE;
PostComm( CommEvent );
//puts("posted command game score\r\n");
// Restart comm request timer
ES_Timer_InitTimer(REQUEST_STATUS_TIMER, REQUEST_STATUS_TIME);
return ReturnEvent;
}
}
switch ( CurrentRound )
{
// Waiting To Start
case WaitingToStart :
if (ThisEvent.EventType == ES_NEW_ROUND)
{
puts("Entering Round 1 \r");
CurrentRound = Round1;
Round1SM(ThisEvent);
}
break;
// Round 1
case Round1 :
{
puts("Entering Recess 1 \r");
// New round event, stop Round1SM, start Recess1SM
CurrentRound = Recess1;
Round1SM(StopEvent);
Recess1SM(ThisEvent);
}
else
{
// Run round 1 state machine
}
break;
// Recess 1
case Recess1 :
{
// New round event, stop Recess1SM, start Round2SM
Recess1SM(StopEvent);
}
else
{
// Run recess 1 state machine
}
break;
// Round 2
case Round2 :
{
}
else
{
}
break;
// Recess 2
case Recess2 :
{
// New round event, stop Recess2SM, start Round3SM
}
else
{
}
break;
// Round 3
case Round3 :
{
}
else
{
}
break;
// Recess 3
case Recess3 :
{
puts("Entering Sudden Death \r");
// New round event, stop Recess3SM, start SuddenDeath
CurrentRound = SuddenDeath;
SuddenDeathSM(ThisEvent);
}
else
{
}
break;
// Sudden Death
case SuddenDeath :
{
puts("End of match \r");
// New round event, stop Recess3SM, start SuddenDeath
}
else
{
// Run sudden death state machine
break;
}
}
// Go back to WaitingToStart if the game is over
if (ThisEvent.EventType == ES_GAME_OVER)
{
GAME_STATUS_LED = LOW;
commandShooting(STOP_PITCHING);
}
return ReturnEvent;
}
/****************************************************************************
Function
QueryBrain
Parameters
None
Returns
TemplateState_t The current state of the Template state machine
Description
returns the current state of the Brain
****************************************************************************/
RoundState_t QueryBrain ( void )
{
return(CurrentRound);
}
/***************************************************************************
private functions
***************************************************************************/
/****************************************************************************
Function
Round1SM
Parameters
Returns
None
Description
Runs the state machine for round 1
****************************************************************************/
static void Round1SM(ES_Event ThisEvent)
{
static unsigned char CurrentState = NOT_IN_ROUND;
// Reset the state machine and stop the robot if the round or game is over
if ((ThisEvent.EventType == ES_GAME_OVER) || (ThisEvent.EventType ==
ES_ROUND_OVER))
{
commandNavigation(STOP_ROBOT);
CurrentState = NOT_IN_ROUND;
return;
}
switch (CurrentState)
{
// Initialize the state machine for round 1 here
case NOT_IN_ROUND :
{
GAME_STATUS_LED = HIGH;
commandNavigation(DRIVE_TO_B);
CurrentState = DRIVING_FAST;
ES_Timer_InitTimer(BRAIN_TIMER, FAST_DRIVE_TIME);
}
break;
// Driving really fast
case DRIVING_FAST :
if (ThisEvent.EventType == ES_TIMEOUT && ThisEvent.EventParam == BRAIN_TIMER)
{
puts("Slowing down \r");
commandNavigation(SLOW_DOWN);
CurrentState = DRIVING_TO_GOAL;
}
break;
// Driving to Home B
case DRIVING_TO_GOAL :
if (ThisEvent.EventType == ES_NAVIGATION_COMPLETE)
{
}
break;
}
}
/****************************************************************************
Function
Recess1SM
Parameters
Returns
None
Description
Runs the state machine for recess 1
****************************************************************************/
static void Recess1SM(ES_Event ThisEvent)
{
static unsigned char CurrentState = NotInRound;
ES_ROUND_OVER))
{
return;
}
{
case NOT_IN_ROUND :
{
commandNavigation(DRIVE_TO_GOAL);
}
break;
{
commandShooting(START_PITCHING);
}
break;
}
}
/****************************************************************************
Function
Round2SM
Parameters
Returns
None
Description
****************************************************************************/
{
static unsigned char CurrentState = NotInRound;
ES_ROUND_OVER))
{
return;
}
{
case NOT_IN_ROUND :
{
commandShooting(SHOOT_BALL);
CurrentState = SHOOTING_BALLS;
}
break;
// Shooting balls
case SHOOTING_BALLS :
if (ThisEvent.EventType == ES_BALL_SHOT)
{
commandNavigation(DRIVE_TO_A);
}
break;
case DRIVING_FAST :
{
}
break;
// Driving to Home A
{
}
break;
}
}
/****************************************************************************
Function
Recess2SM
Parameters
Returns
None
Description
****************************************************************************/
{
ES_ROUND_OVER))
{
return;
}
{
//Actual code
case NOT_IN_ROUND :
{
commandNavigation(ALIGN_WITH_WALL);
CurrentState = ALIGN;
}
break;
// Aligning with wall before turning
case ALIGN :
{
commandNavigation(TURN_ROBOT_CW_90);
CurrentState = TURNING_TO_TAPE;
}
break;
// Turning to tape
case TURNING_TO_TAPE :
{
puts("Aligned with tape \r");
commandNavigation(DRIVE_TO_TAPE);
CurrentState = DRIVING_TO_TAPE;
}
break;
// Driving to tape
case DRIVING_TO_TAPE :
{
puts("Tape reached \r");
commandNavigation(TURN_ROBOT_CCW_90);
CurrentState = TURNING_TO_WALL;
}
break;
// Turning back to wall
case TURNING_TO_WALL :
{
puts("Aligned with wall \r");
CurrentState = ALIGNING_WITH_WALL;
}
break;
// Aligning with wall
case ALIGNING_WITH_WALL :
{
puts("Aligned with wall \r");
commandNavigation(DRIVE_TO_DISPENSER);
CurrentState = DRIVING_TO_DISPENSER;
}
break;
// Driving to dispenser
case DRIVING_TO_DISPENSER :
{
puts("At ball dispenser");
commandBallDispensing(ES_COLLECT_BALL);
CurrentState = COLLECTING_BALLS;
}
break;
// CollectingBalls
case COLLECTING_BALLS :
if (ThisEvent.EventType == ES_BALL_COLLECTED)
{
commandBallDispensing(ES_COLLECT_BALL);
}
break;
}
}
/****************************************************************************
Function
Round3SM
Parameters
Returns
None
Description
****************************************************************************/
{
ES_ROUND_OVER))
{
return;
}
{
// Round3SM Initialization
case NOT_IN_ROUND :
{
puts("Entering round 3 \r");
CurrentState = ATTACKING;
ES_Timer_InitTimer(BRAIN_TIMER, FAST_DRIVE_TIME / 4);
}
break;
// Attack robot
case ATTACKING :
{
ES_Timer_InitTimer(BRAIN_TIMER, FAST_DRIVE_TIME * 3 / 4);
commandJousting(DEPLOY_JOUST_HEAD);
}
break;
case DRIVING_FAST :
{
}
break;
{
puts("At goal \r");
}
break;
}
}
/****************************************************************************
Function
Recess3SM
Parameters
Returns
None
Description
****************************************************************************/
{
ES_ROUND_OVER))
{
return;
}
{
case NOT_IN_ROUND :
{
GAME_STATUS_LED = HIGH;
}
break;
{
}
break;
}
}
/****************************************************************************
Function
SuddenDeathSM
Parameters
Returns
None
Description
Runs the state machine for sudden death
****************************************************************************/
static void SuddenDeathSM(ES_Event ThisEvent)
{
{
// Round3SM Initialization
case NOT_IN_ROUND :
{
puts("Entering round 3 \r");
commandNavigation(DRIVE_TO_A);
CurrentState = ATTACKING;
ES_Timer_InitTimer(BRAIN_TIMER, FAST_DRIVE_TIME / 4);
}
break;
// Attack robot
case ATTACKING :
{
ES_Timer_InitTimer(BRAIN_TIMER, FAST_DRIVE_TIME * 3 / 4);
commandJousting(DEPLOY_JOUST_BODY);
}
break;
case DRIVING_FAST :
{
}
break;
{
puts("At goal \r");
}
break;
}
}
/****************************************************************************
Function
commandNavigation
Parameters
An unsigned char specifying the command
Returns
None
Description
send a command to the navigation service
****************************************************************************/
static void commandNavigation(unsigned char command)
{
ES_Event commandEvent;
commandEvent.EventType = ES_COMMAND;
commandEvent.EventParam = command;
PostNavigation(commandEvent);
}
/****************************************************************************
Function
commandJousting
Parameters
Returns
None
Description
send a command to the jousting service
****************************************************************************/
static void commandJousting(unsigned char command)
{
PostJousting(commandEvent);
}
/****************************************************************************
Function
commandShooting
Parameters
Returns
None
Description
send a command to the shooting service
****************************************************************************/
static void commandShooting(unsigned char command)
{
PostShooting(commandEvent);
}
/****************************************************************************
Function
commandBallDispensing
Parameters
Returns
None
Description
send a command to the ball dispensing service
****************************************************************************/
static void commandBallDispensing(unsigned char command)
{
commandEvent.EventType = ES_COLLECT_BALL;
PostBallDispensing(commandEvent);
}
/***************************************************************************
public functions
***************************************************************************/
/****************************************************************************
Function
setAllowedReload
Parameters
bool - allowed reload or not.
Returns
Nothing.
Description
Updates isAllowedReload module variable from CommSM.
****************************************************************************/
void setAllowedReload( bool redReloadStatus, bool darkReloadStatus )
{
if (knightColor == RED)
{
isAllowedReload = redReloadStatus;
}
else if (knightColor == DARK)
{
isAllowedReload = darkReloadStatus;
}
}
/****************************************************************************
Function
setGameScore
Parameters
unsigned char - Red knight score
unsigned char - Dark knight score
Returns
Nothing.
Description
Updates redKnightScore and darkKnightScore module variables from CommSM.
****************************************************************************/
void setGameScore( unsigned char RKscore, unsigned char DKscore )
{
if (knightColor == RED)
{
score = RKscore - DKscore;
}
else if (knightColor == DARK)
{
score = DKscore - RKscore;
}
Navigation Module:
Header:
/****************************************************************************
Header file for the navigation module
****************************************************************************/
#ifndef Navigation_H
#define Navigation_H
#include "ES_Types.h"
typedef enum { WaitForCommand, DriveToSubject,DrivingToMidfield,DrivingToPlace,
DrivingToGoal, DrivingToKnight,
DrivingToDispenser,Aligning,AligningWithKnight, AligningWithWall, AligningWithGoal,
AligningWithDispenser,
AligningWithWall_Dispenser, DrivingFull, Turning, DrivingToTape, InitInner,
DrivingToHomeA, DrivingToHomeB, RequestingBall
} Lab8State_t ;
bool InitNavigation ( uint8_t Priority );
bool PostNavigation( ES_Event ThisEvent );
ES_Event RunNavigation( ES_Event ThisEvent );
Lab8State_t QueryNavigation ( void );
float getBackUltrasonicDistance(void);
float getSideUltrasonicDistance1(void);
float getSideUltrasonicDistance2(void);
#endif /* Navigation_H */
Source:
/****************************************************************************
Module
Navigation.c
Revision
1.0.1
Description
Implements the navigation module
****************************************************************************/
/*----------------------------- Include Files -----------------------------*/
*/
#include "navigation.h"
#include <stdio.h>
#include <hidef.h>
constants */
#include <termio.h>
#include <s12vec.h>
#include "Brain.h"
/*----------------------------- Module Defines ----------------------------*/
#define STATIONARY 2
#define CONTROL_ENABLED 1
#define CONTROL_DISABLED 0
#define MAX_PULSE_COUNT 10
#define OFF
0
#define DEBOUNCE_TIME 3
*/
static void drivingToPlaceSM(ES_Event ThisEvent, unsigned char direction);
static void drivingToDispenserSM(ES_Event ThisEvent);
static void setPortDirections(void);
static void initTimers(void);
static void initPWM(void);
static void initSPI(void);
static void driveRobot(char direction, char speed);
static void slowRobot(void);
static void turnRobot(char direction);
static void stopRobot(void);
static void respondToCommand(ES_Event ThisEvent);
static void postNavigationComplete(void);
static void enablePositionControl(unsigned char direction);
static void enableStationaryControl (void);
static void disablePositionControl(void);
static unsigned char echo_flag1=0, echo_flag2=0, echo_flag3=0,ultrasonicEnable=0,
controlDirection;
static unsigned int trigger_send=0,echo_detect=0,echo_detect2=0,echo_detect3=0;
static unsigned char leftSpeed,rightSpeed,testcount=0;
static float useconds,useconds2,useconds3,distance,distance2,distance3, referenceDistance;
static Lab8State_t CurrentState;
static bool isBeacon = false;
/*------------------------------ Module Code ------------------------------*/
/****************************************************************************
Function
InitNavigation
Parameters

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
Notes
Author
J. Edward Carryer, 10/23/11, 18:55
****************************************************************************/
bool InitNavigation ( uint8_t Priority )
{
// put us into the Initial PseudoState
CurrentState = WaitForCommand;
// Start query timer
initTimers();
// Initialize PWM
initPWM();
// Set port directions
setPortDirections();
puts("Navigation initialization completed \r");
return true;
}
/****************************************************************************
Function
PostNavigation
Parameters
Returns
Description
Posts an event to this state machine's queue
Notes
Author
****************************************************************************/
bool PostNavigation( ES_Event ThisEvent )
{
}
/****************************************************************************
Function
RunNavigation
Parameters
Returns
Description
add your description here
Notes
uses nested switch/case to implement the machine.
Author
****************************************************************************/
ES_Event RunNavigation( ES_Event ThisEvent )
{
//static unsigned char CurrentDirection,AlignTarget;
ES_Event BrainEvent;
// Respond to a new command from brain regardless of CurrentState
if (ThisEvent.EventType == ES_COMMAND)
{
puts("New command from brain \r");
respondToCommand(ThisEvent);
}
// If the navigation timer times out, stop the robot and post a failure to brain
if ((ThisEvent.EventType == ES_TIMEOUT) && (ThisEvent.EventParam ==
NAVIGATION_TIMER))
{
stopRobot();
enablePositionControl(STATIONARY);
BrainEvent.EventType = ES_NAVIGATION_FAILURE;
PostBrain(BrainEvent);
return ReturnEvent;
}
// Depending on CurrentState, execute action
{
case DrivingToHomeA :
if (ThisEvent.EventType == ES_HOME_A_DETECTED)
{
// Home reached, stop robot, post to brain and set current state back to
WaitForCommand
stopRobot();
postNavigationComplete();
ES_Timer_StopTimer(NAVIGATION_TIMER);
}
break;
case DrivingToHomeB:
if (ThisEvent.EventType == ES_HOME_B_DETECTED)
{
// Home reached, stop robot, post to brain and set current state back to
WaitForCommand
stopRobot();
}
break;
case DrivingToKnight:
if (ThisEvent.EventType == ES_KNIGHT_DETECTED)
{
// Knight reached, stop robot, post to brain and set current state back to
WaitForCommand
stopRobot();
}
else if ((ThisEvent.EventType == ES_TIMEOUT) && (ThisEvent.EventParam ==
ALIGNMENT_DEBOUNCE_TIMER))
{
if (KNIGHT_BEACON == ALIGNED)
{
// Aligned, stop robot and post navigation complete to brain
stopRobot();
}
else
{
// Not aligned, restart debounce timer
ES_Timer_InitTimer(ALIGNMENT_DEBOUNCE_TIMER, DEBOUNCE_TIME);
}
}
break;
case DrivingToGoal:
if (ThisEvent.EventType == ES_GOAL_DETECTED)
{
// Goal reached, stop robot, post to brain and set current state back to
WaitForCommand
stopRobot();
}
break;
case DrivingToDispenser :
if (ThisEvent.EventType == ES_DISPENSER_DETECTED)
{
// Goal reached, stop robot, post to brain and set current state back to
WaitForCommand
stopRobot();
}
break;
case AligningWithKnight:
{
if (KNIGHT_BEACON == ALIGNED)
{
stopRobot();
}
else
{
}
}
break;
case AligningWithGoal:
{
if (GOAL_BEACON == ALIGNED)
{
disablePositionControl();
stopRobot();
}
else
{
}
}
break;
case AligningWithWall :
if (ThisEvent.EventType == ES_WALL_ALIGNMENT_COMPLETE)
{
// Robot is aligned with wall, stop turning and move to WaitForCommand
stopRobot();
}
break;
case Turning :
if (ThisEvent.EventType == ES_TIMEOUT && ThisEvent.EventParam == TURN_TIMER)
{
// Turning complete, stop robot and post navigation complete to brain
stopRobot();
}
break;
case DrivingToTape :
if (ThisEvent.EventType == ES_TAPE_DETECTED)
{
// Tape reached, stop robot and post navigation complete to brain
stopRobot();
}
break;
} // End switch on CurrentState
return ReturnEvent;
}
/****************************************************************************
Function
QueryNavigation
Parameters
None
Returns
Description
returns the current state of the Template state machine
Notes
Author
****************************************************************************/
Lab8State_t QueryNavigation ( void )
{
return(CurrentState);
}
/***************************************************************************
private functions
***************************************************************************/
/****************************************************************************
Function
respondToCommand
Parameters
unsigned char The SPI command to execute.
Returns
Nothing.
Description
Executes the current SPI command and puts in correct State.
****************************************************************************/
static void respondToCommand(ES_Event ThisEvent)
{
ES_Event InitEvent;
InitEvent.EventType = ES_INNER_INIT;
// Switch based on brain command
switch (ThisEvent.EventParam)
{
case DRIVE_TO_A :
// Set CurrentState to DrivingToHomeA and drive forward
CurrentState = DrivingToHomeA;
//puts("Driving to A \r");
driveRobot(DC_FULL, REVERSE);
enablePositionControl(REVERSE);
break;
case DRIVE_TO_B :
// Set CurrentState to DrivingToHomeB and drive in reverse
CurrentState = DrivingToHomeB;
//puts("Driving to B \r");
driveRobot(DC_FULL, FORWARD);
enablePositionControl(FORWARD);
break;
case DRIVE_FORWARD_TO_KNIGHT :
// Set CurrentState to DrivingToKnight and drive forward
CurrentState = DrivingToKnight;
driveRobot(DC_FULL, FORWARD);
break;
case DRIVE_REVERSE_TO_KNIGHT :
// Set CurrentState to DrivingToKnight and drive in reverse
CurrentState = DrivingToKnight;
driveRobot(DC_FULL, REVERSE);
break;
case DRIVE_TO_GOAL :
// Set CurrentState to DrivingToGoal and drive forward
CurrentState = DrivingToGoal;
driveRobot(DC_HALF, FORWARD);
break;
case DRIVE_TO_DISPENSER :
puts("Driving to dispenser \r");
CurrentState = DrivingToDispenser;
driveRobot(DC_HALF, REVERSE);
break;
case ALIGN_WITH_KNIGHT :
// Align with knight
CurrentState = AligningWithKnight;
turnRobot(CW);
break;
case ALIGN_WITH_GOAL :
// Align with goal
CurrentState = AligningWithGoal;
turnRobot(CW);
break;
case ALIGN_WITH_WALL :
// Align with wall
CurrentState = AligningWithWall;
stopRobot();
break;
case TURN_ROBOT_CW_90 :
// Turn robot clockwise 90 degrees
CurrentState = Turning;
turnRobot(CW);
ES_Timer_InitTimer(TURN_TIMER, TURN_90_TIME);
//puts("Turn robot clockwise 90 \r");
break;
case TURN_ROBOT_CCW_90 :
// Turn robot counter-clockwise 90 degrees
CurrentState = Turning;
turnRobot(CCW);
ES_Timer_InitTimer(TURN_TIMER, TURN_90_TIME);
//puts("Turn robot counter-clockwise 90 \r");
break;
case DRIVE_TO_TAPE :
// Drive robot to tape
CurrentState = DrivingToTape;
driveRobot(DC_HALF, REVERSE);
break;
case STOP_ROBOT :
// Stop the robot and move to wait for command
puts("Command stop robot \r");
stopRobot();
break;
case SLOW_DOWN :
// Slow the robot down
slowRobot();
break;
}
// End switch on EventParam
}
/****************************************************************************
Function
setPortDirections
Parameters
Nothing.
Returns
Nothing.
Description
Sets the port directions for lines used on the E128.
****************************************************************************/
static void setPortDirections(void)
{
// Set motor lines as outputs
MOTOR_LEFT_DIRECTION_LINE = OUTPUT;
MOTOR_RIGHT_DIRECTION_LINE = OUTPUT;
MOTOR_LEFT_LINE = OUTPUT;
MOTOR_RIGHT_LINE = OUTPUT;
// Set lines as inputs
GOAL_BEACON_LINE = INPUT;
KNIGHT_BEACON_LINE = INPUT;
RELOAD_LED_LINE = OUTPUT;
RELOAD_LED = LOW;
//Sets Ultrasonic Trigger as Output
ULTRASONIC_BACK_OUT_LINE = OUTPUT;
ULTRASONIC_LEFT_OUT_LINE = OUTPUT;
ULTRASONIC_RIGHT_OUT_LINE = OUTPUT;
ULTRASONIC_BACK_OUT = LOW;
ULTRASONIC_LEFT_OUT = LOW;
ULTRASONIC_RIGHT_OUT = LOW;
// Stop the robot initially
stopRobot();
}
/****************************************************************************
Function
postNavigationComplete
Parameters
Nothing.
Returns
Nothing.
Description
Posts a navigation complete event to the brain.
****************************************************************************/
static void postNavigationComplete(void)
{
BrainEvent.EventType = ES_NAVIGATION_COMPLETE;
}
/****************************************************************************
Function
initTimers
Parameters
Nothing.
Returns
Nothing.
Description
Initializes channel 4 of timer 0 for input capture of beacon.
****************************************************************************/
static void initTimers(void)
{
//Initialize Ultrasonic Timers
TIM0_TSCR1 = _S12_TEN;
//Enable timer
//Set prescale to divide by 16
TIM0_TSCR2 =_S12_PR2;
TIM0_TIOS|=_S12_IOS4;
//Set IOS4 to Output Compare
TIM0_TIOS&=~_S12_IOS5;
//Set IOS5 to Input Capture
//Set Channel 4 on Timer 0 Output Compare to toggle.
TIM0_TCTL3 |= _S12_OL4;
TIM0_TCTL3 &= ~_S12_OM4;
//Set Chanel 5 on Timer 0 to Input Capture Falling Edge
TIM0_TCTL3 &= ~(_S12_EDG5A);
TIM0_TCTL3 |= _S12_EDG5B;
//Tie timers to Interrupt
TIM0_TIE |= _S12_C4I;
//Associates output compare with interrupt.
//Associates input capture with interrupt.
TIM0_TC4 = TIM0_TCNT + TIMER_PERIOD_COUNT; //Sets period in which Timer 2 triggers
flag
TIM0_TFLG1 = _S12_C4F;
//Clears Flag
//Clears Flag
//Clears Flag
EnableInterrupts;
//Clears Flag
}
/****************************************************************************
Function
initPWM
Parameters
Nothing.
Returns
Nothing.
Description
Initializes channel 4 of timer 0 for input capture of beacon.
****************************************************************************/
static void initPWM(void)
{
// Set up PWM system
PWME |= (_S12_PWME0 | _S12_PWME1 | _S12_PWME2);
PWME0, PWME1, PWME2
PWMPRCLK |= _S12_PCKA1;
prescaler to 4
PWMPRCLK |= (_S12_PCKB2 | _S12_PCKB0);
Set irled prescaler to 4
PWMSCLA = 6;
PWMSCLB = 150;
PWMPOL |= (_S12_PPOL0 | _S12_PPOL1 | _S12_PPOL2);
PWMCLK = (_S12_PCLK0 |_S12_PCLK1 |_S12_PCLK2);
PWMCTL = 0;
No concatenate PWM channels
PWMCAE = 0;
Left align
MODRR |= (_S12_MODRR0 | _S12_MODRR1 | _S12_MODRR2);
Mapping
PWMPER0 = 100;
Set period to 100 ticks
PWMPER1 = 100;
PWMPER2 = 100;
PWMDTY0 = 0;
initially set to 0
PWMDTY1 = 0;//0;
Duty cycle initially set to 0
PWMDTY2 = 0;
initially set to 0
}
/****************************************************************************
Function
turnRobot
Parameters
char The direction the robot is supposed to turn: CW or CCW
Returns
Nothing.
Description
Depending on turn, set directions for the corresponding motors.
****************************************************************************/
static void turnRobot(char direction)
// Enable
// Set motor
//
// 5000Hz
// Polarity initially high
// Use clock SA
//
//
// Turn on Port T
//
//
//
// Duty cycle
//
// Duty cycle
{
// Temporarily stop robot
stopRobot();
// Set motor directions
if (direction == CW)
{
MOTOR_LEFT_DIRECTION = FORWARD;
MOTOR_RIGHT_DIRECTION = REVERSE;
}
else if (direction == CCW)
{
MOTOR_LEFT_DIRECTION = REVERSE;
MOTOR_RIGHT_DIRECTION = FORWARD;
}
// Turn on motors
MOTOR_LEFT_DUTY = DC_TURN;
MOTOR_RIGHT_DUTY = DC_TURN;
}
/****************************************************************************
Function
driveRobot
Parameters
char The direction of the robot to move.
char The speed at which the robot should move.
Returns
Nothing.
Description
Sets the direction of the robot and the speed for it to move.
****************************************************************************/
static void driveRobot(char speed, char direction)
{
// Temporarily stop robot
stopRobot();
// Set motor directions
if (direction == FORWARD)
{
//PWMPOL |= (_S12_PPOL0 | _S12_PPOL1 | _S12_PPOL2);
}
else if (direction == REVERSE)
{
//PWMPOL &= ~(_S12_PPOL0 | _S12_PPOL1 | _S12_PPOL2);
}
// Set motor speeds
MOTOR_LEFT_DUTY = speed;//-4;
leftSpeed = speed;//-4; //Stores left motor speed variables that will be used for control loop.
MOTOR_RIGHT_DUTY = speed + 3;
rightSpeed = speed + 3; //Stores right motor speed variables that will be used for control loop.
}
static void slowRobot()
{
// Set motor speeds
MOTOR_LEFT_DUTY = DC_SLOW;//-4;
leftSpeed = DC_SLOW;//-4; //Stores left motor speed variables that will be used for control
loop.
MOTOR_RIGHT_DUTY = DC_SLOW + 3;
rightSpeed = DC_SLOW + 3; //Stores right motor speed variables that will be used for control
loop.
}
/****************************************************************************
Function
stopRobot
Parameters
Nothing.
Returns
Nothing.
Description
Stops the robot from moving.
****************************************************************************/
static void stopRobot(void)
{
//Set Duty Cycles of Motors to 0 to stop robot
MOTOR_LEFT_DUTY = OFF;
leftSpeed = OFF;
MOTOR_RIGHT_DUTY = OFF;
rightSpeed = OFF;
}
static void enablePositionControl(unsigned char direction)
{
ultrasonicEnable = CONTROL_ENABLED;
controlDirection = direction;
referenceDistance = (distance + distance2) / 2;
puts("Enable Position control \r");
}
static void enableStationaryControl (void)
{
controlDirection = STATIONARY;
puts("Enable Stationary control \r");
}
static void disablePositionControl()
{
ultrasonicEnable = CONTROL_DISABLED;
puts("disable control \r");
}
float getBackUltrasonicDistance(void)
{
return distance3;
}
float getSideUltrasonicDistance1(void)
{
return distance;
}
float getSideUltrasonicDistance2(void)
{
return distance2;
}
/***************************************************************************
public functions
***************************************************************************/
/***************************************************************************
interrupt response routines
***************************************************************************/
void interrupt _Vec_tim0ch4 sonic_trigger()
{
static float Kp=1.8, Kd=0.5, KdForward=0.8, KdReverse=1,
positionError,lastError=0;//positionError1, positionError2,lastError=0;
signed char dutyChange;
static unsigned char pulse_count=0,stationaryMultiplier=2;
TIM0_TFLG1=_S12_C4F;
if (pulse_count == 0)
{
ULTRASONIC_LEFT_OUT = HIGH;
ULTRASONIC_RIGHT_OUT = HIGH;
pulse_count = 1;
TIM0_TC4 = TIM0_TC4+TIMER_PERIOD_COUNT;
}
else if (pulse_count == 1)
{
ULTRASONIC_LEFT_OUT = LOW;
ULTRASONIC_RIGHT_OUT = LOW;
trigger_send = TIM0_TC4;
TIM0_TC4 = TIM0_TC4+TIMER_PERIOD_LAG_COUNT;
pulse_count++;
}
{
pulse_count = pulse_count++;
if (ultrasonicEnable == CONTROL_ENABLED) //Control motors if motor control is
enabled
{
float avgDistance = ((distance + distance2)/2);
positionError = referenceDistance - ( ( distance + distance2 ) / 2 );
switch (controlDirection) //Different influences of control based on the direction of
travel
{
case FORWARD:
dutyChange = (char)(Kp*positionError + KdForward*(positionError - lastError));
if (dutyChange > 5)
{
dutyChange = 5;
}
else if (dutyChange < -5)
{
dutyChange = -5;
}
if ((positionError > 0.5) || (positionError < -0.5))
{
MOTOR_LEFT_DUTY = leftSpeed + dutyChange;
MOTOR_RIGHT_DUTY = rightSpeed - dutyChange;// + 3;
}
else
{
if (dutyChange < 0)
{
dutyChange = - dutyChange;
}
if ( distance < distance2 )
{
MOTOR_RIGHT_DUTY = rightSpeed - dutyChange;
}
else if ( distance > distance2 )
{
MOTOR_LEFT_DUTY = leftSpeed - dutyChange;
MOTOR_RIGHT_DUTY = rightSpeed + dutyChange;
}
}
lastError = positionError;
break;
case REVERSE:
dutyChange = (char)(Kp*positionError + KdReverse*(positionError - lastError));
if (dutyChange > 5)
{
dutyChange = 5;
}
else if (dutyChange < -5)
{
dutyChange = -5;
}
if ((positionError > 0.5) || (positionError < -0.5))
{
MOTOR_RIGHT_DUTY = rightSpeed - dutyChange + 2;// + 3;
}
else
{
if (dutyChange < 0)
{
dutyChange = - dutyChange;
}
if ( distance < distance2 )
{
MOTOR_LEFT_DUTY = leftSpeed - dutyChange;
MOTOR_RIGHT_DUTY = rightSpeed + dutyChange + 2;
}
else if ( distance > distance2 )
{
MOTOR_RIGHT_DUTY = rightSpeed - dutyChange + 2;
}
}
lastError = positionError;
break;
case STATIONARY:
{
float positionError2 = distance - distance2;
if ((positionError2 > -5) && (positionError2 < 5))
{
if (positionError2 < -0.25)
{
MOTOR_LEFT_DUTY = ALIGN_TURN;
MOTOR_RIGHT_DUTY = ALIGN_TURN;
//MOTOR_LEFT_DUTY = stationaryMultiplier*dutyChange;
}
else if (positionError2 > 0.25)
{
MOTOR_LEFT_DUTY = ALIGN_TURN;
MOTOR_RIGHT_DUTY = ALIGN_TURN;
//MOTOR_LEFT_DUTY = stationaryMultiplier*dutyChange;
}
else
{
MOTOR_LEFT_DUTY = OFF;
MOTOR_RIGHT_DUTY = OFF;
}
}
break;
}
}
}
ULTRASONIC_BACK_OUT = HIGH;
TIM0_TC4 = TIM0_TC4+TIMER_PERIOD_COUNT;
}
{
pulse_count = 0;
ULTRASONIC_BACK_OUT = LOW;
trigger_send = TIM0_TC4;
TIM0_TC4 = TIM0_TC4+TIMER_PERIOD_LAG_COUNT;
}
}
void interrupt _Vec_tim0ch5 echo_detect_function()
{
static float currDistance = 0;
static float lastDistance = 0;
//puts("Enter echo1 interrupt \r");
echo_detect = TIM0_TC5;
useconds = TICK_RESOLUTION_uS*(echo_detect-trigger_send); //Calculates microseconds
corresponding to ticks.
currDistance = (useconds)/(148)-2.9;
if ((lastDistance != 0) && (currDistance > 0) && (((currDistance - lastDistance) <
SONIC_TOLERANCE) && ((lastDistance - currDistance) < SONIC_TOLERANCE)))
{
distance = currDistance;
}
lastDistance = currDistance;
}
void interrupt _Vec_tim0ch6 echo_detect2_function()
{

echo_detect2 = TIM0_TC6;
useconds2 = TICK_RESOLUTION_uS*(echo_detect2-trigger_send); //Calculates
microseconds corresponding to ticks.
currDistance = (useconds2)/(148)-2.9;
if ((lastDistance != 0) && currDistance > 0 && ((currDistance - lastDistance) <
SONIC_TOLERANCE) && ((lastDistance - currDistance) < SONIC_TOLERANCE))
{
distance2 = currDistance;
}
lastDistance = currDistance;
}
void interrupt _Vec_tim0ch7 echo_detect3_function()
{
Jousting Module:
Header:
/****************************************************************************
Header file for the jousting module
****************************************************************************/
#ifndef Jousting_H
#define Jousting_H
typedef enum { ReadyToJoust, JoustRetracted, Jousting } TemplateState_t ;
bool InitJousting ( uint8_t Priority );
bool PostJousting( ES_Event ThisEvent );
ES_Event RunJousting( ES_Event ThisEvent );
TemplateState_t QueryJousting ( void );
#endif /* Jousting_H */
Source:
/****************************************************************************
Module
Jousting.c
Revision
1.0.1
Description
Jousting service for the 218B robot
****************************************************************************/
/*----------------------------- Include Files -----------------------------*/
*/
#include "Jousting.h"
#include <stdio.h>
#include <hidef.h>
constants */
#include <termio.h>
#include <s12vec.h>
#include "Servos.h"
#include "Brain.h"
/*----------------------------- Module Defines ----------------------------*/
*/
static void joustingHardwareInit(void);
static void deployJoust(unsigned int position);
static void retractJoust(void);
static TemplateState_t CurrentState;
/*------------------------------ Module Code ------------------------------*/
/****************************************************************************
Function
InitJousting
Parameters
Returns
Description
Initializes the jousting module
****************************************************************************/
bool InitJousting( uint8_t Priority )
{
// Init hardware for jousting
joustingHardwareInit();
// Start in ReadyToJoust state
CurrentState = ReadyToJoust;
return true;
}
/****************************************************************************
Function
PostJousting
Parameters
Returns
Description
Posts an event to the Jousting module
****************************************************************************/
bool PostJousting( ES_Event ThisEvent )
{
}
/****************************************************************************
Function
RunJousting
Parameters
Returns
Description
Runs the Jousting state machine
****************************************************************************/
ES_Event RunJousting( ES_Event ThisEvent )
{
switch ( CurrentState )
{
case ReadyToJoust :
// This state means that the robot is ready to joust, but the joust is not deployed
if ((ThisEvent.EventType == ES_COMMAND) && (ThisEvent.EventParam ==
DEPLOY_JOUST_BODY))
{
// Brain wants to joust body. Deploy the joust and set the current state to jousting
CurrentState = Jousting;
deployJoust(AIM_JOUST_BODY);
}
else if ((ThisEvent.EventType == ES_COMMAND) && (ThisEvent.EventParam ==
DEPLOY_JOUST_HEAD))
{
// Brain wants to joust head. Deploy the joust and set the current state to jousting
CurrentState = Jousting;
deployJoust(AIM_JOUST_HEAD);
}
break;
case Jousting :
// This state means the robot's joust is deployed
JOUSTING_TIMER))
{
// Three second limit is up, retract the joust
CurrentState = JoustRetracted;
retractJoust();
}
break;
case JoustRetracted :
// This state means the robot's joust is retracted and it is not allowed
// to joust because it must wait 1 second
JOUSTING_TIMER))
{
// One second limit is up, move to ready to joust state
ES_Event ThisEvent;
ThisEvent.EventType = ES_JOUST_READY;
PostBrain(ThisEvent);
CurrentState = ReadyToJoust;
}
break;
}
return ReturnEvent;
}
/****************************************************************************
Function
QueryJousting
Parameters
None
Returns
TemplateState_t The current state of the Jousting state machine
Description
returns the current state of the Jousting state machine
****************************************************************************/
TemplateState_t QueryJousting( void )
{
}
/***************************************************************************
private functions
***************************************************************************/
/****************************************************************************
Function
joustingHardwareInit
Parameters
Nothing.
Returns
Nothing.
Description
Initializes all hardware for the jousting module, including the servo
****************************************************************************/
static void joustingHardwareInit(void)
{
enableServo(JOUST_SERVO);
setPulseWidth(JOUST_SERVO, JOUST_RETRACTED);
}
/****************************************************************************
Function
deployJoust
Parameters
Nothing.
Returns
Nothing.
Description
Deploys the joust and starts a three second timer before the joust must
be retracted
****************************************************************************/
static void deployJoust(unsigned int position)
{
setPulseWidth(JOUST_SERVO, position);
ES_Timer_InitTimer(JOUSTING_TIMER, THREE_SECONDS);
}
/****************************************************************************
Function
retractJoust
Parameters
Nothing.
Returns
Nothing.
Description
Retracts the joust and starts a one second timer before the joust can
be deployed again
****************************************************************************/
static void retractJoust(void)
{
setPulseWidth(JOUST_SERVO, JOUST_RETRACTED);
ES_Timer_InitTimer(JOUSTING_TIMER, ONE_SECOND);
Shooting Module:
Header:
/****************************************************************************
Header file for the shooting module

****************************************************************************/
#ifndef Shooting_H
#define Shooting_H
typedef enum { BallReady, GateOpen } TemplateState_t ;
bool InitShooting ( uint8_t Priority );
bool PostShooting( ES_Event ThisEvent );
ES_Event RunShooting( ES_Event ThisEvent );
TemplateState_t QueryShooting ( void );
#endif /* Shooting_H */
Source:
/****************************************************************************
Module
Shooting.c
Revision
1.0.1
Description
Shooting service for the 218B robot
****************************************************************************/
/*----------------------------- Include Files -----------------------------*/
*/
#include "Shooting.h"
#include <stdio.h>
#include <hidef.h>
constants */
#include <termio.h>
#include <s12vec.h>
#include "Servos.h"
#include "Brain.h"
/*----------------------------- Module Defines ----------------------------*/
*/
static void shootingHardwareInit(void);
static void startPitchingMachineMotor(void);

static void stopPitchingMachineMotor(void);
static void openGate(unsigned char gate);
static void closeGate(unsigned char gate);
/*------------------------------ Module Code ------------------------------*/
/****************************************************************************
Function
InitShooting
Parameters
Returns
Description
Initializes the shooting module
****************************************************************************/
bool InitShooting ( uint8_t Priority )
{
ES_Event ThisEvent;
// Init shooting hardware
shootingHardwareInit();
// Set CurrentState to BallReady and stop the pitching motor
CurrentState = BallReady;
stopPitchingMachineMotor();
return true;
}
/****************************************************************************
Function
PostShooting
Parameters
Returns
Description
Posts an event to the shooting module
****************************************************************************/
bool PostShooting( ES_Event ThisEvent )
{
}
/****************************************************************************
Function
RunShooting
Parameters
Returns
Description
State machine for the shooting module
****************************************************************************/
ES_Event RunShooting( ES_Event ThisEvent )

{
{
case BallReady :
if (ThisEvent.EventType == ES_COMMAND)
{
if (ThisEvent.EventParam == SHOOT_BALL)
{
// Brain wants to shoot all the balls, turn on the pitching machine
// motor and release a ball
puts("Shooting balls command \r");
CurrentState = GateOpen;
startPitchingMachineMotor();
openGate(BALL_GATE);
ES_Timer_InitTimer(BALL_RELEASE_TIMER, GATE_OPEN_TIME);
}
else if (ThisEvent.EventParam == START_PITCHING)
{
startPitchingMachineMotor();
}
else if (ThisEvent.EventParam == STOP_PITCHING)
{
}
}
break;
case GateOpen :
BALL_RELEASE_TIMER))
{
// All balls have been shot, post to brain and close the gate
BrainEvent.EventType = ES_BALL_SHOT;
CurrentState = BallReady;
puts("Shooting complete, close gate\r");
closeGate(BALL_GATE);
}
break;
}
return ReturnEvent;
}
/****************************************************************************
Function
QueryShooting
Parameters
None
Returns
TemplateState_t The current state of the shooting state machine
Description
returns the current state of the shooting state machine
****************************************************************************/
TemplateState_t QueryShooting ( void )

{
}
/***************************************************************************
private functions
***************************************************************************/
/****************************************************************************
Function
shootingHardwareInit
Parameters
Nothing.
Returns
Nothing.
Description
Initializes all hardware for the shooting module, including the pitching
motor, ball detector, and servo gate
****************************************************************************/
static void shootingHardwareInit(void)
{
BALL_DETECTOR_IR_LINE = INPUT;
PITCHING_MOTOR_LINE = OUTPUT;
PITCHING_MOTOR = LOW;
enableServo(BALL_GATE);
setPulseWidth(BALL_GATE, GATE_CLOSED);
}
/****************************************************************************
Function
startPitchingMotor
Parameters
Nothing.
Returns
Nothing.
Description
Turns the pitching motor on and starts a timer to turn the motor off
after a pre-determined amount of time
****************************************************************************/
static void startPitchingMachineMotor(void)
{
PITCHING_MOTOR = HIGH;
}
/****************************************************************************
Function
stopPitchingMotor
Parameters
Nothing.
Returns
Nothing.
Description
Turns off the pitching motor
****************************************************************************/
static void stopPitchingMachineMotor(void)
{
PITCHING_MOTOR = LOW;
}
/****************************************************************************
Function
openGate
Parameters
gate number
Returns
Nothing.
Description
Releases a ball by opening the back servo gate.
****************************************************************************/
static void openGate(unsigned char gate)
{
setPulseWidth(gate, GATE_OPEN);
}
/****************************************************************************
Function
closeFrontGate
Parameters
gate number
Returns
Nothing.
Description
Closes the front servo gate
****************************************************************************/
static void closeGate(unsigned char gate)
{
setPulseWidth(gate, GATE_CLOSED);
}
Ball Dispensing Module:

Header:
/****************************************************************************
Header file for the ball dispensing module
****************************************************************************/
#ifndef BallDispensing_H
#define BallDispensing_H
typedef enum { ReadyToCollect, CollectingBall, PulsingLEDs } TemplateState_t ;
bool InitBallDispensing ( uint8_t Priority );
bool PostBallDispensing( ES_Event ThisEvent );
ES_Event RunBallDispensing( ES_Event ThisEvent );
TemplateState_t QueryBallDispensing( void );

#endif /* BallDispensing_H */
Source:
/****************************************************************************
Module
BallDispensing.c
Revision
1.0.1
Description
Ball dispensing service for the 218B robot
****************************************************************************/
/*----------------------------- Include Files -----------------------------*/
*/
#include "BallDispensing.h"
#include <stdio.h>
#include <hidef.h>
constants */
#include <termio.h>
#include <s12vec.h>
#include "Brain.h"
/*----------------------------- Module Defines ----------------------------*/
*/
static void ballDispensingHardwareInit(void);
static void pulseIRs(void);
static void stopPulsingIRs(void);
static unsigned char requestCount=0,pulseCount=0;
static unsigned int timeLow=45000,timeHigh=15000;
/*------------------------------ Module Code ------------------------------*/
/****************************************************************************
Function
InitBallDispensing
Parameters

Returns
Description
Initializes the ball dispensing module
****************************************************************************/
bool InitBallDispensing ( uint8_t Priority )
{
ES_Event ThisEvent;
// Initialize hardware
ballDispensingHardwareInit();
// Start in the ready state
CurrentState = ReadyToCollect;
return true;
}
/****************************************************************************
Function
PostBallDispensing
Parameters
Returns
Description
Posts an event to the ball dispensing module
****************************************************************************/
bool PostBallDispensing( ES_Event ThisEvent )
{
}
/****************************************************************************
Function
RunBallDispensing
Parameters
Returns
Description
Runs the ball dispensing state machine
****************************************************************************/
ES_Event RunBallDispensing( ES_Event ThisEvent )
{
{
case ReadyToCollect:
// This state means the ball dispenser is ready to collect a ball
if (ThisEvent.EventType == ES_COLLECT_BALL)
{
puts("Collect a ball command \r");
// The brain wants to collect a ball. Pulse the IRs, start the timeout
// timer and set the current state to CollectingBall
pulseIRs();
ES_Timer_InitTimer(DISPENSING_TIMER, DISPENSING_TIME);
CurrentState = PulsingLEDs;
}
break;
case PulsingLEDs:
REQUEST_BALL_TIMER))
{
// Ball request sent, stop pulsing;
stopPulsingIRs();
CurrentState = CollectingBall;
}
else if (ThisEvent.EventType == ES_COLLECT_BALL)
{
// The brain is trying to collect a ball before it is ready. Post an error
BrainEvent.EventType = ES_BALL_DISPENSING_ERROR;
}
break;
case CollectingBall :
// This state means a ball is being collected. The brain should not try to collect
// another ball while a ball is being collected.
DISPENSING_TIMER))
{
// It has been three seconds since the start of ball collection so it
// is safe to collect another ball
BrainEvent.EventType = ES_BALL_COLLECTED;
}
else if (ThisEvent.EventType == ES_BALL_READY)
{
// The ball has been collected so move to ready
BrainEvent.EventType = ES_BALL_COLLECTED;
}
else if (ThisEvent.EventType == ES_COLLECT_BALL)
{
// The brain is trying to collect a ball before it is ready. Post an error
BrainEvent.EventType = ES_BALL_DISPENSING_ERROR;
}
break;
}
return ReturnEvent;
}
/****************************************************************************
Function
QueryBallDispensing
Parameters
None
Returns
Description
queries the state of the ball dispensing module
****************************************************************************/
TemplateState_t QueryBallDispensing ( void )
{
}
/***************************************************************************
private functions
***************************************************************************/
/****************************************************************************
Function
ballDispensingHardwareInit
Parameters
Nothing.
Returns
Nothing.
Description
Initializes all hardware for the ball dispensing module, including the
IR LED and the visible LED
****************************************************************************/
static void ballDispensingHardwareInit(void)
{
IR_LED_LINE = OUTPUT;
RELOAD_LED_LINE = OUTPUT;
IR_LED = LOW;
RELOAD_LED = LOW;
}
/****************************************************************************
Function
ballDispensingTimerInit
Parameters
Nothing.
Returns
Nothing.
Description
Initializes the timer for doing pulsing
****************************************************************************/
/****************************************************************************
Function
pulseIRs
Parameters
Nothing.
Returns
Nothing.
Description
Schedules the first output compare and resets the counter
****************************************************************************/
static void pulseIRs(void)
{
IR_LED_DUTY = 25;
ES_Timer_InitTimer(REQUEST_BALL_TIMER, 500);
}
/****************************************************************************
Function
stopPulsingIRs
Parameters
Nothing.
Returns
Nothing.
Description
Schedules the first output compare and resets the counter
****************************************************************************/
static void stopPulsingIRs(void)
{
IR_LED_DUTY = 0;
}
Event Checkers:
Header:
/****************************************************************************
Module
EventCheckers.h
Description
header file for the event checking functions
Notes
History
When
Who What/Why
-------------- --- -------08/06/13 14:37 jec
started coding
*****************************************************************************/
#ifndef EventCheckers_H
#define EventCheckers_H
// prototypes for event checkers
// prototypes for test event checkers
bool BallReadyChecker(void);
bool GoalReachedChecker(void);
bool WallAlignmentChecker(void);
bool Check4Keystroke(void);
#include <bitdefs.h>
// #defines that go with the Event Checkers
#endif /* EventCheckers_H */
Source:
/****************************************************************************
Module
EventCheckers.c
Revision
1.0.1
Description
This is the sample for writing event checkers along with the event
checkers used in the basic framework test harness.
Notes
Note the use of static variables in sample event checker to detect
ONLY transitions.
History
When
Who What/Why
-------------- --- -------08/06/13 13:36 jec initial version
****************************************************************************/
// this will pull in the symbolic definitions for events, which we will want
// to post in response to detecting events
// this will get us the structure definition for events, which we will need
// in order to post events in response to detecting events
#include "ES_Events.h"
// if you want to use distribution lists then you need those function
// definitions too.
#include "ES_PostList.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"
// this test harness for the framework references the serial routines that
// are defined in ES_Port.c
#include "ES_Port.h"
// include our own prototypes to insure consistency between header &
// actual functionsdefinition
// 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"
#include "Brain.h"
#include "Navigation.h"
bool BallReadyChecker(void)
{
static unsigned char LastPinState = 0;
unsigned char CurrentPinState;
bool ReturnVal = false;
CurrentPinState = BALL_DETECTOR_IR;
// check for pin low and different from last time
if ((CurrentPinState != LastPinState) && (CurrentPinState == LOW))
{
// Post to shooting that a ball is ready
ES_Event ThisEvent;
ThisEvent.EventType = ES_BALL_READY;
//PostNavigation(ThisEvent);
//PostShooting(ThisEvent); //TEMPORARY COMMENTOUT WHILE COMPILING STUFF
ReturnVal = true;
}
LastPinState = CurrentPinState;
return ReturnVal;
}
bool GoalReachedChecker(void)
{
Lab8State_t navigationState = QueryNavigation();
float currDistance = getBackUltrasonicDistance();
if (navigationState == DrivingToHomeA)
{
if (currDistance <= HOME_A_SONIC_DISTANCE)
{
ES_Event ThisEvent;
ThisEvent.EventType = ES_HOME_A_DETECTED;
PostNavigation(ThisEvent);
ReturnVal = true;
printf("Distance = %f \r \n", currDistance);
}
}
else if (navigationState == DrivingToHomeB)
{
if (currDistance >= HOME_B_SONIC_DISTANCE)
{
ES_Event ThisEvent;
ThisEvent.EventType = ES_HOME_B_DETECTED;
ReturnVal = true;
}
}
else if (navigationState == DrivingToDispenser)
{
if (currDistance <= DISPENSER_SONIC_DISTANCE)
{
ES_Event ThisEvent;
ThisEvent.EventType = ES_DISPENSER_DETECTED;
ReturnVal = true;
}
}
else if (navigationState == DrivingToTape)
{
if (currDistance <= (TAPE_SONIC_DISTANCE + 1))
{
ES_Event ThisEvent;
ThisEvent.EventType = ES_TAPE_DETECTED;
ReturnVal = true;
}
}
else if (navigationState == DrivingToGoal)
{
if (currDistance >= GOAL_SONIC_DISTANCE)
{
ES_Event ThisEvent;
ThisEvent.EventType = ES_GOAL_DETECTED;
ReturnVal = true;
}
}
return ReturnVal;
}
bool WallAlignmentChecker(void)
{
Lab8State_t navigationState = QueryNavigation();
float currDistanceDifference = getSideUltrasonicDistance1() - getSideUltrasonicDistance2();
if (navigationState == AligningWithWall)
{
if ((currDistanceDifference < 0.1) && (currDistanceDifference > -0.1))
{
ES_Event ThisEvent;
ThisEvent.EventType = ES_WALL_ALIGNMENT_COMPLETE;
ReturnVal = true;
printf("Wall Alignment complete \r \n");
}
}
return ReturnVal;
}
bool Check4Keystroke(void)
{
if ( IsNewKeyReady() ) // new key waiting?
{
ES_Event ThisEvent;
ES_Event NewEvent;
ThisEvent.EventType = ES_NEW_KEY;
ThisEvent.EventParam = GetNewKey();
// test distribution list functionality by sending the 'L' key out via
// a distribution list.
switch (ThisEvent.EventParam)
{
case 'R' :
puts("Generated new round event \r");
NewEvent.EventType = ES_NEW_ROUND;
setAllowedReload(true, true);
PostBrain(NewEvent);
break;
case 'A' :
puts("Generated home A detected event \r");
NewEvent.EventType = ES_HOME_A_DETECTED;
PostNavigation(NewEvent);
break;
case 'B' :
puts("Generated home B detected event \r");
NewEvent.EventType = ES_HOME_B_DETECTED;
break;
case 'D' :
puts("Generated dispenser detected event \r");
NewEvent.EventType = ES_DISPENSER_DETECTED;
break;
case 'K' :
puts("Generated knight detected event \r");
NewEvent.EventType = ES_KNIGHT_DETECTED;
break;
case 'G' :
puts("Generated goal detected event \r");
NewEvent.EventType = ES_GOAL_DETECTED;
break;
}
return true;
}
return false;
}
Servo Library:
Header:
// Servos.h
#ifndef Servos_H
#define Servos_H
void enableServo (unsigned char channel);
void setPulseWidth (unsigned char channel, unsigned int newWidth);
#endif /* Servos_H */
Source:
#include <string.h>
#include <stdio.h>
#include <hidef.h>
constants */
#include <termio.h>
#include <s12vec.h>
#include "Servos.h"
#define NUM_CHANNELS 4
#define INITIAL_POSITION 0xFFFF-4500
#define TicksPerMicroSec 3
// Channel Struct
typedef struct ChannelDef
{
char IOSelVal;
unsigned char * ModeReg;
unsigned char ModeVal;
unsigned char ToggleBit;
unsigned int * CompareReg;
} ChannelDef_t;
// Channel definitions
ChannelDef_t Channels[NUM_CHANNELS] =
{
{_S12_IOS4, &TIM1_TCTL1, (_S12_OM4 | _S12_OL4), _S12_TOV4, &TIM1_TC4 },
{_S12_IOS7, &TIM1_TCTL1, (_S12_OM7 | _S12_OL7), _S12_TOV7, &TIM1_TC7 }

};
/****************************************************************************
Function
enableServo
Parameters
Channel on Port T to enable servo (between 4 and 7)
Returns
Nothing.
Description
Initializes the corresponding pin on Port T as a servo pin
and sets it to the center position
****************************************************************************/
void enableServo (unsigned char channel)
{
unsigned char index = channel - NUM_CHANNELS;
TIM1_TSCR1 |= _S12_TEN;
// Enable Timer 1
TIM1_TSCR2 = _S12_PR1 | _S12_PR0;
// Set prescaler to /8 (3MHz timer clock)
TIM1_TIOS |= Channels[index].IOSelVal;
// Select channel for output compare
*(Channels[index].ModeReg) |= Channels[index].ModeVal;
// Program a rise on compare
TIM1_TTOV |= Channels[index].ToggleBit;
// Set bit to toggle overflow
*(Channels[index].CompareReg) = INITIAL_POSITION;
// Output pulse width for initial
position
}
/****************************************************************************
Function
setPulseWidth
Parameters
Channel on Port T to enable servo (between 4 and 7), and a new width
(between 1000 and 2000)
Returns
Nothing.
Description
Sets the pulse width on a channel to a new width between 1000 and 2000
****************************************************************************/
void setPulseWidth (unsigned char channel, unsigned int newWidth)
{
unsigned char index = channel - NUM_CHANNELS;
*(Channels[index].CompareReg) = 0xFFFF - (newWidth * TicksPerMicroSec);
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
-------------- --- -------10/21/13 20:54 jec
lots of added entries to bring the number of timers
and services up to 16 each
08/06/13 14:10 jec
removed PostKeyFunc stuff since we are moving that
functionality out of the framework and putting it
explicitly into the event checking functions
01/15/12 10:03 jec
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
// corresponding to an 8-bit(uint8_t) and 16-bit(uint16_t) Ready variable size
#define MAX_NUM_SERVICES 16
/****************************************************************************/
// 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 6
/****************************************************************************/
// 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
#define SERV_0_HEADER "Brain.h"
// the name of the Init function
#define SERV_0_INIT InitBrain
// the name of the run function
#define SERV_0_RUN RunBrain
// 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 "Navigation.h"

#define SERV_1_INIT InitNavigation
#define SERV_1_RUN RunNavigation
#endif
/****************************************************************************/
#define SERV_2_HEADER "Jousting.h"
#define SERV_2_INIT InitJousting
#define SERV_2_RUN RunJousting
#endif
/****************************************************************************/
#define SERV_3_HEADER "Shooting.h"
#define SERV_3_INIT InitShooting
#define SERV_3_RUN RunShooting
#endif
/****************************************************************************/
#define SERV_4_HEADER "BallDispensing.h"
#define SERV_4_INIT InitBallDispensing
#define SERV_4_RUN RunBallDispensing
#endif
/****************************************************************************/
#define SERV_5_HEADER "Comm.h"
#define SERV_5_INIT InitComm
#define SERV_5_RUN RunComm
#endif
/****************************************************************************/

#define SERV_6_HEADER "TestHarnessService6.h"
#define SERV_6_INIT InitTestHarnessService6
#define SERV_6_RUN RunTestHarnessService6
#endif
/****************************************************************************/
#endif
/****************************************************************************/
#endif
/****************************************************************************/
#endif
/****************************************************************************/
#endif
/****************************************************************************/
#endif
/****************************************************************************/
#endif
/****************************************************************************/
#endif
/****************************************************************************/
#endif
/****************************************************************************/

#endif
/****************************************************************************/
// 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_TIMEOUT, /* signals that the timer has expired */
/* User-defined events start here */
ES_NEW_KEY, /* signals a new key received from terminal */
// Brain Events
ES_NEW_ROUND,
ES_GAME_OVER,
ES_ROUND_OVER,
// Comm Events
ES_SEND_COMMAND, /* tells comm to send 4 byte message to JSR */
ES_BYTE_READ, /* posted by SPI interrupt that it read a byte */
// Navigation Events
ES_COMMAND,
ES_NAVIGATION_COMPLETE,
ES_NAVIGATION_FAILURE,
ES_INNER_INIT,
ES_KNIGHT_DETECTED,
ES_GOAL_DETECTED,
ES_HOME_A_DETECTED,
ES_HOME_B_DETECTED,
ES_WALL_ALIGNMENT_COMPLETE,
ES_TAPE_DETECTED,
ES_DEPOT_REACHED,
ES_DISPENSER_DETECTED,
// Shooting events
ES_BALL_READY,
ES_SHOOT_BALL,
ES_BALL_SHOT,
ES_BALL_ERROR,
// Jousting events
ES_DEPLOY_JOUST_BODY,
ES_DEPLOY_JOUST_HEAD,
ES_RETRACT_JOUST,
ES_JOUST_READY,
ES_JOUST_ERROR,
// Ball dispensing events
ES_COLLECT_BALL,
ES_BALL_COLLECTED,
ES_BALL_DISPENSING_ERROR
} 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 2
#if NUM_DIST_LISTS > 0

#define DIST_LIST0 PostNavigation
#endif
#define DIST_LIST1 PostNavigation
#endif
#define DIST_LIST2 PostTemplateFSM
#endif
#endif
#endif
#endif
#endif
#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 BallReadyChecker, GoalReachedChecker, Check4Keystroke,
WallAlignmentChecker
/****************************************************************************/
// These are the definitions for the post functions to be executed when the
// correspnding timer expires. All 16 must be defined. If you are not using
// a timer, then you should use TIMER_UNUSED
// Unlike services, any combination of timers may be used and there is no
// priority in servicing them
#define TIMER_UNUSED ((pPostFunc)0)
#define TIMER0_RESP_FUNC PostNavigation
#define TIMER2_RESP_FUNC PostShooting
#define TIMER3_RESP_FUNC PostShooting
#define TIMER4_RESP_FUNC PostJousting
#define TIMER5_RESP_FUNC PostBallDispensing
#define TIMER7_RESP_FUNC PostBrain
#define TIMER8_RESP_FUNC PostComm
#define TIMER11_RESP_FUNC PostBallDispensing
#define TIMER12_RESP_FUNC TIMER_UNUSED
/****************************************************************************/
// 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
// These symbolic names should be changed to be relevant to your application
#define SERVICE0_TIMER 15
#define REQUEST_BALL_TIMER 11
#define REQUEST_STATUS_TIMER 10
#define REQUEST_SCORE_TIMER 9
#define COMM_TIMER 8
#define BRAIN_TIMER 7
#define ALIGNMENT_DEBOUNCE_TIMER 6
#define DISPENSING_TIMER 5
#define JOUSTING_TIMER 4
#define BALL_RELEASE_TIMER 3
#define PITCHING_TIMER 2
#define NAVIGATION_TIMER 1
#define TURN_TIMER 0
#endif /* CONFIGURE_H */

Code Listings

Uploaded by

Copyright:

Available Formats

Code Listings

Uploaded by

Document Information

Original Description:

Copyright

Available Formats

Share this document

Share or Embed Document

Sharing Options

Did you find this document useful?

Is this content inappropriate?

Copyright:

Available Formats

Code Listings

Uploaded by

Copyright:

Available Formats

Full Code Listing

static bool isEndOfMatch = false;

uint8_t : the priorty of this service

static float lastDistance = 0;

//puts("Enter echo2 interrupt \r");

Header file for the shooting module

static void startPitchingMachineMotor(void);

ES_Event RunShooting( ES_Event ThisEvent )

TemplateState_t QueryShooting ( void )

Ball Dispensing Module:

TemplateState_t QueryBallDispensing( void );

uint8_t : the priorty of this service

{_S12_IOS7, &TIM1_TCTL1, (_S12_OM7 | _S12_OL7), _S12_TOV7, &TIM1_TC7 }

// the name of the Init function

#if NUM_SERVICES > 6

#define SERV_15_INIT InitTestHarnessService15

#if NUM_DIST_LISTS > 0

You might also like