Hierarchical State Machine Program Code
Our software was built using both Hierarchical State Machines (HSMs) and Flat State Machines. There are two major state machines, TopRobot and TopCommunication. TopRobot handles motion across the field, shooting and strategy, while TopCommunication handles communication with the SPI. The descriptions for the different State Machines are as follows:
State Charts
Top Robot
This top level HSM handles motion, shooting and strategy, implemented via each of its sub State Machines.
Top Communication
This State Machine handles the communication of the PAC with the Super-PAC and shuttles information back and forth to TopRobot upon initiation of an SPI Transfer request. All 5 bytes are sent to the Super-PAC at once, and the data from the Super-PAC is collected upon an End-of-Transmit Interrupt.
Substates
Station Capturing
This SM implements the entire sequence of SPI back and forth events to capture a state, including all the sub-cases wherein an Acknowledgement packet is not received, or if the robot is blocked by an AdAttack from an opposing team.
Game Intelligence
This SM updates the winning/losing status every time 3idiots captures a station. Further, the initial game start event, which switches on the green status LED on top of the robot happens in this SM. The information about the win/lose status is used to decide whether to shoot an AdAttack or not.
Shooting
This SM implements the “hard-coded” strategy to move towards the AdAttack bucket and shoot a ball in using the gate servo and the ruler servo. We only enter this state if we are winning by a margin of 3 and the time has crossed 38 seconds from the end of the game. Knowledge of which direction to move is found by keeping track of previous captured cities by querying the StationCapturing SM.
The zip file contains the entire program including the HSM framework that was used.
Alternatively, you can view the individual header file, and source file for each module by clicking on the links below.
Modules
Top Robot
Top level HSM which handles motion, shooting and strategy, implemented via each of its sub State Machines.
Header File
/****************************************************************************
Template header file for Hierarchical Sate Machines AKA StateCharts
****************************************************************************/
#ifndef TopRobot_H
#define TopRobot_H
// State definitions for use with the query function
typedef enum {GameIntelligence, LineFollowing, StationCapturing, LineFollowingAfterStationCaptured, Shooting } TopRobotState_t ;
// Public Function Prototypes
ES_Event RunTopRobot( ES_Event CurrentEvent );
void StartTopRobot ( ES_Event CurrentEvent );
bool PostTopRobot ( ES_Event ThisEvent );
bool InitTopRobot ( uint8_t Priority );
uint8_t QueryPollingCode(void);
uint8_t QueryRightWallDetection(void);
uint8_t QueryLeftWallDetection(void);
uint8_t QueryShootingAllowedStatus(void);
uint8_t QueryExitStationAllowed(void);
void SetPWMGateServo(uint16_t);
void SetPWMRulerServo(uint8_t);
void SetPWMLeftWheel(uint16_t Duty, uint8_t Direction);
void SetPWMRightWheel(uint16_t Duty, uint8_t Direction);
void TenSecondOneShotResponse(void);
void InitOneShotTimer(void);
void InitTenSecondOneShotTimer(void);
void GameOneShotTimerResponse(void);
void InitGameOneShotTimer(void);
void InitAutoDelayResponseOneShotTimer(void);
void AutoDelayResponse(void);
uint8_t QueryGameOverStatus (void);
#endif /*TopRobot_H */
Source File
/****************************************************************************
Module
TopRobot.c
Revision
2.0.1
Description
This is a template for the top level Hierarchical state machine
Notes
History
When Who What/Why
-------------- --- --------
02/12/16 14:10 ASY Done Fully.
****************************************************************************/
/*----------------------------- 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 "TopRobot.h"
#include "ES_DeferRecall.h"
#include "inc/hw_memmap.h"
#include "inc/hw_types.h"
#include "inc/hw_gpio.h"
#include "inc/hw_sysctl.h"
#include "driverlib/sysctl.h"
#include "driverlib/pin_map.h"
#include "driverlib/gpio.h"
#include "inc/hw_pwm.h"
#include "inc/hw_timer.h"
#include "inc/hw_ssi.h"
#include "inc/hw_nvic.h"
#include "driverlib/interrupt.h"
#include "inc/tm4c123gh6pm.h"
#include "GameIntelligence.h"
#include "ADMulti.h"
#include "StationCapturing.h"
#include "Shooting.h"
/*----------------------------- Module Defines ----------------------------*/
#define TicksPerMS 40000U
#define START_SHOOTING_COUNTDOWN 10000 //This value needs to be changed
#define LINE_FOLLOWER_PERIOD 8
#define PeriodInMS 1
#define PWMTicksPerMS 62 // her 1250 is 1k - Currently set to ~20k
#define ALL_BITS (0xff<<2)
#define NUM_HALL_SAMPLES 2 //PLAY - 5 WORKS REALLY REALLY WELL
#define WHEEL_STOP 5
#define SLOW_PWM 5
#define TICKS_PER_MS 3800
#define TICKS_PER_US 40
#define ONE_SEC 976
#define AFTER_STATION_CAPTURED_LINE_FOLLOWING_TIME 500 //TUNE THIS TIME!!!!!
#define YES_SHOOT 1
#define NO_SHOOT 0
#define NUM_FREQUENCIES 16
#define CONVERSION_FACTOR 320
#define TURN_180_TIME 1100 //NEED TO TUNE THIS VALUE.
#define INVALID_STATION_CODE 0
#define EXIT_STATION_CAPTURING_TIME 3000 //PLAY WITH VALUE
#define WALL_DETECTED 0
#define FWD 0
#define BACK 1
#define ROBOT_PWM 75 //IS SET POINT FOR COMPETITION
#define ServoPWMTicksPerMS 1250
#define ServoPeriodMS 20 // 50 Hz
//#define ServoPWMTicksPerMS 1250
//#define ServoPeriodMS 20
#define TURN_SPEED 69
#define NUM_BEACON_SAMPLES 5
#define SHOOTING_TIME 40000U
#define NUM_BEACON_FREQUENCIES 4
//Definitions for 50 Hz.
#define RULER_UP_PWM 3
#define RULER_DOWN_PWM 10
#define GATE_CLOSED 11590 // 7.4
#define GATE_OPEN 11912 // 4.7 -- RULER DOWN OPEN
#define LINE_FOLLOW_UPDATE_TIME 2
#define LEFT_MAGNETIC_SENSOR_AVERAGE_VALUE 750 //MIGHT NEED TO TUNE THIS VALUE. A bit risky
#define LEFT_MAGNETIC_SENSOR_VARIANCE_VALUE 1500 //Safe.
#define NUM_MAGNETIC_SAMPLES 100 //THIS IS 5 SECONDS SECONDS (EACH IS 50 MS LONG).
#define LINE_FOLLOW_AVERAGE_TIME 100
#define TEN_SECOND_TIME 10000
#define FIELD_ALLOWED 1
#define FIELD_NOT_ALLOWED 0
#define EXIT_ALLOWED 1
#define EXIT_NOT_ALLOWED 0
#define GAME_TIME 28000U
#define DELAY_TIME 70000U
#define GAME_OVER 1
#define GAME_STILL_ON 0
/*---------------------------- Module Functions ---------------------------*/
void InitPeriodicInt( void );
static ES_Event DuringGameIntelligence( ES_Event Event);
static ES_Event DuringStationCapturing( ES_Event Event);
void SetPWM(uint16_t);
void InitFeedbackLED(void);
void PWMInit(void);
void ADInit(void);
static uint32_t ADCData[4];
void SetPWMRightWheel(uint16_t Duty, uint8_t Direction);
void SetPWMLeftWheel(uint16_t Duty, uint8_t Direction);
static uint8_t CheckForPollingStation( uint32_t );
static uint8_t RequestColor(void);
static uint8_t CheckForBeacon( uint32_t);
uint8_t QueryPollingCode(void);
uint8_t QueryLongRangeBeaconSensor(void);
uint8_t QueryShortRangeBeaconSensor(void);
void InitHallCapture(void);
void InitWallSensors(void);
void InitWallSensorPeriodicInterupt(void);
void WallDetectionResponse(void);
void InitPWMServo(void);
void SetPWMServo(void);
void SetPWMGateServo(uint16_t);
void SetPWMRulerServo(uint8_t);
void InitOneShotTimer(void);
void InitFieldAverager(void);
void FieldAveragerResponse(void);
void InitTenSecondOneShotTimer(void);
uint8_t QueryRightWallDetection(void);
uint8_t QueryLeftWallDetection(void);
uint8_t QueryShootingAllowedStatus (void);
static ES_Event DuringShooting( ES_Event Event);
void OneShotResponse(void);
void TenSecondOneShotResponse(void);
void InitStationCapturingOneShotTimer(void);
void StationCapturingOneShotTimerResponse(void);
uint8_t QueryExitStationAllowed(void);
uint8_t QueryGameOverStatus (void);
void GameOneShotTimerResponse(void);
void InitGameOneShotTimer(void);
void InitAutoDelayResponseOneShotTimer(void);
void AutoDelayResponse(void);
/*---------------------------- 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 TopRobotState_t CurrentState;
// with the introduction of Gen2, we need a module level Priority var as well
static uint8_t MyPriority;
static int16_t LeftWireSensorReading;
static int16_t PreviousLeftWireSensorReading;
static int16_t PreviousRightWireSensorReading;
static int16_t RightWireSensorReading;
static int16_t error;
static int16_t previous_error;
static int16_t LeftDutyCycle;
static int16_t RightDutyCycle;
static int16_t LeftDutyCycleSetPoint = ROBOT_PWM +1; // MUST CHECK THAT THESE MATCH ON TERA TERM!!
static int16_t RightDutyCycleSetPoint = ROBOT_PWM;
double k_p = 0.0109;//PID Controller Values
double k_d = 0.36; //PID Controller Values.
static int16_t Derivative_Term;
static uint16_t AverageLeftField;
static uint32_t LastHallCapture = 0;
static uint32_t AverageHallPeriod = 0;
static uint32_t LastLongRangeBeaconCapture = 0;
static uint32_t AverageLongRangeBeaconPeriod = 0;
static uint32_t LastShortRangeBeaconCapture = 0;
static uint32_t AverageShortRangeBeaconPeriod = 0;
static uint32_t ConvertedAverageHallPeriod;
static uint32_t ConvertedLongRangeBeaconPeriod;
static uint32_t ConvertedShortRangeBeaconPeriod;
static bool CenterWallDetected = false;
static uint8_t Shooting_Allowed = NO_SHOOT;
static uint8_t Exit_Station_Capturing_Allowed = EXIT_NOT_ALLOWED;
static uint8_t Game_End_Status = GAME_STILL_ON;
// Polling station upper limits
static uint32_t PollingPeriodUpper[NUM_FREQUENCIES] = {1348, 1292, 1237,1181,1126,1070,1015,959,904,848,793,737,682,626,571,515};
static uint32_t PollingPeriodLower[NUM_FREQUENCIES] = {1318, 1262, 1207,1151,1096,1040,985,929,874,818,763,707,652,596,541,485};
//static uint32_t PollingPeriodUpper[NUM_FREQUENCIES] = {1343, 1287, 1232,1176,1121,1065,1010,954,859,843,788,732,677,621,566,510}; //TIGHTER TOLERANCE
//static uint32_t PollingPeriodLower[NUM_FREQUENCIES] = {1323, 1267, 1212,1156,1101,1045,990,934,879,823, 768,712,657,601,546,490}; //TIGHTER TOLERANCE
static uint8_t PollingStationCode [NUM_FREQUENCIES] = {0xF0,0xF1,0xF2,0xF3,0xF4,0xF5,0xF6,0xF7,0xF8,0xF9,0xFA,0xFB,0xFC,0xFD,0xFE,0xFF};
static uint32_t BeaconPeriodLower[NUM_BEACON_FREQUENCIES] = {678,580,784,506};
static uint32_t BeaconPeriodUpper[NUM_BEACON_FREQUENCIES] = {701,597,816,519};
static uint8_t BeaconCodeList[NUM_BEACON_FREQUENCIES] = {0x01,0x02,0x03,0x04}; // 01 - Red Ad Attack Buckt, whereas 03 - BlueAdAttackBucket!
static uint8_t PollingReturnCode = 0x00;
static uint8_t TeamColor;
// Variables for wall detection sensors
static uint8_t WallDectionLeft = 1;
static uint8_t WallDectionRight = 1;
static uint8_t WallDectionCenter = 1;
static uint16_t LeftFieldAve[NUM_MAGNETIC_SAMPLES] = {0};
static uint8_t Field_Averaging_Allowed = FIELD_NOT_ALLOWED;
double Left_Field_Variance;
//double k_d = 0.01;
/*------------------------------ Module Code ------------------------------*/
/****************************************************************************
Function
InitTopRobot
Parameters
uint8_t : the priorty of this service
Returns
boolean, False if error in initialization, True otherwise
Description
Saves away the priority, and starts
the top level state machine
Notes
****************************************************************************/
bool InitTopRobot ( uint8_t Priority )
{
ES_Event ThisEvent;
MyPriority = Priority; // save our priority
ThisEvent.EventType = ES_ENTRY;
InitPeriodicInt();
InitHallCapture();
InitFeedbackLED();
InitWallSensors();
InitWallSensorPeriodicInterupt();
PWMInit();
ADInit();
TeamColor = RequestColor();
StartTopRobot( ThisEvent );
InitPWMServo();
SetPWMRulerServo(RULER_UP_PWM);
SetPWMGateServo(GATE_CLOSED);
InitFieldAverager();
HWREG(GPIO_PORTC_BASE+ (GPIO_O_DATA + ALL_BITS)) &= ~GPIO_PIN_7; //PC7 - SWITCH OFF THE GAME STATUS LED.
InitStationCapturingOneShotTimer();
return true;
}
/****************************************************************************
Function
PostTopRobot
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
Notes
****************************************************************************/
bool PostTopRobot( ES_Event ThisEvent )
{
return ES_PostToService( MyPriority, ThisEvent);
}
/****************************************************************************
Function
RunTopRobot
Parameters
ES_Event: the event to process
Returns
ES_Event: an event to return
Description
the run function for the top level state machine
Notes
uses nested switch/case to implement the machine.
Author
J. Edward Carryer, 02/06/12, 22:09
****************************************************************************/
ES_Event RunTopRobot( ES_Event CurrentEvent )
{
bool MakeTransition = false;/* are we making a state transition? */
TopRobotState_t NextState = CurrentState;
ES_Event EntryEventKind = { ES_ENTRY, 0 };// default to normal entry to new state
ES_Event ReturnEvent = { ES_NO_EVENT, 0 }; // assume no error
switch ( CurrentState )
{
case GameIntelligence :
CurrentEvent = DuringGameIntelligence(CurrentEvent);
if ( CurrentEvent.EventType != ES_NO_EVENT ) //If an event is active
{
if((ES_GAME_TACTIC == CurrentEvent.EventType) && (0x00 == CurrentEvent.EventParam)){
NextState = LineFollowing;//Decide what the next state will be
MakeTransition = true; //mark that we are taking a transition
ES_Timer_InitTimer(LineFollowingTimer, LINE_FOLLOWER_PERIOD); //Starts the Line Follower Timer (50ms Updates).
}
}
break;
case LineFollowing:
if ( CurrentEvent.EventType != ES_NO_EVENT ) //If an event is active
{
// Convert the average period in ticks returned from the interrupt
ConvertedAverageHallPeriod = AverageHallPeriod / TICKS_PER_US;
// Generate event with paramater value equal to the polling return code
CenterWallDetected = false;
if(ES_TIMEOUT == CurrentEvent.EventType && LineFollowingTimer == CurrentEvent.EventParam){
if (FIELD_ALLOWED == Field_Averaging_Allowed && AverageLeftField < LEFT_MAGNETIC_SENSOR_AVERAGE_VALUE && Left_Field_Variance < LEFT_MAGNETIC_SENSOR_VARIANCE_VALUE) {
SetPWMLeftWheel (WHEEL_STOP,FWD);
SetPWMRightWheel (WHEEL_STOP,FWD);
if (Game_End_Status == GAME_STILL_ON){
SetPWMRightWheel (TURN_SPEED, BACK);
SetPWMLeftWheel (TURN_SPEED, FWD);
}
if (Game_End_Status == GAME_OVER){
SetPWMLeftWheel (WHEEL_STOP,FWD);
SetPWMRightWheel (WHEEL_STOP,FWD);
}
ES_Timer_InitTimer(LineFollowingTimer, TURN_180_TIME);
CenterWallDetected = true;
NextState = LineFollowing;
MakeTransition = false;
Field_Averaging_Allowed = FIELD_NOT_ALLOWED;
InitTenSecondOneShotTimer();
}
else{
NextState = LineFollowing;//Decide what the next state will be
if (Game_End_Status == GAME_STILL_ON){
SetPWMLeftWheel (LeftDutyCycle,FWD);
SetPWMRightWheel (RightDutyCycle,FWD);
}
if (Game_End_Status == GAME_OVER){
SetPWMLeftWheel (WHEEL_STOP,FWD);
SetPWMRightWheel (WHEEL_STOP,FWD);
HWREG(GPIO_PORTC_BASE+ (GPIO_O_DATA + ALL_BITS)) &= ~(GPIO_PIN_7); //PC7 - SWITCH OFF THE GAME STATUS LED.
}
ES_Timer_InitTimer(LineFollowingTimer, LINE_FOLLOWER_PERIOD);
MakeTransition = false; //mark that we are taking a transition
CurrentEvent.EventType = ES_STATION_DETECTED;
CurrentEvent.EventParam = CheckForPollingStation(ConvertedAverageHallPeriod);
if ( CurrentEvent.EventParam != INVALID_STATION_CODE){
SetPWMLeftWheel (SLOW_PWM,FWD);
SetPWMRightWheel (SLOW_PWM,FWD);
MakeTransition = true;
NextState = StationCapturing;
PostTopRobot(CurrentEvent);
InitStationCapturingOneShotTimer();
Exit_Station_Capturing_Allowed = EXIT_NOT_ALLOWED;
}
}
}
}
break;
case StationCapturing:
CurrentEvent = DuringStationCapturing(CurrentEvent);
if ( CurrentEvent.EventType != ES_NO_EVENT ) //If an event is active
{
if(ES_STATION_CAPTURED == CurrentEvent.EventType && CurrentEvent.EventParam == NO_SHOOT){//This percolates in from the lower level state machine
NextState = LineFollowingAfterStationCaptured;//Decide what the next state will be
MakeTransition = true; //mark that we are taking a transition
// After capturing a station, continue for a few seconds, and set the periodic timer for control law
ES_Timer_InitTimer(LineFollowingTimer, LINE_FOLLOWER_PERIOD);
ES_Timer_InitTimer(LineFollowingAfterStationCapturedTimer, AFTER_STATION_CAPTURED_LINE_FOLLOWING_TIME);
}
if(ES_STATION_CAPTURED == CurrentEvent.EventType && CurrentEvent.EventParam == YES_SHOOT){//This percolates in from the lower level state machine
NextState = Shooting;//Decide what the next state will be
MakeTransition = true; //mark that we are taking a transition
CurrentEvent.EventType = ES_START_SHOOTING;
PostTopRobot(CurrentEvent);
// After capturing a station, continue for a few seconds, and set the periodic timer for control law
ES_Timer_InitTimer(LineFollowingTimer, LINE_FOLLOWER_PERIOD);
ES_Timer_InitTimer(LineFollowingAfterStationCapturedTimer, AFTER_STATION_CAPTURED_LINE_FOLLOWING_TIME);
}
if(ES_EXIT_STATION_READY == CurrentEvent.EventType) {
NextState = LineFollowingAfterStationCaptured;//Decide what the next state will be
MakeTransition = true; //mark that we are taking a transition
// After capturing a station, continue for a few seconds, and set the periodic timer for control law
ES_Timer_InitTimer(LineFollowingTimer, LINE_FOLLOWER_PERIOD);
ES_Timer_InitTimer(LineFollowingAfterStationCapturedTimer, AFTER_STATION_CAPTURED_LINE_FOLLOWING_TIME);
}
}
break;
// after you capture a station, you want to avoid re-capturing the same one
case LineFollowingAfterStationCaptured:
if ( CurrentEvent.EventType != ES_NO_EVENT ) //If an event is active
{
HWREG(GPIO_PORTB_BASE+ (GPIO_O_DATA + ALL_BITS)) &= ~ GPIO_PIN_3;
// implement the PID for about one second to move away from the station
if(ES_TIMEOUT == CurrentEvent.EventType && LineFollowingTimer == CurrentEvent.EventParam){
NextState = LineFollowingAfterStationCaptured;//Decide what the next state will be
SetPWMLeftWheel (LeftDutyCycle,FWD);
SetPWMRightWheel (RightDutyCycle,FWD);
ES_Timer_InitTimer(LineFollowingTimer, LINE_FOLLOWER_PERIOD);
MakeTransition = false; //mark that we are taking a transition
}
if(ES_TIMEOUT == CurrentEvent.EventType && LineFollowingAfterStationCapturedTimer == CurrentEvent.EventParam){
NextState = GameIntelligence;
AverageHallPeriod = 0;
MakeTransition = true;
}
}
break;
case Shooting:
CurrentEvent = DuringShooting(CurrentEvent);
if ( CurrentEvent.EventType != ES_NO_EVENT ){ //If an event is active
}
break;
}
// If we are making a state transition
if (MakeTransition == true)
{
// Execute exit function for current state
CurrentEvent.EventType = ES_EXIT;
RunTopRobot(CurrentEvent);
CurrentState = NextState; //Modify state variable
// Execute entry function for new state
// this defaults to ES_ENTRY
RunTopRobot(EntryEventKind);
}
// in the absence of an error the top level state machine should
// always return ES_NO_EVENT, which we initialized at the top of func
return(ReturnEvent);
}
/****************************************************************************
Function
StartTopRobot
Parameters
ES_Event CurrentEvent
Returns
nothing
Description
Does any required initialization for this state machine
Notes
****************************************************************************/
void StartTopRobot ( ES_Event CurrentEvent )
{
CurrentState = GameIntelligence;
RunTopRobot(CurrentEvent);
return;
}
/***************************************************************************
private functions
***************************************************************************/
static ES_Event DuringGameIntelligence( ES_Event Event)
{
ES_Event ReturnEvent = Event; // assme no re-mapping or comsumption
// process ES_ENTRY, ES_ENTRY_HISTORY & ES_EXIT events
if ( (Event.EventType == ES_ENTRY) ||
(Event.EventType == ES_ENTRY_HISTORY) )
{
StartGameIntelligence(Event);
}
else if ( Event.EventType == ES_EXIT )
{
RunGameIntelligence(Event);
}else
{
ReturnEvent = RunGameIntelligence(Event);
}
return(ReturnEvent);
}
static ES_Event DuringStationCapturing( ES_Event Event)
{
ES_Event ReturnEvent = Event; // assme no re-mapping or comsumption
// process ES_ENTRY, ES_ENTRY_HISTORY & ES_EXIT events
if ( (Event.EventType == ES_ENTRY) ||
(Event.EventType == ES_ENTRY_HISTORY) )
{
StartStationCapturing(Event);
}
else if ( Event.EventType == ES_EXIT )
{
RunStationCapturing(Event);
}else
{
ReturnEvent = RunStationCapturing(Event);
}
return(ReturnEvent);
}
static ES_Event DuringShooting( ES_Event Event)
{
ES_Event ReturnEvent = Event; // assme no re-mapping or comsumption
// process ES_ENTRY, ES_ENTRY_HISTORY & ES_EXIT events
if ( (Event.EventType == ES_ENTRY) ||
(Event.EventType == ES_ENTRY_HISTORY) )
{
StartShooting(Event);
}
else if ( Event.EventType == ES_EXIT )
{
RunShooting(Event);
}else
{
ReturnEvent = RunShooting(Event);
}
return(ReturnEvent);
}
void InitPeriodicInt( void ){ //Use Wide Timer0 Sub-Timer B for this lab.
volatile uint32_t Dummy; // use volatile to avoid over-optimization
// start by enabling the clock to the timer (Wide Timer 0)
HWREG(SYSCTL_RCGCWTIMER) |= SYSCTL_RCGCWTIMER_R0; // kill a few cycles to let the clock get going
Dummy = HWREG(SYSCTL_RCGCGPIO);
// make sure that timer (Timer B) is disabled before configuring
HWREG(WTIMER0_BASE+TIMER_O_CTL) &= ~TIMER_CTL_TBEN;
// set it up in 32bit wide (individual, not concatenated) mode
HWREG(WTIMER0_BASE+TIMER_O_CFG) = TIMER_CFG_16_BIT;
// set up timer B in periodic mode so that it repeats the time-outs
HWREG(WTIMER0_BASE+TIMER_O_TBMR) =(HWREG(WTIMER0_BASE+TIMER_O_TBMR)& ~TIMER_TBMR_TBMR_M)|TIMER_TBMR_TBMR_PERIOD;
// set timeout to 2mS by writing into the Interval Load Register (ILR)
HWREG(WTIMER0_BASE+TIMER_O_TBILR) = TicksPerMS * LINE_FOLLOW_UPDATE_TIME;
// enable a local timeout interrupt
HWREG(WTIMER0_BASE+TIMER_O_IMR) |= TIMER_IMR_TBTOIM;
// enable the Timer B in Wide Timer 0 interrupt in the NVIC
// it is interrupt number 95 so appears in EN2 at bit 30
HWREG(NVIC_EN2) = BIT31HI;
//HWREG(WTIMER0_BASE+0x45C) = BIT29HI; //Make this interrupt LOWER priority than the encoder interrupt
// make sure interrupts are enabled globally
__enable_irq();
// now kick the timer off by enabling it and enabling the timer to
// stall while stopped by the debugger
HWREG(WTIMER0_BASE+TIMER_O_CTL) |= (TIMER_CTL_TBEN |TIMER_CTL_TBSTALL);
}
void PWMInit(void) //This is to initialize the PWM service the proper way using the TIVA timing subystem We will USE PB6 AND PB7.
{
volatile uint32_t Dummy; // use volatile to avoid over-optimization
// start by enabling the clock to the PWM Module (PWM0)
HWREG(SYSCTL_RCGCPWM) |= SYSCTL_RCGCPWM_R0;
// enable the clock to Port B
HWREG(SYSCTL_RCGCGPIO) |= SYSCTL_RCGCGPIO_R1;
// Select the PWM clock as System Clock/32
HWREG(SYSCTL_RCC) = (HWREG(SYSCTL_RCC) & ~SYSCTL_RCC_PWMDIV_M) |(SYSCTL_RCC_USEPWMDIV | SYSCTL_RCC_PWMDIV_32);
// make sure that the PWM module clock has gotten going
while ((HWREG(SYSCTL_PRPWM) & SYSCTL_PRPWM_R0) != SYSCTL_PRPWM_R0);
// disable the PWM while initializing
HWREG( PWM0_BASE+PWM_O_0_CTL ) = 0;
// program generator A to go to 1 at rising compare A, 0 on falling compare A
HWREG( PWM0_BASE+PWM_O_0_GENA) =
(PWM_0_GENA_ACTCMPAU_ONE | PWM_0_GENA_ACTCMPAD_ZERO );
// program generator B to go to 1 at rising compare B, 0 on falling compare B
HWREG( PWM0_BASE+PWM_O_0_GENB) =
(PWM_0_GENB_ACTCMPBU_ONE | PWM_0_GENB_ACTCMPBD_ZERO );
// Set the PWM period. Since we are counting both up & down, we initialize
// the load register to 1/2 the desired total period. We will also program
// the match compare registers to 1/2 the desired high time
HWREG( PWM0_BASE+PWM_O_0_LOAD) = (((PeriodInMS * PWMTicksPerMS)-1) >> 1);
// Set the Duty cycle on A and B to 0% by programming the compare value
// to the load value. (Period/2 - 0 = Period/2)
// Since the CmpADn action (set to zero) wins, the output stays low
HWREG( PWM0_BASE+PWM_O_0_CMPA) = HWREG( PWM0_BASE+PWM_O_0_LOAD);
HWREG( PWM0_BASE+PWM_O_0_CMPB) = HWREG( PWM0_BASE+PWM_O_0_LOAD);
// enable the PWM outputs
HWREG( PWM0_BASE+PWM_O_ENABLE) |= (PWM_ENABLE_PWM0EN|PWM_ENABLE_PWM1EN); //Enables PB6 (LEFT) and PB7(RIGHT)
// now configure the Port B pins to be PWM outputs
// start by selecting the alternate function for PB6 & 7
HWREG(GPIO_PORTB_BASE+GPIO_O_AFSEL) |= (BIT6HI|BIT7HI);
// now choose to map PWM to those pins, this is a mux value of 4 that we
// want to use for specifying the function on bits 6, 7, 1, 2
HWREG(GPIO_PORTB_BASE+GPIO_O_PCTL) = (HWREG(GPIO_PORTB_BASE+GPIO_O_PCTL) & 0x00ffffff) +(4<<(6*4)) +(4<<(7*4));
// Enable pins 6 & 7 on Port B for digital I/O 1, 2, 4, 5, 6, 7
HWREG(GPIO_PORTB_BASE+GPIO_O_DEN) |= (BIT6HI|BIT7HI|BIT2HI|BIT1HI); // adding inits for motor dir, servos
// make pins 6 & 7 on Port B into outputs, 1, 2, 6, 7
HWREG(GPIO_PORTB_BASE+GPIO_O_DIR) |= (BIT6HI|BIT7HI|BIT2HI|BIT1HI);
// set the up/down count mode, enable the PWM generator and make
// both generator updates locally synchronized to zero count
HWREG(PWM0_BASE+ PWM_O_0_CTL) = ((PWM_0_CTL_MODE | PWM_0_CTL_ENABLE |PWM_0_CTL_GENAUPD_LS | PWM_0_CTL_GENBUPD_LS));
}
void InitPWMServo(void)
{
volatile uint32_t Dummy; // use volatile to avoid over-optimization
// start by enabling the clock to the PWM Module (PWM0)
HWREG(SYSCTL_RCGCPWM) |= SYSCTL_RCGCPWM_R0;
// enable the clock to Port B
HWREG(SYSCTL_RCGCGPIO) |= SYSCTL_RCGCGPIO_R1;
// Select the PWM clock as System Clock/32
HWREG(SYSCTL_RCC) = (HWREG(SYSCTL_RCC) & ~SYSCTL_RCC_PWMDIV_M) |(SYSCTL_RCC_USEPWMDIV | SYSCTL_RCC_PWMDIV_32);
// make sure that the PWM module clock has gotten going
while ((HWREG(SYSCTL_PRPWM) & SYSCTL_PRPWM_R0) != SYSCTL_PRPWM_R0);
// disable the PWM while initializing (PWM GENERATOR 1)
HWREG( PWM0_BASE+PWM_O_1_CTL ) = 0;
// program generator A to go to 1 at rising compare A, 0 on falling compare A
HWREG( PWM0_BASE+PWM_O_1_GENA) =
(PWM_0_GENA_ACTCMPAU_ONE | PWM_0_GENA_ACTCMPAD_ZERO );
// program generator B to go to 1 at rising compare B, 0 on falling compare B
HWREG( PWM0_BASE+PWM_O_1_GENB) =
(PWM_0_GENB_ACTCMPBU_ONE | PWM_0_GENB_ACTCMPBD_ZERO );
// Set the PWM period. Since we are counting both up & down, we initialize
// the load register to 1/2 the desired total period. We will also program
// the match compare registers to 1/2 the desired high time
HWREG( PWM0_BASE+PWM_O_1_LOAD) = (((ServoPeriodMS * ServoPWMTicksPerMS)-1) >> 1);
// Set the Duty cycle on A and B to 0% by programming the compare value
// to the load value. (Period/2 - 0 = Period/2)
// Since the CmpADn action (set to zero) wins, the output stays low
HWREG( PWM0_BASE+PWM_O_1_CMPA) = HWREG( PWM0_BASE+PWM_O_1_LOAD);
HWREG( PWM0_BASE+PWM_O_1_CMPB) = HWREG( PWM0_BASE+PWM_O_1_LOAD);
// enable the PWM outputs
HWREG( PWM0_BASE+PWM_O_ENABLE) |= (PWM_ENABLE_PWM2EN|PWM_ENABLE_PWM3EN); //PB4(Gate) PB5(RULER)
// now configure the Port B pins to be PWM outputs
// start by selecting the alternate function for PB4 (GATE) & PB5(RULER)
HWREG(GPIO_PORTB_BASE+GPIO_O_AFSEL) |= (BIT4HI|BIT5HI);
// now choose to map PWM to those pins, this is a mux value of 4 that we
// want to use for specifying the function on bits 4, 5
HWREG(GPIO_PORTB_BASE+GPIO_O_PCTL) = (HWREG(GPIO_PORTB_BASE+GPIO_O_PCTL) & 0xff00ffff) +(4<<(4*4)) +(4<<(5*4));
// Enable pins 4 & 5 on Port B for digital I/O 4, 5
HWREG(GPIO_PORTB_BASE+GPIO_O_DEN) |= (BIT4HI|BIT5HI); // inits for servos
// make pins 4 & 5 on Port B into outputs 4, 5,
HWREG(GPIO_PORTB_BASE+GPIO_O_DIR) |= (BIT4HI|BIT5HI);
// set the up/down count mode, enable the PWM generator and make
// both generator updates locally synchronized to zero count
HWREG(PWM0_BASE+ PWM_O_1_CTL) = ((PWM_0_CTL_MODE | PWM_0_CTL_ENABLE |PWM_0_CTL_GENAUPD_LS | PWM_0_CTL_GENBUPD_LS));
}
// PWM function using generator 1 to set the Gate PWM
void SetPWMGateServo(uint16_t GateServoTime ){
HWREG( PWM0_BASE+PWM_O_1_GENB) = (PWM_0_GENB_ACTCMPBU_ONE | PWM_0_GENB_ACTCMPBD_ZERO );
HWREG( PWM0_BASE+PWM_O_1_CMPB) = GateServoTime;
}
// PWM function using generator 1 to set the Ruler PWM
void SetPWMRulerServo( uint8_t RulerDuty) { //need to work on the Direction.
if (RulerDuty==100){
HWREG( PWM0_BASE+PWM_O_1_CMPA) = 0;
HWREG( PWM0_BASE+PWM_O_1_GENA) = (PWM_0_GENA_ACTCMPAU_ONE | PWM_0_GENA_ACTCMPAD_ONE );
}
else{
HWREG( PWM0_BASE+PWM_O_1_GENA) = (PWM_0_GENA_ACTCMPAU_ONE | PWM_0_GENA_ACTCMPAD_ZERO );
HWREG( PWM0_BASE+PWM_O_1_CMPA) = ((99-RulerDuty)*((ServoPeriodMS * ServoPWMTicksPerMS)/2))/100;
}
}
void ADInit(void)
{
HWREG(SYSCTL_RCGCGPIO) |= SYSCTL_RCGCGPIO_R4;
HWREG(GPIO_PORTE_BASE+GPIO_O_DEN) |= (GPIO_PIN_0 | GPIO_PIN_1); //Initialize Port E Bit 0 & 1 to be digital
while ((HWREG(SYSCTL_PRGPIO) & SYSCTL_PRGPIO_R4) != SYSCTL_PRGPIO_R4);
HWREG(GPIO_PORTE_BASE+GPIO_O_DIR) &= ~(GPIO_PIN_0 | GPIO_PIN_1); //Initialize Port E Bit 0 & 1 to be input
ADC_MultiInit(4); //Initialize PE0 & PE1 as an A/D pin
}
void ControlLawPeriodicResponse (void){
// Using wide timer 0 subtimer B
HWREG(WTIMER0_BASE+TIMER_O_ICR) = TIMER_ICR_TBTOCINT; //Clear the source of the interrupt.
ADC_MultiRead(ADCData);
LeftWireSensorReading = ADCData[0];
RightWireSensorReading = ADCData[1];
error = LeftWireSensorReading - RightWireSensorReading;
previous_error = PreviousLeftWireSensorReading - PreviousRightWireSensorReading;
Derivative_Term = ((error-previous_error)*k_d)/ (LINE_FOLLOW_UPDATE_TIME); //This is in UNITS OF ms - 2ms
LeftDutyCycle = LeftDutyCycleSetPoint - k_p*error - k_d*Derivative_Term;
RightDutyCycle = RightDutyCycleSetPoint + k_p*error + k_d*Derivative_Term;
if(LeftDutyCycle<5){
LeftDutyCycle = 5;}
if(LeftDutyCycle>95){
LeftDutyCycle =95;}
if(RightDutyCycle<5){
RightDutyCycle = 5;}
if(RightDutyCycle>95){
RightDutyCycle =95;}
PreviousLeftWireSensorReading = LeftWireSensorReading;
PreviousRightWireSensorReading = RightWireSensorReading;
}
void SetPWMLeftWheel(uint16_t Duty, uint8_t Direction){
if(Direction == FWD) { //FORWARD
HWREG(GPIO_PORTB_BASE+(GPIO_O_DATA + ALL_BITS)) &= BIT1LO; // PB7 Pin
if (Duty==100){
HWREG( PWM0_BASE+PWM_O_0_CMPB) = 0;
HWREG( PWM0_BASE+PWM_O_0_GENB) = (PWM_0_GENB_ACTCMPBU_ONE | PWM_0_GENB_ACTCMPBD_ONE);
}
else{
HWREG( PWM0_BASE+PWM_O_0_GENB) = (PWM_0_GENB_ACTCMPBU_ONE | PWM_0_GENB_ACTCMPBD_ZERO );
HWREG( PWM0_BASE+PWM_O_0_CMPB) = ((100-Duty)*((PeriodInMS * PWMTicksPerMS)/2))/100;
}
}
if(Direction != FWD) { //BACK
HWREG(GPIO_PORTB_BASE+(GPIO_O_DATA + ALL_BITS)) |= BIT1HI; // PB7 Pin
Duty = 100-Duty;
if (Duty==100){
HWREG( PWM0_BASE+PWM_O_0_CMPB) = 0;
HWREG( PWM0_BASE+PWM_O_0_GENB) = (PWM_0_GENB_ACTCMPBU_ONE | PWM_0_GENB_ACTCMPBD_ONE);
}
else{
HWREG( PWM0_BASE+PWM_O_0_GENB) = (PWM_0_GENB_ACTCMPBU_ONE | PWM_0_GENB_ACTCMPBD_ZERO );
HWREG( PWM0_BASE+PWM_O_0_CMPB) = ((100-Duty)*((PeriodInMS * PWMTicksPerMS)/2))/100;
}
}
}
void SetPWMRightWheel(uint16_t Duty, uint8_t Direction ){ //need to work on the Direction.
if(Direction == FWD) { //FORWARD
HWREG(GPIO_PORTB_BASE+(GPIO_O_DATA + ALL_BITS)) &= BIT2LO; // PB6 pin
if (Duty==100){
HWREG( PWM0_BASE+PWM_O_0_CMPA) = 0;
HWREG( PWM0_BASE+PWM_O_0_GENA) = (PWM_0_GENA_ACTCMPAU_ONE | PWM_0_GENA_ACTCMPAD_ONE );
}
else{
HWREG( PWM0_BASE+PWM_O_0_GENA) = (PWM_0_GENA_ACTCMPAU_ONE | PWM_0_GENA_ACTCMPAD_ZERO );
HWREG( PWM0_BASE+PWM_O_0_CMPA) = ((99-Duty)*((PeriodInMS * PWMTicksPerMS)/2))/100;
}
}
if(Direction != FWD) { //BACK
HWREG(GPIO_PORTB_BASE+ (GPIO_O_DATA + ALL_BITS)) |= BIT2HI; // PB6 pin
Duty = 100-Duty;
if (Duty==100){
HWREG( PWM0_BASE+PWM_O_0_CMPA) = 0;
HWREG( PWM0_BASE+PWM_O_0_GENA) = (PWM_0_GENA_ACTCMPAU_ONE | PWM_0_GENA_ACTCMPAD_ONE );
}
else{
HWREG( PWM0_BASE+PWM_O_0_GENA) = (PWM_0_GENA_ACTCMPAU_ONE | PWM_0_GENA_ACTCMPAD_ZERO );
HWREG( PWM0_BASE+PWM_O_0_CMPA) = ((99-Duty)*((PeriodInMS * PWMTicksPerMS)/2))/100;
}
}
}
void InitHallCapture(void ){//WideTimer0 SubTimer A
// start by enabling the clock to the timer (Wide Timer 0)
HWREG(SYSCTL_RCGCWTIMER) |= SYSCTL_RCGCWTIMER_R0;
// enable the clock to Port C
HWREG(SYSCTL_RCGCGPIO) |= SYSCTL_RCGCGPIO_R2;
// since we added this Port C clock init, we can immediately start
// into configuring the timer, no need for further delay
// make sure that timer (Timer A) is disabled before configuring
HWREG(WTIMER0_BASE+TIMER_O_CTL) &= ~TIMER_CTL_TAEN;
// set it up in 32bit wide (individual, not concatenated) mode
// the constant name derives from the 16/32 bit timer, but this is a 32/64
// bit timer so we are setting the 32bit mode
HWREG(WTIMER0_BASE+TIMER_O_CFG) = TIMER_CFG_16_BIT;
// we want to use the full 32 bit count, so initialize the Interval Load
// register to 0xffff.ffff (its default value :-)
HWREG(WTIMER0_BASE+TIMER_O_TAILR) = 0xffffffff;
// set up timer A in capture mode (TAMR=3, TAAMS = 0),
// for edge time (TACMR = 1) and up-counting (TACDIR = 1)
HWREG(WTIMER0_BASE+TIMER_O_TAMR) =
(HWREG(WTIMER0_BASE+TIMER_O_TAMR) & ~TIMER_TAMR_TAAMS) |
(TIMER_TAMR_TACDIR | TIMER_TAMR_TACMR | TIMER_TAMR_TAMR_CAP);
// To set the event to rising edge, we need to modify the TAEVENT bits
// in GPTMCTL. Rising edge = 00, so we clear the TAEVENT bits
HWREG(WTIMER0_BASE+TIMER_O_CTL) &= ~TIMER_CTL_TAEVENT_M;
// Now Set up the port to do the capture (clock was enabled earlier)
// start by setting the alternate function for Port C bit 4 (WT0CCP0)
HWREG(GPIO_PORTC_BASE+GPIO_O_AFSEL) |= BIT4HI;
// Then, map bit 4's alternate function to WT0CCP0
// 7 is the mux value to select WT0CCP0, 16 to shift it over to the
// right nibble for bit 4 (4 bits/nibble * 4 bits)
HWREG(GPIO_PORTC_BASE+GPIO_O_PCTL) =
(HWREG(GPIO_PORTC_BASE+GPIO_O_PCTL) & 0xfff0ffff) + (7<<16);
// Enable pin on Port C for digital I/O
HWREG(GPIO_PORTC_BASE+GPIO_O_DEN) |= BIT4HI;
// make pin 4 on Port C into an input
HWREG(GPIO_PORTC_BASE+GPIO_O_DIR) &= BIT4LO;
// back to the timer to enable a local capture interrupt
HWREG(WTIMER0_BASE+TIMER_O_IMR) |= TIMER_IMR_CAEIM;
// enable the Timer A in Wide Timer 0 interrupt in the NVIC
// it is interrupt number 94 so appears in EN2 at bit 30
HWREG(NVIC_EN2) |= BIT30HI;
// make sure interrupts are enabled globally
__enable_irq();
// now kick the timer off by enabling it and enabling the timer to
// stall while stopped by the debugger
HWREG(WTIMER0_BASE+TIMER_O_CTL) |= (TIMER_CTL_TAEN | TIMER_CTL_TASTALL);
}
void HallCaptureResponse( void ){
uint32_t ThisHallCapture;
static uint32_t PeriodAve[NUM_HALL_SAMPLES]= {0};
uint32_t PeriodSum = 0;
// start by clearing the source of the interrupt, the input capture event
HWREG(WTIMER0_BASE+TIMER_O_ICR) = TIMER_ICR_CAECINT;
// now grab the captured value and calculate the period
ThisHallCapture = HWREG(WTIMER0_BASE+TIMER_O_TAR);
for(uint16_t i = NUM_HALL_SAMPLES; i > 0; i--){
PeriodAve[i] = PeriodAve[i-1];
}
PeriodAve[0] = ThisHallCapture - LastHallCapture;
for (uint16_t averager = 0; averager < NUM_HALL_SAMPLES; averager++){
PeriodSum = PeriodSum + (PeriodAve[averager]);
}
AverageHallPeriod = PeriodSum / NUM_HALL_SAMPLES;
// update LastCapture to prepare for the next edge
LastHallCapture = ThisHallCapture;
}
static uint8_t CheckForPollingStation( uint32_t ConvertedAverageHallPeriod) {
//printf("AverageHallPeriod%d\r\n",AverageHallPeriod);
// printf("ConvertedAverageHallPeriod%d\r\n",ConvertedAverageHallPeriod);
PollingReturnCode = 0;
for (uint8_t i = 0; i < NUM_FREQUENCIES; i++){
if ((ConvertedAverageHallPeriod >= PollingPeriodLower[i]) && (ConvertedAverageHallPeriod <= PollingPeriodUpper[i])){
PollingReturnCode = PollingStationCode[i];
HWREG(GPIO_PORTB_BASE+ (GPIO_O_DATA + ALL_BITS)) |= GPIO_PIN_3;
}
}
return PollingReturnCode;
}
static uint8_t RequestColor(void){
HWREG(SYSCTL_RCGCGPIO) |= SYSCTL_RCGCGPIO_R5;
HWREG(GPIO_PORTF_BASE+GPIO_O_DEN) |= BIT1HI;
while ((HWREG(SYSCTL_PRGPIO) & SYSCTL_PRGPIO_R5) != SYSCTL_PRGPIO_R5);
HWREG(GPIO_PORTF_BASE+GPIO_O_DIR) &= BIT1LO; //Make This an INPUT PIN.
TeamColor = (HWREG(GPIO_PORTF_BASE+(GPIO_O_DATA + ALL_BITS))) & BIT1HI; // 1 IMPLIES RED LED ON, 0 IMPLIES BLUE LED ON.
return TeamColor;
}
uint8_t QueryPollingCode(void)
{
ConvertedAverageHallPeriod = AverageHallPeriod / TICKS_PER_US;
PollingReturnCode = CheckForPollingStation(ConvertedAverageHallPeriod);
return PollingReturnCode; // Returns the speed of the motor
}
void InitFeedbackLED(void){
//First Initialize the Hall Sensor Feedback LED, which is currently assigned to PB3.
HWREG(SYSCTL_RCGCGPIO) |= SYSCTL_RCGCGPIO_R1;
HWREG(GPIO_PORTB_BASE+GPIO_O_DEN) |= (GPIO_PIN_3);
//Manual delay to ensure clock is ready
while ((HWREG(SYSCTL_PRGPIO) & SYSCTL_PRGPIO_R1) != SYSCTL_PRGPIO_R1);
HWREG(GPIO_PORTB_BASE+GPIO_O_DIR) |= (GPIO_PIN_3);
//Then Initialize the BEACON DETECTION LED - PA7.
HWREG(SYSCTL_RCGCGPIO) |= SYSCTL_RCGCGPIO_R0;
HWREG(GPIO_PORTA_BASE+GPIO_O_DEN) |= (GPIO_PIN_7);
//Manual delay to ensure clock is ready
while ((HWREG(SYSCTL_PRGPIO) & SYSCTL_PRGPIO_R0) != SYSCTL_PRGPIO_R0);
HWREG(GPIO_PORTA_BASE+GPIO_O_DIR) |= (GPIO_PIN_7);
//Finally Initialize the GAME STATUS LED - PC7
HWREG(SYSCTL_RCGCGPIO) |= SYSCTL_RCGCGPIO_R2;
HWREG(GPIO_PORTC_BASE+GPIO_O_DEN) |= (GPIO_PIN_7); // PC7
while ((HWREG(SYSCTL_PRGPIO) & SYSCTL_PRGPIO_R2) != SYSCTL_PRGPIO_R2);
HWREG(GPIO_PORTC_BASE+GPIO_O_DIR) |= (GPIO_PIN_7); //PC7.
}
void InitWallSensors(void){
HWREG(SYSCTL_RCGCGPIO) |= SYSCTL_RCGCGPIO_R4;
HWREG(GPIO_PORTE_BASE+GPIO_O_DEN) |= (GPIO_PIN_3|GPIO_PIN_4|GPIO_PIN_5);
//Manual delay to ensure clock is ready
while ((HWREG(SYSCTL_PRGPIO) & SYSCTL_PRGPIO_R4) != SYSCTL_PRGPIO_R4);
HWREG(GPIO_PORTE_BASE+GPIO_O_DIR) &= ~(GPIO_PIN_3|GPIO_PIN_4|GPIO_PIN_5); //Initialize Port E Bit 2, 3, 4 & 5 to be outputs
}
void InitWallSensorPeriodicInterupt( void ){ //Use Wide Timer1 Sub-Timer B for Wall Detection
volatile uint32_t Dummy; // use volatile to avoid over-optimization
// start by enabling the clock to the timer (Wide Timer 1)
HWREG(SYSCTL_RCGCWTIMER) |= SYSCTL_RCGCWTIMER_R1; // kill a few cycles to let the clock get going
Dummy = HWREG(SYSCTL_RCGCGPIO);
// make sure that timer (Timer B) is disabled before configuring
HWREG(WTIMER1_BASE+TIMER_O_CTL) &= ~TIMER_CTL_TBEN;
// set it up in 32bit wide (individual, not concatenated) mode
HWREG(WTIMER1_BASE+TIMER_O_CFG) = TIMER_CFG_16_BIT;
// set up timer B in periodic mode so that it repeats the time-outs
HWREG(WTIMER1_BASE+TIMER_O_TBMR) =(HWREG(WTIMER1_BASE+TIMER_O_TBMR)& ~TIMER_TBMR_TBMR_M)|TIMER_TBMR_TBMR_PERIOD;
// set timeout to 2mS by writing into the Interval Load Register (ILR)
HWREG(WTIMER1_BASE+TIMER_O_TBILR) = TicksPerMS * 2;
// enable a local timeout interrupt
HWREG(WTIMER1_BASE+TIMER_O_IMR) |= TIMER_IMR_TBTOIM;
// enable the Timer B in Wide Timer 1 interrupt in the NVIC
// it is interrupt number 97 so appears in EN3 at bit 1
HWREG(NVIC_EN3) = BIT1HI;
//HWREG(WTIMER0_BASE+0x45C) = BIT29HI; //Make this interrupt LOWER priority than the encoder interrupt
// make sure interrupts are enabled globally
__enable_irq();
// now kick the timer off by enabling it and enabling the timer to
// stall while stopped by the debugger
HWREG(WTIMER1_BASE+TIMER_O_CTL) |= (TIMER_CTL_TBEN |TIMER_CTL_TBSTALL);
}
void WallDetectionResponse(void){
HWREG(WTIMER1_BASE+TIMER_O_ICR) = TIMER_ICR_TBTOCINT; //Clear the source of the interrupt.
WallDectionLeft = (HWREG(GPIO_PORTE_BASE+(GPIO_O_DATA + ALL_BITS))) & BIT5HI;
WallDectionCenter = (HWREG(GPIO_PORTE_BASE+(GPIO_O_DATA + ALL_BITS))) & BIT4HI;
WallDectionRight = (HWREG(GPIO_PORTE_BASE+(GPIO_O_DATA + ALL_BITS))) & BIT3HI;
}
uint8_t QueryLeftWallDetection(void){
return WallDectionLeft;
}
uint8_t QueryRightWallDetection(void){
return WallDectionRight;
}
void InitOneShotTimer(void){ //Use Wide Timer 1 Sub-Timer A for this purpose.
volatile uint32_t Dummy; // use volatile to avoid over-optimization
// start by enabling the clock to the timer (Wide Timer 1)
HWREG(SYSCTL_RCGCWTIMER) |= SYSCTL_RCGCWTIMER_R1; // kill a few cycles to let the clock get going
Dummy = HWREG(SYSCTL_RCGCGPIO);
// make sure that timer (Timer A) is disabled before configuring
HWREG(WTIMER1_BASE+TIMER_O_CTL) &= ~TIMER_CTL_TAEN;
// set it up in 32bit wide (individual, not concatenated) mode
HWREG(WTIMER1_BASE+TIMER_O_CFG) = TIMER_CFG_16_BIT;
// set up timer A in ONE SHOT mode so that it times-out-for-sure.
HWREG(WTIMER1_BASE+TIMER_O_TAMR) =(HWREG(WTIMER1_BASE+TIMER_O_TAMR)& ~TIMER_TAMR_TAMR_M)|TIMER_TAMR_TAMR_1_SHOT;
// set timeout to SHOOTING TIME by writing into the Interval Load Register (ILR)
HWREG(WTIMER1_BASE+TIMER_O_TAILR) = TicksPerMS * SHOOTING_TIME;
// enable a local timeout interrupt
HWREG(WTIMER1_BASE+TIMER_O_IMR) |= TIMER_IMR_TATOIM;
// enable the Timer B in Wide Timer 0 interrupt in the NVIC
// it is interrupt number 95 so appears in EN2 at bit 30
HWREG(NVIC_EN3) |= BIT0HI;
//HWREG(WTIMER0_BASE+0x45C) = BIT29HI; //Make this interrupt LOWER priority than the encoder interrupt
// make sure interrupts are enabled globally
__enable_irq();
// now kick the timer off by enabling it and enabling the timer to
// stall while stopped by the debugger
HWREG(WTIMER1_BASE+TIMER_O_CTL) |= (TIMER_CTL_TAEN |TIMER_CTL_TASTALL);
}
void OneShotResponse(void){
HWREG(WTIMER1_BASE+TIMER_O_ICR) = TIMER_ICR_TATOCINT; //Clear the source of the interrupt.
Shooting_Allowed = YES_SHOOT;
InitGameOneShotTimer();
}
uint8_t QueryShootingAllowedStatus (void){
return Shooting_Allowed;
}
void InitFieldAverager( void ){ //Use Wide Timer2 Sub-Timer B.
volatile uint32_t Dummy; // use volatile to avoid over-optimization
// start by enabling the clock to the timer (Wide Timer 2)
HWREG(SYSCTL_RCGCWTIMER) |= SYSCTL_RCGCWTIMER_R2; // kill a few cycles to let the clock get going
Dummy = HWREG(SYSCTL_RCGCGPIO);
// make sure that timer (Timer B) is disabled before configuring
HWREG(WTIMER2_BASE+TIMER_O_CTL) &= ~TIMER_CTL_TBEN;
// set it up in 32bit wide (individual, not concatenated) mode
HWREG(WTIMER2_BASE+TIMER_O_CFG) = TIMER_CFG_16_BIT;
// set up timer B in periodic mode so that it repeats the time-outs
HWREG(WTIMER2_BASE+TIMER_O_TBMR) =(HWREG(WTIMER2_BASE+TIMER_O_TBMR)& ~TIMER_TBMR_TBMR_M)|TIMER_TBMR_TBMR_PERIOD;
// set timeout to 500mS by writing into the Interval Load Register (ILR)
HWREG(WTIMER2_BASE+TIMER_O_TBILR) = TicksPerMS * LINE_FOLLOW_AVERAGE_TIME;
// enable a local timeout interrupt
HWREG(WTIMER2_BASE+TIMER_O_IMR) |= TIMER_IMR_TBTOIM;
// enable the Timer B in Wide Timer 2 interrupt in the NVIC
// it is interrupt number 99 so appears in EN3 at bit 31
HWREG(NVIC_EN3) |= BIT3HI;
// make sure interrupts are enabled globally
__enable_irq();
// now kick the timer off by enabling it and enabling the timer to
// stall while stopped by the debugger
HWREG(WTIMER2_BASE+TIMER_O_CTL) |= (TIMER_CTL_TBEN |TIMER_CTL_TBSTALL);
}
void FieldAveragerResponse(void){
HWREG(WTIMER2_BASE+TIMER_O_ICR) = TIMER_ICR_TBTOCINT; //Clear the source of the interrupt.
uint32_t LeftFieldPeriodSum = 0;
uint32_t Variance = 0;
for(uint16_t i = NUM_MAGNETIC_SAMPLES; i > 0; i--){
LeftFieldAve[i] = LeftFieldAve[i-1];
}
LeftFieldAve [0] = LeftWireSensorReading;
for (uint16_t averager = 0; averager < NUM_MAGNETIC_SAMPLES; averager++){
LeftFieldPeriodSum = LeftFieldPeriodSum + (LeftFieldAve[averager]);
}
AverageLeftField = LeftFieldPeriodSum/NUM_MAGNETIC_SAMPLES;
for (uint16_t averager = 0; averager < NUM_MAGNETIC_SAMPLES; averager++){
Variance = Variance + (LeftFieldAve[averager]-AverageLeftField)*(LeftFieldAve[averager]-AverageLeftField);
}
Left_Field_Variance = Variance/NUM_MAGNETIC_SAMPLES; //Variance should be really small for long term averages
}
void InitTenSecondOneShotTimer(void){ //Use Wide Timer 4 Sub-Timer A for this purpose.
volatile uint32_t Dummy; // use volatile to avoid over-optimization
// start by enabling the clock to the timer (Wide Timer 4)
HWREG(SYSCTL_RCGCWTIMER) |= SYSCTL_RCGCWTIMER_R4; // kill a few cycles to let the clock get going
Dummy = HWREG(SYSCTL_RCGCGPIO);
HWREG(WTIMER4_BASE+TIMER_O_CTL) &= ~TIMER_CTL_TAEN;
HWREG(WTIMER4_BASE+TIMER_O_CFG) = TIMER_CFG_16_BIT;
HWREG(WTIMER4_BASE+TIMER_O_TAMR) =(HWREG(WTIMER4_BASE+TIMER_O_TAMR)& ~TIMER_TAMR_TAMR_M)|TIMER_TAMR_TAMR_1_SHOT;
HWREG(WTIMER4_BASE+TIMER_O_TAILR) = TicksPerMS * TEN_SECOND_TIME;
HWREG(WTIMER4_BASE+TIMER_O_IMR) |= TIMER_IMR_TATOIM;
HWREG(NVIC_EN3) |= BIT6HI;
__enable_irq();
HWREG(WTIMER4_BASE+TIMER_O_CTL) |= (TIMER_CTL_TAEN |TIMER_CTL_TASTALL);
}
void TenSecondOneShotResponse(void){
HWREG(WTIMER4_BASE+TIMER_O_ICR) = TIMER_ICR_TATOCINT; //Clear the source of the interrupt.
Field_Averaging_Allowed = FIELD_ALLOWED;
}
void InitStationCapturingOneShotTimer(void){ //Use Wide Timer 5 Sub-Timer B for this purpose.
volatile uint32_t Dummy; // use volatile to avoid over-optimization
// start by enabling the clock to the timer (Wide Timer 5)
HWREG(SYSCTL_RCGCWTIMER) |= SYSCTL_RCGCWTIMER_R5; // kill a few cycles to let the clock get going
Dummy = HWREG(SYSCTL_RCGCGPIO);
// make sure that timer (Timer B) is disabled before configuring
HWREG(WTIMER5_BASE+TIMER_O_CTL) &= ~TIMER_CTL_TBEN;
// set it up in 32bit wide (individual, not concatenated) mode
HWREG(WTIMER5_BASE+TIMER_O_CFG) = TIMER_CFG_16_BIT;
// set up timer B in ONE SHOT mode so that it times-out-for-sure.
HWREG(WTIMER5_BASE+TIMER_O_TBMR) =(HWREG(WTIMER5_BASE+TIMER_O_TBMR)& ~TIMER_TBMR_TBMR_M)|TIMER_TBMR_TBMR_1_SHOT;
// set timeout to SHOOTING TIME by writing into the Interval Load Register (ILR)
HWREG(WTIMER5_BASE+TIMER_O_TBILR) = TicksPerMS * EXIT_STATION_CAPTURING_TIME;
// enable a local timeout interrupt
HWREG(WTIMER5_BASE+TIMER_O_IMR) |= TIMER_IMR_TBTOIM;
HWREG(NVIC_EN3) |= BIT9HI;
// make sure interrupts are enabled globally
__enable_irq();
// now kick the timer off by enabling it and enabling the timer to
// stall while stopped by the debugger
HWREG(WTIMER5_BASE+TIMER_O_CTL) |= (TIMER_CTL_TBEN |TIMER_CTL_TBSTALL);
}
void StationCapturingOneShotTimerResponse(void){
HWREG(WTIMER5_BASE+TIMER_O_ICR) = TIMER_ICR_TBTOCINT; //Clear the source of the interrupt.
Exit_Station_Capturing_Allowed = EXIT_ALLOWED;
}
uint8_t QueryExitStationAllowed(){
return Exit_Station_Capturing_Allowed;
}
void InitGameOneShotTimer(void){ //Use Wide Timer 2 Sub-Timer A for this purpose.
volatile uint32_t Dummy; // use volatile to avoid over-optimization
// start by enabling the clock to the timer (Wide Timer 2)
HWREG(SYSCTL_RCGCWTIMER) |= SYSCTL_RCGCWTIMER_R2; // kill a few cycles to let the clock get going
Dummy = HWREG(SYSCTL_RCGCGPIO);
// make sure that timer (Timer A) is disabled before configuring
HWREG(WTIMER2_BASE+TIMER_O_CTL) &= ~TIMER_CTL_TAEN;
// set it up in 32bit wide (individual, not concatenated) mode
HWREG(WTIMER2_BASE+TIMER_O_CFG) = TIMER_CFG_16_BIT;
// set up timer A in ONE SHOT mode so that it times-out-for-sure.
HWREG(WTIMER2_BASE+TIMER_O_TAMR) =(HWREG(WTIMER2_BASE+TIMER_O_TAMR)& ~TIMER_TAMR_TAMR_M)|TIMER_TAMR_TAMR_1_SHOT;
// set timeout to SHOOTING TIME by writing into the Interval Load Register (ILR)
HWREG(WTIMER2_BASE+TIMER_O_TAILR) = TicksPerMS * GAME_TIME;
// enable a local timeout interrupt
HWREG(WTIMER2_BASE+TIMER_O_IMR) |= TIMER_IMR_TATOIM;
HWREG(NVIC_EN3) |= BIT2HI;
// make sure interrupts are enabled globally
__enable_irq();
// now kick the timer off by enabling it and enabling the timer to
// stall while stopped by the debugger
HWREG(WTIMER2_BASE+TIMER_O_CTL) |= (TIMER_CTL_TAEN |TIMER_CTL_TASTALL);
}
void GameOneShotTimerResponse(void){
HWREG(WTIMER2_BASE+TIMER_O_ICR) = TIMER_ICR_TATOCINT; //Clear the source of the interrupt.
Game_End_Status = GAME_OVER;
}
uint8_t QueryGameOverStatus (void){
return Game_End_Status;
}
void InitAutoDelayResponseOneShotTimer(void){ //Use Wide Timer 3 Sub-Timer B for this purpose.
volatile uint32_t Dummy; // use volatile to avoid over-optimization
// start by enabling the clock to the timer (Wide Timer 3)
HWREG(SYSCTL_RCGCWTIMER) |= SYSCTL_RCGCWTIMER_R3; // kill a few cycles to let the clock get going
Dummy = HWREG(SYSCTL_RCGCGPIO);
HWREG(WTIMER3_BASE+TIMER_O_CTL) &= ~TIMER_CTL_TBEN;
HWREG(WTIMER3_BASE+TIMER_O_CFG) = TIMER_CFG_16_BIT;
HWREG(WTIMER3_BASE+TIMER_O_TBMR) =(HWREG(WTIMER3_BASE+TIMER_O_TBMR)& ~TIMER_TBMR_TBMR_M)|TIMER_TBMR_TBMR_1_SHOT;
HWREG(WTIMER3_BASE+TIMER_O_TBILR) = TicksPerMS * DELAY_TIME;
HWREG(WTIMER3_BASE+TIMER_O_IMR) |= TIMER_IMR_TBTOIM;
HWREG(NVIC_EN3) |= BIT5HI;
__enable_irq();
HWREG(WTIMER3_BASE+TIMER_O_CTL) |= (TIMER_CTL_TBEN |TIMER_CTL_TBSTALL);
}
void AutoDelayResponse(void){
HWREG(WTIMER3_BASE+TIMER_O_ICR) = TIMER_ICR_TBTOCINT; //Clear the source of the interrupt.
InitOneShotTimer();
}
Station Capturing
Implements the entire sequence of SPI back and forth events to capture a state, including all the sub-cases wherein an Acknowledgement packet is not received, or if the robot is blocked by an AdAttack from an opposing team.
Header File
/****************************************************************************
Template header file for Hierarchical Sate Machines AKA StateCharts
02/08/12 adjsutments for use with the Events and Services Framework Gen2
3/17/09 Fixed prototpyes to use Event_t
****************************************************************************/
#ifndef StationCapturing_H
#define StationCapturing_H
// typedefs for the states
// State definitions for use with the query function
typedef enum {InitializeChangeStationRequest,RequestChangeWithFrequency} StationCapturingState_t ;
// Public Function Prototypes
ES_Event RunStationCapturing( ES_Event CurrentEvent );
void StartStationCapturing( ES_Event CurrentEvent );
StationCapturingState_t QueryStationCapturing (void);
uint32_t QueryPreviousCity (void);
#endif /*StationCapturing_H */
Source File
/****************************************************************************
Module
StationCapturing.c
Revision
2.0.1
Description
This is a template file for implementing state machines.
Notes
History
****************************************************************************/
/*----------------------------- Include Files -----------------------------*/
// Basic includes for a program using the Events and Services Framework
#include "ES_Configure.h"
#include "ES_Framework.h"
#include "StationCapturing.h"
#include "ES_DeferRecall.h"
#include "inc/hw_memmap.h"
#include "inc/hw_types.h"
#include "inc/hw_gpio.h"
#include "inc/hw_sysctl.h"
#include "driverlib/sysctl.h"
#include "driverlib/pin_map.h"
#include "driverlib/gpio.h"
#include "inc/hw_pwm.h"
#include "inc/hw_timer.h"
#include "inc/hw_ssi.h"
#include "inc/hw_nvic.h"
#include "driverlib/interrupt.h"
#include "inc/tm4c123gh6pm.h"
#include "TopCommunication.h"
#include "TopRobot.h"
#include "GameIntelligence.h"
#define ALL_BITS (0xff<<2)
#define BLUE_COLOR 0
#define RED_COLOR 2
/* 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
*/
/*----------------------------- Module Defines ----------------------------*/
// define constants for the states for this machine
// and any other local defines
#define ENTRY_STATE InitializeChangeStationRequest
#define RESPONSE_READY_TIME 20 //ACTUAL VALUE IS 20
#define QUERY_PERIOD 50
#define REQUEST_PERIOD 250
#define MIAMI 0x00070000
#define SEATTLE 0x00020000
#define YES_SHOOT 1
#define NO_SHOOT 0
#define EXIT_ALLOWED 1
#define EXIT_NOT_ALLOWED 0
#define SACRAMENTO 0x00010000
#define BILLINGS 0x00030000
#define DALLAS 0x00050000
#define MIAMI 0x00070000
#define SEATTLE 0x00020000
#define WASHINGTON 0x00080000
#define DENVER 0x00040000
#define CONCORD 0x00090000
#define CHICAGO 0x00060000
#define NUMBER_OF_STATIONS_TO_GO_INTO_SHOOTING_STATE 18 //This is for Eds Checkoff
#define NUMBER_OF_WINS_TO_GO_INTO_SHOOTING_STATE 3 //This is for the Tournament
/*---------------------------- 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 uint8_t RequestColor(void);
uint32_t QueryPreviousCity (void);
/*---------------------------- Module Variables ---------------------------*/
// everybody needs a state variable, you may need others as well
static StationCapturingState_t CurrentState;
static uint8_t FrequencyCode;
static uint8_t TeamColor;
static uint32_t ResponseReadyandResponseStatusMask = 0x00C0FF00;
static uint32_t ResponseReadyMask = 0x0000FF00;
static uint8_t QueryByte = 0x70;
static uint32_t ResponseReadyStatus = 0;
static uint32_t ResponseIsReadyandACK = 0x0040AA00; // Acknowledged Frequency Measure
static uint32_t ResponseIsReadyandNACK = 0x0000AA00; // Did not acknowledge Frequency Measure
static uint32_t ResponseIsReadyandBlocked = 0x0080AA00; // Implies that we are under a capture
static uint32_t ResponseIsNotReady = 0;
static uint32_t CityMask = 0x000F0000;
static uint32_t CityStatus;
static uint8_t Color;
static uint8_t ShootingAllowed = 0;
static uint32_t CurrentCity=0;
static uint32_t PreviousCity=0;
static uint8_t QueryExitAllowed=EXIT_NOT_ALLOWED;
static uint8_t Num_Captured_Stations = 0; //Number of Stations Captured Intially.
static int16_t WinLoseStatus;
// 00 RS RR FF
/*------------------------------ Module Code ------------------------------*/
/****************************************************************************
Function
RunStationCapturing
Parameters
ES_Event: the event to process
Returns
ES_Event: an event to return
Description
add your description here
Notes
****************************************************************************/
// Only for testing
ES_Event RunStationCapturing( ES_Event CurrentEvent )
{
bool MakeTransition = false;// are we making a state transition?
StationCapturingState_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 InitializeChangeStationRequest:
if ( CurrentEvent.EventType != ES_NO_EVENT ) //If an event is active
{
if(ES_STATION_DETECTED == CurrentEvent.EventType && 0!= CurrentEvent.EventParam){
ES_Timer_InitTimer(RequestTimer, REQUEST_PERIOD);
FrequencyCode = CurrentEvent.EventParam;
NextState = InitializeChangeStationRequest;
MakeTransition = false;
}
if (CurrentEvent.EventType == ES_TIMEOUT && CurrentEvent.EventParam == RequestTimer) {
TeamColor = RequestColor();
CurrentEvent.EventType = ES_SPI_XFER;
CurrentEvent.EventParam = ((0x0F&FrequencyCode)|(TeamColor));
PostTopCommunication(CurrentEvent);
ES_Timer_InitTimer(QueryTimer, QUERY_PERIOD);
NextState = InitializeChangeStationRequest;
MakeTransition = false;
}
if (CurrentEvent.EventType == ES_TIMEOUT && CurrentEvent.EventParam == QueryTimer){
CurrentEvent.EventType = ES_SPI_XFER;//Send a response Ready Request.
CurrentEvent.EventParam = QueryByte;
PostTopCommunication(CurrentEvent);
NextState = InitializeChangeStationRequest;
MakeTransition = false;
}
if (CurrentEvent.EventType == ES_SPI_COMPLETE){
ResponseReadyStatus = (CurrentEvent.EventParam & ResponseReadyandResponseStatusMask);
ShootingAllowed = QueryShootingAllowedStatus();
if( ResponseIsReadyandACK == ResponseReadyStatus){
CityStatus = (CurrentEvent.EventParam & CityMask);
PreviousCity = CurrentCity;
CurrentCity = (CurrentEvent.EventParam & CityMask);
WinLoseStatus = QueryWinLoseStatus();
if (((BLUE_COLOR == Color && CityStatus == SEATTLE && (SACRAMENTO == PreviousCity || BILLINGS == PreviousCity|| DALLAS == PreviousCity)) || (RED_COLOR == Color && CityStatus == MIAMI && (DALLAS == PreviousCity || WASHINGTON == PreviousCity)))
&& ShootingAllowed == YES_SHOOT && WinLoseStatus>= NUMBER_OF_WINS_TO_GO_INTO_SHOOTING_STATE){
CurrentEvent.EventType = ES_STATION_CAPTURED;
CurrentEvent.EventParam = YES_SHOOT;
NextState = InitializeChangeStationRequest;
MakeTransition = true;
PostTopRobot(CurrentEvent);
HWREG(GPIO_PORTA_BASE+ (GPIO_O_DATA + ALL_BITS)) |= GPIO_PIN_7;
}
else{
CurrentEvent.EventType = ES_RESPONSE_READY;
NextState = RequestChangeWithFrequency;
MakeTransition = true;
PostTopRobot(CurrentEvent);
}
}
if( (ResponseIsReadyandNACK == ResponseReadyStatus) || (ResponseIsReadyandBlocked == ResponseReadyStatus) ){
QueryExitAllowed = QueryExitStationAllowed();
if(EXIT_NOT_ALLOWED == QueryExitAllowed){
CurrentEvent.EventType = ES_STATION_DETECTED;
FrequencyCode = QueryPollingCode();
CurrentEvent.EventParam = ((0x0F&FrequencyCode)|(TeamColor));
NextState =InitializeChangeStationRequest;
MakeTransition = false;
PostTopRobot(CurrentEvent);
}
if(EXIT_ALLOWED == QueryExitAllowed){
CurrentEvent.EventType = ES_EXIT_STATION_READY;
NextState =InitializeChangeStationRequest;
MakeTransition = false;
PostTopRobot(CurrentEvent);
}
}
if(ResponseIsNotReady == (ResponseReadyStatus&ResponseReadyMask)){
ES_Timer_InitTimer(QueryTimer, QUERY_PERIOD);
NextState = InitializeChangeStationRequest;
MakeTransition = false;
}
}
}
break;
case RequestChangeWithFrequency:
if ( CurrentEvent.EventType != ES_NO_EVENT ) //If an event is active
{
if(ES_RESPONSE_READY == CurrentEvent.EventType){ //Now, First Measure the New Frequency Again
ES_Timer_InitTimer(RequestTimer, REQUEST_PERIOD);
NextState = RequestChangeWithFrequency;
MakeTransition = false;
}
if (CurrentEvent.EventType == ES_TIMEOUT && CurrentEvent.EventParam == RequestTimer) {
FrequencyCode = QueryPollingCode();
CurrentEvent.EventType = ES_SPI_XFER;
CurrentEvent.EventParam = ((0x0F&FrequencyCode)|(TeamColor));
PostTopCommunication(CurrentEvent);
NextState = RequestChangeWithFrequency;
MakeTransition = false;
ES_Timer_InitTimer(QueryTimer, QUERY_PERIOD);
}
if (CurrentEvent.EventType == ES_TIMEOUT && CurrentEvent.EventParam == QueryTimer){
CurrentEvent.EventType = ES_SPI_XFER;//Send a response Ready Request.
CurrentEvent.EventParam = QueryByte;
PostTopCommunication(CurrentEvent);
NextState = RequestChangeWithFrequency;
MakeTransition = false;
}
if (CurrentEvent.EventType == ES_SPI_COMPLETE){
ResponseReadyStatus = (CurrentEvent.EventParam & ResponseReadyandResponseStatusMask);
if( ResponseIsReadyandACK == ResponseReadyStatus){
Num_Captured_Stations = Num_Captured_Stations + 1; //Check for uniqueness of stations?
CurrentEvent.EventType = ES_STATION_CAPTURED; //Leave this state via the upper level. Decide Whether to Enter Shooting State Here, Ideally.
CurrentEvent.EventParam = NO_SHOOT;
NextState = InitializeChangeStationRequest;
MakeTransition = true;
PostTopRobot(CurrentEvent);
}
if(ResponseIsNotReady == (ResponseReadyStatus&ResponseReadyMask)){
ES_Timer_InitTimer(QueryTimer, QUERY_PERIOD);
NextState = RequestChangeWithFrequency;
MakeTransition = false;
}
if( (ResponseIsReadyandNACK == ResponseReadyStatus) || (ResponseIsReadyandBlocked == ResponseReadyStatus) ){
QueryExitAllowed = QueryExitStationAllowed();
if(EXIT_NOT_ALLOWED == QueryExitAllowed){
CurrentEvent.EventType = ES_RESPONSE_READY;
FrequencyCode = QueryPollingCode();
CurrentEvent.EventParam = ((0x0F&FrequencyCode)|(TeamColor));
NextState = RequestChangeWithFrequency;
MakeTransition = false;
PostTopRobot(CurrentEvent);
}
if(EXIT_ALLOWED == QueryExitAllowed){
CurrentEvent.EventType = ES_EXIT_STATION_READY;
NextState =InitializeChangeStationRequest;
MakeTransition = true;
PostTopRobot(CurrentEvent);
}
}
}
}
break;
}
// If we are making a state transition
if (MakeTransition == true)
{
// Execute exit function for current state
CurrentEvent.EventType = ES_EXIT;
RunStationCapturing(CurrentEvent);
CurrentState = NextState; //Modify state variable
// Execute entry function for new state
// this defaults to ES_ENTRY
RunStationCapturing(EntryEventKind);
}
return(ReturnEvent);
}
/****************************************************************************
Function
StartStationCapturing
Parameters
None
Returns
None
Description
Does any required initialization for this state machine
Notes
****************************************************************************/
void StartStationCapturing ( ES_Event 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 = ENTRY_STATE;
}
// call the entry function (if any) for the ENTRY_STATE
RunStationCapturing(CurrentEvent);
}
static uint8_t RequestColor(void){
Color = (HWREG(GPIO_PORTF_BASE+(GPIO_O_DATA + ALL_BITS))) & BIT1HI; // 1 IMPLIES RED LED ON, 0 IMPLIES BLUE LED ON.
if (BLUE_COLOR == Color){
return 0xB0; // Case for Blue Team
}
if (RED_COLOR == Color){
return 0x80; // Case for Red Team
}
}
uint32_t QueryPreviousCity (void){
return PreviousCity;
}
Top Communication
State Machine that handles the communication of the PAC with the Super-PAC and shuttles information back and forth to TopRobot upon initiation of an SPI Transfer request. All 5 bytes are sent to the Super-PAC at once, and the data from the Super-PAC is collected upon an End-of-Transmit Interrupt.
Header File
/****************************************************************************
Template header file for Hierarchical Sate Machines AKA StateCharts
****************************************************************************/
#ifndef TopCommunication_H
#define TopCommunication_H
// State definitions for use with the query function
typedef enum { Waiting, WaitingForEOT,WaitingForTimeout} TopCommunicationState_t ;
// Public Function Prototypes
ES_Event RunTopCommunication( ES_Event CurrentEvent );
void StartTopCommunication ( ES_Event CurrentEvent );
bool PostTopCommunication( ES_Event ThisEvent );
bool InitTopCommunication ( uint8_t Priority );
#endif /*TopCommunication_H */
Source File
/****************************************************************************
Module
TopCommunication.c
Revision
2.0.1
Description
This is a template for the top level Hierarchical state machine
Notes
History
When Who What/Why
-------------- --- --------
02/12/16 14:10 asy started coding for the first time - Fully wrote the base module.
****************************************************************************/
/*----------------------------- 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 "TopCommunication.h"
#include "ES_DeferRecall.h"
#include "inc/hw_memmap.h"
#include "inc/hw_types.h"
#include "inc/hw_gpio.h"
#include "inc/hw_sysctl.h"
#include "driverlib/sysctl.h"
#include "driverlib/pin_map.h"
#include "driverlib/gpio.h"
#include "inc/hw_pwm.h"
#include "inc/hw_timer.h"
#include "inc/hw_ssi.h"
#include "inc/hw_nvic.h"
#include "driverlib/interrupt.h"
#include "inc/tm4c123gh6pm.h"
#include "TopRobot.h"
/*----------------------------- Module Defines ----------------------------*/
void ConfigureSPI(void); //Performs the Initialization for the SPI Module. Assignment: PA2 Clock, PA3 Slave Select and PA5 Transmitter
void SPIInterruptResponse(void);
#define SENDING_BYTE_TIMER_VALUE 2
/*---------------------------- Module Functions ---------------------------*/
/*---------------------------- 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 TopCommunicationState_t CurrentState;
// with the introduction of Gen2, we need a module level Priority var as well
static uint8_t MyPriority;
static uint8_t FirstByte; //The firstbyte from RobotSM, which contains the relevant SPI Message.
static uint8_t ZeroByte = 0x00;
static uint8_t ThisByte;
static uint32_t FullMessage=0x00000000; //Contains the Full SPI Message.
/*------------------------------ Module Code ------------------------------*/
/****************************************************************************
Function
InitTopCommunication
Parameters
uint8_t : the priorty of this service
Returns
boolean, False if error in initialization, True otherwise
Description
Saves away the priority, and starts
the top level state machine
Notes
****************************************************************************/
bool InitTopCommunication( uint8_t Priority )
{
ES_Event ThisEvent;
MyPriority = Priority; // save our priority
ThisEvent.EventType = ES_ENTRY;
ConfigureSPI(); //InitializetheSPIModule
StartTopCommunication( ThisEvent );
return true;
}
/****************************************************************************
Function
PostTopCommunication
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
Notes
****************************************************************************/
bool PostTopCommunication( ES_Event ThisEvent )
{
return ES_PostToService( MyPriority, ThisEvent);
}
/****************************************************************************
Function
RunTopCommunication
Parameters
ES_Event: the event to process
Returns
ES_Event: an event to return
Description
the run function for the top level state machine
Notes
uses nested switch/case to implement the machine.
****************************************************************************/
ES_Event RunTopCommunication( ES_Event CurrentEvent )
{
bool MakeTransition = false;/* are we making a state transition? */
TopCommunicationState_t NextState = CurrentState;
ES_Event EntryEventKind = { ES_ENTRY, 0 };// default to normal entry to new state
ES_Event ReturnEvent = { ES_NO_EVENT, 0 }; // assume no error
switch ( CurrentState )
{
case Waiting :
if ( CurrentEvent.EventType != ES_NO_EVENT ) //If an event is active
{
if(ES_SPI_XFER == CurrentEvent.EventType){
FirstByte = CurrentEvent.EventParam;
//Now, write to th SSI 5 times in a row, with the first byte as the actual command byte followed by the next 4 bytes to be zero-bytes.
HWREG(SSI0_BASE+SSI_O_DR) = FirstByte;
HWREG(SSI0_BASE+SSI_O_DR) = ZeroByte;
HWREG(SSI0_BASE+SSI_O_DR) = ZeroByte;
HWREG(SSI0_BASE+SSI_O_DR) = ZeroByte;
HWREG(SSI0_BASE+SSI_O_DR) = ZeroByte;
HWREG(NVIC_EN0) |= BIT7HI; //Enable the NVIC Interrupt
NextState = WaitingForEOT;//Decide what the next state will be
MakeTransition = true; //mark that we are taking a transition
}
}
break;
case WaitingForEOT:
if ( CurrentEvent.EventType != ES_NO_EVENT ) //If an event is active
{
if(ES_EOT == CurrentEvent.EventType){
for ( uint8_t receive_count = 0; receive_count < 5; receive_count++ ) {
ThisByte = HWREG(SSI0_BASE+SSI_O_DR); // Read from the SPI data register.
if(receive_count>0){
FullMessage = FullMessage + (ThisByte << (receive_count-1)*8);}
}
NextState = WaitingForTimeout;//Decide what the next state will be
ES_Timer_InitTimer(SendingByteTimer,SENDING_BYTE_TIMER_VALUE);
MakeTransition = true; //mark that we are taking a transition
}
}
break;
case WaitingForTimeout:
if ( CurrentEvent.EventType != ES_NO_EVENT ) //If an event is active
{
if(ES_TIMEOUT == CurrentEvent.EventType && SendingByteTimer == CurrentEvent.EventParam){
NextState = Waiting;//Decide what the next state will be
CurrentEvent.EventType = ES_SPI_COMPLETE;
CurrentEvent.EventParam = FullMessage; //Send the SPIMessage back to Robot.
PostTopRobot(CurrentEvent); //Post this Message to the Top level of the Robot State Machine.
MakeTransition = true; //mark that we are taking a transition
FullMessage = 0x00000000;
}
}
break;
}
// If we are making a state transition
if (MakeTransition == true)
{
// Execute exit function for current state
CurrentEvent.EventType = ES_EXIT;
RunTopCommunication(CurrentEvent);
CurrentState = NextState; //Modify state variable
RunTopCommunication(EntryEventKind);
}
// in the absence of an error the top level state machine should
// always return ES_NO_EVENT, which we initialized at the top of func
return(ReturnEvent);
}
/****************************************************************************
Function
StartTopCommunication
Parameters
ES_Event CurrentEvent
Returns
nothing
Description
Does any required initialization for this state machine
****************************************************************************/
void StartTopCommunication ( ES_Event CurrentEvent )
{
CurrentState = Waiting;
RunTopCommunication(CurrentEvent);
return;
}
/***************************************************************************
private functions
***************************************************************************/
// No During Functions
void ConfigureSPI(void){ // Change the Transmit FIFO CONDITION
// PA2 Clock PA3 Slave Select PA4 Receiver PA5 Transmitter
// First, initialize Port A
HWREG(SYSCTL_RCGCGPIO) |= SYSCTL_RCGCGPIO_R0;
// Second, Enable clock to SSI module
HWREG(SYSCTL_RCGCSSI) |= GPIO_PIN_0; // Enable SSI module 0, set bit 0 HI.
while ((HWREG(SYSCTL_PRGPIO) & SYSCTL_PRGPIO_R0) != SYSCTL_PRGPIO_R0);
HWREG(GPIO_PORTA_BASE + GPIO_O_AFSEL) |= (BIT2HI | BIT3HI | BIT4HI | BIT5HI);// Set alternate functions on the SSI pins: PA2, PA3, PA4, PA5.
HWREG(GPIO_PORTA_BASE + GPIO_O_PCTL) = (HWREG(GPIO_PORTA_BASE + GPIO_O_PCTL) & 0xff0000ff) + (2<<(2*4)) + (2<<(3*4)) + (2<<(4*4)) + (2<<(5*4)); // Set Mux Positions, Mux value 2, bits 2, 3, 4, 5
HWREG(GPIO_PORTA_BASE + GPIO_O_DEN) |= (GPIO_PIN_2 | GPIO_PIN_3 | GPIO_PIN_4 | GPIO_PIN_5); // Port lines for digital I/O
HWREG(GPIO_PORTA_BASE + GPIO_O_DIR) |= (GPIO_PIN_2 | GPIO_PIN_3 | GPIO_PIN_5); // Outputs: PA2 Clock, PA3 Slave Select and PA5 Transmitter
HWREG(GPIO_PORTA_BASE + GPIO_O_DIR) &= ~GPIO_PIN_4; // Input: PA4 Receiver
HWREG(GPIO_PORTA_BASE + GPIO_O_PUR) |= GPIO_PIN_2; // ENABLE PUR to the clock line - We are using Mode 3, thus we should be idling high in this case.
// Disable SSI (Control Register 1 SSE = 0) before programming mode bits
HWREG(SSI0_BASE + SSI_O_CR1) &= ~SSI_CR1_SSE;
// Select master mode (MS) & TXRIS indicating End of Transmit (EOT)
HWREG(SSI0_BASE + SSI_O_CR1) &= ~SSI_CR1_MS;
HWREG(SSI0_BASE + SSI_O_CR1) |= SSI_CR1_EOT;
// Configure the SSI clock sourse to the system clock
HWREG(SSI0_BASE + SSI_O_CC) = SSI_CC_CS_SYSPLL;
// Configure clock pre-scaler, set to 12.5kHz, CPSDVSR = 128, SCR = 24 //Max Permitted Value ~ 15kHz (1/(66us));
HWREG(SSI0_BASE + SSI_O_CPSR) = 128; // Value of CPSDVSR, must be an even number, ranges from 2 - 256.
// Configure clock rate (SCR bit 8 - 15), phase & polarity (SPH bit 7, SPO bit 6), mode (FRF), data size (DSS) (Those are in control register 0)
HWREG(SSI0_BASE + SSI_O_CR0) = (HWREG(SSI0_BASE + SSI_O_CR0) & 0xffff0000) + ((0x18) << (2*4)) + (1<<6) + (1<<7) + (0x7); // SCR = 24, SPH = 1, SPO = 1, Freescale SPI and 8-bit data size (0x7).
// Locally enable interrupts (TXIM in SSIIM)
HWREG(SSI0_BASE + SSI_O_IM) |= SSI_IM_TXIM; // Transmit FIFO interrupt unmasked
// Enable SSI (Control Register 1 SSE = 1)
HWREG(SSI0_BASE + SSI_O_CR1) |= SSI_CR1_SSE; // SSE is at bit 1
// Enable the NVIC interrupt
HWREG(NVIC_EN0) |= BIT7HI;
}
void SPIInterruptResponse(void){
HWREG(NVIC_EN0) &= BIT7LO; //Clearing
ES_Event CurrentEvent;
CurrentEvent.EventType = ES_EOT;
PostTopCommunication(CurrentEvent);
}
Shooting
Implements the “hard-coded” strategy to move towards the AdAttack bucket and shoot a ball in using the gate servo and the ruler servo. We only enter this state if we are winning by a margin of 3 and the time has crossed 38 seconds from the end of the game. Knowledge of which direction to move is found by keeping track of previous captured cities by querying the StationCapturing SM.
Header File
/****************************************************************************
Template header file for Hierarchical Sate Machines AKA StateCharts
02/08/12 adjsutments for use with the Events and Services Framework Gen2
3/17/09 Fixed prototpyes to use Event_t
****************************************************************************/
#ifndef Shooting_H
#define Shooting_H
// typedefs for the states
// State definitions for use with the query function
typedef enum {Stop,Rotating,Driving,Loading,Shoot} ShootingState_t ;
// Public Function Prototypes
ES_Event RunShooting( ES_Event CurrentEvent );
void StartShooting ( ES_Event CurrentEvent );
#endif /*Shooting_H */
Source File
/****************************************************************************
Module
Shooting.c
Revision
2.0.1
Description
This is a template file for implementing state machines.
Notes
****************************************************************************/
/*----------------------------- Include Files -----------------------------*/
// Basic includes for a program using the Events and Services Framework
#include "ES_Configure.h"
#include "ES_Framework.h"
#include "ES_DeferRecall.h"
#include "inc/hw_memmap.h"
#include "inc/hw_types.h"
#include "inc/hw_gpio.h"
#include "inc/hw_sysctl.h"
#include "driverlib/sysctl.h"
#include "driverlib/pin_map.h"
#include "driverlib/gpio.h"
#include "inc/hw_pwm.h"
#include "inc/hw_timer.h"
#include "inc/hw_ssi.h"
#include "inc/hw_nvic.h"
#include "driverlib/interrupt.h"
#include "inc/tm4c123gh6pm.h"
#include "TopRobot.h"
#include "Shooting.h"
#include "StationCapturing.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
*/
/*----------------------------- Module Defines ----------------------------*/
// define constants for the states for this machine
// and any other local defines
#define ENTRY_STATE Stop
#define TicksPerMS 40000
#define START_SHOOTING_COUNTDOWN 10000 //This value needs to be changed
#define LINE_FOLLOWER_PERIOD 25
#define PeriodInMS 1
#define PWMTicksPerMS 62 // her 1250 is 1k - Currently set to ~20k
#define ALL_BITS (0xff<<2)
#define NUM_HALL_SAMPLES 10
#define WHEEL_STOP 5
#define TICKS_PER_MS 3800
#define TICKS_PER_US 40
#define ONE_SEC 976
#define AFTER_STATION_CAPTURED_LINE_FOLLOWING_TIME 4000 //TUNE THIS TIME!!!!!
#define NUM_FREQUENCIES 16
#define CONVERSION_FACTOR 320
#define TURN_180_TIME 4000
#define INVALID_STATION_CODE 0
#define EXIT_STATION_CAPTURING_TIME 10000
#define WALL_DETECTED 0
#define FWD 0
#define BACK 1
#define ROBOT_PWM 60
#define ServoPWMTicksPerMS 1500
#define ServoPeriodMS 2 // servo pwm is at 2kHz/12
#define TURN_SPEED 70 // - Previously 45.
#define NUM_BEACON_SAMPLES 10
#define SHOOTING_TIME 5000
#define NUM_BEACON_FREQUENCIES 4
#define BEACON_ALIGN_PERIOD 10
#define BLUE_COLOR 0
#define RED_COLOR 1
#define SHOOT_DRIVE_PERIOD 1000
#define TRANSITION_PERIOD 10
#define RULER_UP_PWM 3
#define RULER_DOWN_PWM 10
#define GATE_CLOSED 11590 // 7.4
#define GATE_OPEN 11912 // 4.7 -- RULER DOWN OPEN
#define BEACON_NOT_DETECTED 0
#define BLUE_BEACON_DETECTED 1
#define RED_BEACON_DETECTED 3
#define GATE_PERIOD 3000
#define SHOOT_WAIT_PERIOD 7000
#define SHOOT_TO_LOAD_TRANSITION_TIME 100
#define BLUE_COLOR_ROTATE_TIME_SACRAMENTO_APPROACH 1600 // Centering of 400 Works, Rotate of 1700 Works - TUNED TO ROTATE SPEED OF 70 and ROBOT PWM OF 60.
#define BLUE_COLOR_ROTATE_TIME_BILLINGS_APPROACH 450 // Centering of 800 Works, Rotate of 450 Works - TUNED TO ROTATE SPEED OF 70 and ROBOT PWM OF 60.
#define RED_COLOR_ROTATE_TIME_DALLAS_APPROACH 450 // Centering of 1300 Works, Rotate of 550 Works, Straight of 1000 Works.
#define RED_COLOR_ROTATE_TIME_WASHINGTON_APPROACH 1450 // Centering of 50 Works, Rotate of 1500 Works, Straight of 1000 Works.
#define BLUE_STRAIGHT_DRIVE_TIME 1000
#define RED_STRAIGHT_DRIVE_TIME 1000
#define RED_COLOR_DALLAS_APPROACH_CENTERING_TIME 1300 //TUNED.
#define BLUE_COLOR_SACRAMENTO_APPROACH_CENTERING_TIME 200
#define BLUE_COLOR_BILLINGS_APPROACH_CENTERING_TIME 500
#define RED_COLOR_WASHINGTON_APPROACH_CENTERING_TIME 50 // TUNED.
#define SACRAMENTO 0x00010000
#define BILLINGS 0x00030000
#define DALLAS 0x00050000
#define MIAMI 0x00070000
#define SEATTLE 0x00020000
#define WASHINGTON 0x00080000
#define DENVER 0x00040000
#define CONCORD 0x00090000
#define CHICAGO 0x00060000
#define FIRST_SHOT_NOT_ALLOWED 0
#define FIRST_SHOT_ALLOWED 1
#define GAME_OVER 1
#define GAME_STILL_ON 0
/*---------------------------- 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 uint8_t RequestColor(void);
/*---------------------------- Module Variables ---------------------------*/
// everybody needs a state variable, you may need others as well
static ShootingState_t CurrentState;
static uint8_t TeamColor;
static uint32_t PreviousCity;
static uint8_t FirstShotAllowed = FIRST_SHOT_NOT_ALLOWED;
static uint8_t Game_End_Status = GAME_STILL_ON;
/*------------------------------ Module Code ------------------------------*/
/****************************************************************************
Function
RunShooting
Parameters
ES_Event: the event to process
Returns
ES_Event: an event to return
Description
add your description here
Notes
uses nested switch/case to implement the machine.
Author
J. Edward Carryer, 2/11/05, 10:45AM
****************************************************************************/
// Only for testing
ES_Event RunShooting( ES_Event CurrentEvent )
{
bool 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 Stop :
if ( CurrentEvent.EventType != ES_NO_EVENT ) //If an event is active
{
if (CurrentEvent.EventType == ES_START_SHOOTING){
SetPWMLeftWheel(WHEEL_STOP,FWD);
SetPWMRightWheel(WHEEL_STOP,FWD);
SetPWMLeftWheel(ROBOT_PWM,FWD);
SetPWMRightWheel(ROBOT_PWM,FWD);//FirstStartDriving & Start a Timer.
TeamColor = RequestColor();
PreviousCity = QueryPreviousCity();
if(RED_COLOR == TeamColor && (DALLAS == PreviousCity || SACRAMENTO == PreviousCity)){
ES_Timer_InitTimer(ShootDriveTimer, RED_COLOR_DALLAS_APPROACH_CENTERING_TIME); //CENTERS THE ROBOT BY MOVING FORWARD SLIGHLTY.
}
if(BLUE_COLOR == TeamColor && (SACRAMENTO == PreviousCity || DALLAS == PreviousCity) ){
ES_Timer_InitTimer(ShootDriveTimer, BLUE_COLOR_SACRAMENTO_APPROACH_CENTERING_TIME); //STARTS THE ROTATING TIMER.
}
if(BLUE_COLOR == TeamColor && (BILLINGS == PreviousCity || DENVER == PreviousCity)){
ES_Timer_InitTimer(ShootDriveTimer, BLUE_COLOR_BILLINGS_APPROACH_CENTERING_TIME); //STARTS THE ROTATING TIMER.
}
if(RED_COLOR == TeamColor && (WASHINGTON == PreviousCity || CONCORD == PreviousCity )){
ES_Timer_InitTimer(ShootDriveTimer, RED_COLOR_WASHINGTON_APPROACH_CENTERING_TIME); //STARTS THE ROTATING TIMER.
}
NextState = Rotating;//Decide what the next state will be
MakeTransition = true; //mark that we are taking a transition
}
}
break;
case Rotating:
if ( CurrentEvent.EventType != ES_NO_EVENT ) //If an event is active
{
if (ES_TIMEOUT ==CurrentEvent.EventType && ShootDriveTimer == CurrentEvent.EventParam){ // WHEN THE CENTERING STEP IS COMPLETED.
SetPWMLeftWheel(WHEEL_STOP,FWD);
SetPWMRightWheel(WHEEL_STOP,FWD);
SetPWMRightWheel(TURN_SPEED, BACK);
SetPWMLeftWheel(TURN_SPEED, FWD); //START TURNING.
if(BLUE_COLOR == TeamColor && (SACRAMENTO == PreviousCity || DALLAS == PreviousCity) ){
ES_Timer_InitTimer(RotatingTimer, BLUE_COLOR_ROTATE_TIME_SACRAMENTO_APPROACH); //STARTS THE ROTATING TIMER.
}
if(BLUE_COLOR == TeamColor && (BILLINGS == PreviousCity || DENVER == PreviousCity)){
ES_Timer_InitTimer(RotatingTimer, BLUE_COLOR_ROTATE_TIME_BILLINGS_APPROACH); //STARTS THE ROTATING TIMER.
}
if(RED_COLOR == TeamColor && (DALLAS == PreviousCity || SACRAMENTO == PreviousCity)){
ES_Timer_InitTimer(RotatingTimer, RED_COLOR_ROTATE_TIME_DALLAS_APPROACH); //STARTS THE ROTATING TIMER.
}
if(RED_COLOR == TeamColor && (WASHINGTON == PreviousCity || CONCORD == PreviousCity )){
ES_Timer_InitTimer(RotatingTimer, RED_COLOR_ROTATE_TIME_WASHINGTON_APPROACH); //STARTS THE ROTATING TIMER.
}
MakeTransition = true;
NextState = Driving;
}
}
break;
case Driving:
if ( CurrentEvent.EventType != ES_NO_EVENT ) //If an event is active
{
if (ES_TIMEOUT ==CurrentEvent.EventType && RotatingTimer == CurrentEvent.EventParam){ // WHEN THE CENTERING STEP IS COMPLETED.
SetPWMLeftWheel(WHEEL_STOP,FWD);
SetPWMRightWheel(WHEEL_STOP,FWD);
SetPWMLeftWheel (ROBOT_PWM,FWD);
SetPWMRightWheel(ROBOT_PWM,FWD); //Now complete the final Drive.
if(BLUE_COLOR == TeamColor){
ES_Timer_InitTimer(StraightDriveTimer, BLUE_STRAIGHT_DRIVE_TIME); //STARTS THE ROTATING TIMER.
FirstShotAllowed = FIRST_SHOT_ALLOWED;
}
if(RED_COLOR == TeamColor){
ES_Timer_InitTimer(StraightDriveTimer, RED_STRAIGHT_DRIVE_TIME); //STARTS THE ROTATING TIMER.
FirstShotAllowed = FIRST_SHOT_ALLOWED;
}
MakeTransition = true;
NextState = Loading;
}
}
break;
case Loading:
if (CurrentEvent.EventType != ES_NO_EVENT ){ //If an event is active
if (ES_TIMEOUT ==CurrentEvent.EventType && StraightDriveTimer == CurrentEvent.EventParam){ //When StraightDrive is completed.
if(FIRST_SHOT_ALLOWED == FirstShotAllowed){
SetPWMRulerServo(RULER_DOWN_PWM); //Reload the Ruler Servo to the UP Position
FirstShotAllowed = FIRST_SHOT_NOT_ALLOWED; //Only Corrects for the First Shot.
}
SetPWMLeftWheel(WHEEL_STOP,FWD);
SetPWMRightWheel(WHEEL_STOP,FWD);
Game_End_Status = QueryGameOverStatus();
if (Game_End_Status == GAME_STILL_ON){
SetPWMGateServo(GATE_OPEN); //First Open the Gate, Start a Timer to allow for this to happen reliably.
}
if (Game_End_Status == GAME_OVER){
SetPWMGateServo(GATE_CLOSED); //First Open the Gate, Start a Timer to allow for this to happen reliably.
}
ES_Timer_InitTimer(GateTimer, GATE_PERIOD); //2s timer at this point!
MakeTransition = false;
NextState = Loading;
}
if (ES_TIMEOUT == CurrentEvent.EventType && GateTimer == CurrentEvent.EventParam){
SetPWMRulerServo(RULER_UP_PWM);
NextState = Shoot;
SetPWMGateServo(GATE_CLOSED);
ES_Timer_InitTimer(ShootServoTimer,SHOOT_WAIT_PERIOD); //atleast 10s logically.
MakeTransition = true;
HWREG(GPIO_PORTA_BASE+ (GPIO_O_DATA + ALL_BITS)) |= GPIO_PIN_7;
}
}
break;
case Shoot:
if (CurrentEvent.EventType != ES_NO_EVENT ){ //If an event is active
if (ES_TIMEOUT == CurrentEvent.EventType && ShootServoTimer == CurrentEvent.EventParam){
Game_End_Status = QueryGameOverStatus();
if (Game_End_Status == GAME_STILL_ON){
SetPWMRulerServo(RULER_DOWN_PWM); //Reload the Ruler Servo to the UP Position
}
if (Game_End_Status == GAME_OVER){
SetPWMRulerServo(RULER_UP_PWM); //Reload the Ruler Servo to the UP Position
HWREG(GPIO_PORTC_BASE+ (GPIO_O_DATA + ALL_BITS)) &= ~(GPIO_PIN_7); //Switch off LED LIGHT.
}
NextState = Loading;
MakeTransition = true;
ES_Timer_InitTimer(StraightDriveTimer, SHOOT_TO_LOAD_TRANSITION_TIME);
HWREG(GPIO_PORTA_BASE+ (GPIO_O_DATA + ALL_BITS)) &= ~(GPIO_PIN_7);
}
}
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
// this defaults to ES_ENTRY
RunShooting(EntryEventKind);
}
return(ReturnEvent);
}
/****************************************************************************
Function
StartShooting
Parameters
None
Returns
None
Description
Does any required initialization for this state machine
Notes
Author
J. Edward Carryer, 2/18/99, 10:38AM
****************************************************************************/
void StartShooting ( ES_Event 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 = ENTRY_STATE;
}
// call the entry function (if any) for the ENTRY_STATE
RunShooting(CurrentEvent);
}
static uint8_t RequestColor(void){
uint8_t Color;
Color = (HWREG(GPIO_PORTF_BASE+(GPIO_O_DATA + ALL_BITS))) & BIT1HI; // 1 IMPLIES RED LED ON, 0 IMPLIES BLUE LED ON.
if(0 == Color){
return BLUE_COLOR; //Corresponds to Blue Team - 1 - Corresponds to the RED Team.
}
else{
return RED_COLOR;
}
}
Game Intelligence
Updates the winning/losing status every time 3idiots captures a station. Further, the initial game start event, which switches on the green status LED on top of the robot happens in this SM. The information about the win/lose status is used to decide whether to shoot an AdAttack or not.
Header File
/****************************************************************************
Template header file for Hierarchical Sate Machines AKA StateCharts
02/08/12 adjsutments for use with the Events and Services Framework Gen2
3/17/09 Fixed prototpyes to use Event_t
****************************************************************************/
#ifndef GameIntelligence_H
#define GameIntelligence_H
//State definitions for use with the query function
typedef enum { QueryingGameStatus, ProcessingGameStatus} GameIntelligenceState_t ;
// Public Function Prototypes
ES_Event RunGameIntelligence( ES_Event CurrentEvent );
void StartGameIntelligence ( ES_Event CurrentEvent );
int16_t QueryWinLoseStatus (void);
#endif /*GameIntelligence_H */
Source File
/****************************************************************************
Module
GameIntelligence.c
Revision
2.0.1
Description
This is a template file for implementing state machines.
Notes
History
When Who What/Why
-------------- --- --------
****************************************************************************/
/*----------------------------- Include Files -----------------------------*/
// Basic includes for a program using the Events and Services Framework
#include "ES_Configure.h"
#include "ES_Framework.h"
#include "GameIntelligence.h"
#include "ES_DeferRecall.h"
#include "inc/hw_memmap.h"
#include "inc/hw_types.h"
#include "inc/hw_gpio.h"
#include "inc/hw_sysctl.h"
#include "driverlib/sysctl.h"
#include "driverlib/pin_map.h"
#include "driverlib/gpio.h"
#include "inc/hw_pwm.h"
#include "inc/hw_timer.h"
#include "inc/hw_ssi.h"
#include "inc/hw_nvic.h"
#include "driverlib/interrupt.h"
#include "inc/tm4c123gh6pm.h"
#include "TopCommunication.h"
#include "TopRobot.h"
#define FWD 0
#define BACK 1
/* 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
*/
#define START 0
#define DO_NOT_RESTART 1
/*----------------------------- Module Defines ----------------------------*/
// define constants for the states for this machine
// and any other local defines
#define ENTRY_STATE QueryingGameStatus
#define REQUEST_GAME_STATUS 0xC0
//GameStatusType == SS3 SS2 SS1 0xFF - For Caimpaining, what happens is that the SS3 Bye Position - 0x01000000
//Example of Winning of Losing Status
// SS3 - 11000000
// SS2 - 11111111
// SS2 - 11111111
//STATION MASKS, DEFINED HEREBY AS:
#define PS1Mask 0x0000C000
#define PS2Mask 0x00003000
#define PS3Mask 0x00000C00
#define PS4Mask 0x00000300
#define PS5Mask 0x00C00000
#define PS6Mask 0x00300000
#define PS7Mask 0x000C0000
#define PS8Mask 0x00030000
#define PS9Mask 0xC0000000
#define BLUEPS1 0x00004000
#define REDPS1 0x00008000
#define BLUEPS2 0x00001000
#define REDPS2 0x00002000
#define BLUEPS3 0x00000400
#define REDPS3 0x00000800
#define BLUEPS4 0x00000100
#define REDPS4 0x00000200
#define BLUEPS5 0x00400000
#define REDPS5 0x00800000
#define BLUEPS6 0x00100000
#define REDPS6 0x00200000
#define BLUEPS7 0x00040000
#define REDPS7 0x00080000
#define BLUEPS8 0x00010000
#define REDPS8 0x00020000
#define BLUEPS9 0x40000000
#define REDPS9 0x80000000
static uint32_t Caimpaigning_mask = 0x01000000;
#define ALL_BITS (0xff<<2)
#define WHEEL_STOP 5
#define BLUE_COLOR 0
#define RED_COLOR 2
/*---------------------------- 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
*/
uint8_t DecideStationJumping ( uint32_t GameStatus);
int16_t WinOrLose(uint32_t);
int16_t QueryWinLoseStatus (void);
/*---------------------------- Module Variables ---------------------------*/
// everybody needs a state variable, you may need others as well
static GameIntelligenceState_t CurrentState;
static uint8_t StartShootingOneShotTimer = START;
static uint8_t Color;
static int16_t WinLoseStatus;
/*------------------------------ Module Code ------------------------------*/
/****************************************************************************
Function
RunGameIntelligence
Parameters
ES_Event: the event to process
Returns
ES_Event: an event to return
Description
add your description here
Notes
uses nested switch/case to implement the machine.
Author
J. Edward Carryer, 2/11/05, 10:45AM
****************************************************************************/
// Only for testing
ES_Event RunGameIntelligence( ES_Event CurrentEvent )
{
bool MakeTransition = false;// are we making a state transition?
GameIntelligenceState_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 QueryingGameStatus :
if ( CurrentEvent.EventType != ES_NO_EVENT ) //If an event is active
{
if (CurrentEvent.EventType == ES_ENTRY){
ES_Event CurrentEvent;
CurrentEvent.EventType = ES_SPI_XFER;
CurrentEvent.EventParam = REQUEST_GAME_STATUS;
PostTopCommunication(CurrentEvent);
NextState = ProcessingGameStatus;
MakeTransition = true;
}
}
break;
case ProcessingGameStatus:
if ( CurrentEvent.EventType != ES_NO_EVENT ) //If an event is active
{
if(CurrentEvent.EventType == ES_SPI_COMPLETE && ((CurrentEvent.EventParam & Caimpaigning_mask)!=0) ){ //Implies that the Game is ACTIVE.
uint32_t GameStatus = CurrentEvent.EventParam;
uint8_t StationJumping = DecideStationJumping (GameStatus); //Decides the Event Parameter.
CurrentEvent.EventType = ES_GAME_TACTIC;
CurrentEvent.EventParam = StationJumping;
WinLoseStatus = WinOrLose(GameStatus);
PostTopRobot(CurrentEvent);
NextState = QueryingGameStatus;
MakeTransition = true;
HWREG(GPIO_PORTC_BASE+ (GPIO_O_DATA + ALL_BITS)) |= GPIO_PIN_7; //PC7 - SWITCH ON THE GAME STATUS LED.
if(StartShootingOneShotTimer == START){
InitAutoDelayResponseOneShotTimer();
InitTenSecondOneShotTimer();
StartShootingOneShotTimer = DO_NOT_RESTART;
}
}
if (CurrentEvent.EventType == ES_SPI_COMPLETE && ((CurrentEvent.EventParam & Caimpaigning_mask)==0) ){ //Implies that the Game is NOT ACTIVE.
ES_Event CurrentEvent;
CurrentEvent.EventType = ES_SPI_XFER;
CurrentEvent.EventParam = REQUEST_GAME_STATUS;
PostTopCommunication(CurrentEvent);
MakeTransition = false;
NextState = ProcessingGameStatus;
SetPWMLeftWheel (WHEEL_STOP,FWD);
SetPWMRightWheel (WHEEL_STOP,FWD);
HWREG(GPIO_PORTC_BASE+ (GPIO_O_DATA + ALL_BITS)) &= ~(GPIO_PIN_7); //PC7 - SWITCH OFF THE GAME STATUS LED.
}
}
break;
}
// If we are making a state transition
if (MakeTransition == true)
{
// Execute exit function for current state
CurrentEvent.EventType = ES_EXIT;
RunGameIntelligence(CurrentEvent);
CurrentState = NextState; //Modify state variable
// Execute entry function for new state
// this defaults to ES_ENTRY
RunGameIntelligence(EntryEventKind);
}
return(ReturnEvent);
}
/****************************************************************************
Function
StartGameIntelligence
Parameters
None
Returns
None
Description
Does any required initialization for this state machine
Notes
****************************************************************************/
void StartGameIntelligence ( ES_Event 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 = ENTRY_STATE;
}
// call the entry function (if any) for the ENTRY_STATE
RunGameIntelligence(CurrentEvent);
}
uint8_t DecideStationJumping ( uint32_t GameStatus){
return 0;
}
int16_t WinOrLose(uint32_t GameStatus)
{
Color = (HWREG(GPIO_PORTF_BASE+(GPIO_O_DATA + ALL_BITS))) & BIT1HI; // 1 IMPLIES RED LED ON, 0 IMPLIES BLUE LED ON.
int16_t BlueCount=0;
int16_t RedCount=0;
if((GameStatus&PS1Mask)==BLUEPS1){
BlueCount = BlueCount+1;
}
if((GameStatus&PS2Mask)==BLUEPS2){
BlueCount = BlueCount+1;
}
if((GameStatus&PS3Mask)==BLUEPS3){
BlueCount = BlueCount+1;
}
if((GameStatus&PS4Mask)==BLUEPS4){
BlueCount = BlueCount+1;
}
if((GameStatus&PS5Mask)==BLUEPS5){
BlueCount = BlueCount+1;
}
if((GameStatus&PS6Mask)==BLUEPS6){
BlueCount = BlueCount+1;
}
if((GameStatus&PS7Mask)==BLUEPS7){
BlueCount = BlueCount+1;
}
if((GameStatus&PS8Mask)==BLUEPS8){
BlueCount = BlueCount+1;
}
if((GameStatus&PS9Mask)==BLUEPS9){
BlueCount = BlueCount+1;
}
//RED
if((GameStatus&PS1Mask)==REDPS1){
RedCount = RedCount+1;
}
if((GameStatus&PS2Mask)==REDPS2){
RedCount = RedCount+1;
}
if((GameStatus&PS3Mask)==REDPS3){
RedCount = RedCount+1;
}
if((GameStatus&PS4Mask)==REDPS4){
RedCount = RedCount+1;
}
if((GameStatus&PS5Mask)==REDPS5){
RedCount = RedCount+1;
}
if((GameStatus&PS6Mask)==REDPS6){
RedCount = RedCount+1;
}
if((GameStatus&PS7Mask)==REDPS7){
RedCount = RedCount+1;
}
if((GameStatus&PS8Mask)==REDPS8){
RedCount = RedCount+1;
}
if((GameStatus&PS9Mask)==REDPS9){
RedCount = RedCount+1;
}
if(Color==RED_COLOR){
WinLoseStatus = RedCount-BlueCount;
}
if(Color==BLUE_COLOR){
WinLoseStatus = BlueCount-RedCount;
}
return WinLoseStatus;
}
int16_t QueryWinLoseStatus (void){
return WinLoseStatus;
}