Code

/*******************
Module
Controlling_Motor.c
Revision
1.0.0
Description
This is our Controlling_Motor events services module.
driving functions of our motor.
It controls all
Notes
History
When
Who
What/Why
-------------- ---------2/18/15
our Lab 8 Controlling_Motor code.
DT
The below Changes were made:
Pulled
- Changed all names to Controlling_Motor as this module is only for

controlling the motor not all robot movements
- Removed all calls in the Run function other than stopping the motor if the
END_MANEUVER_TIMER Timeout is received. Did this
because we will mainly be calling the public functions in this module, not
running our robot control through this module.
- Removed READSPIX.h, TIMING.h, SPI.h, CheckSensors.h
****************************************************************************/
/*----------------------------- Include Files -----------------------------*/
/* include header files for the framework and this service
*/
#include "ES_Configure.h"
#include "ES_EVENTS.h"
#include "ES_Framework.h"
#include "ES_Port.h"
#include <stdint.h>
#include "inc/hw_memmap.h"
#include "inc/hw_gpio.h"
#include "inc/hw_pwm.h"
#include "inc/hw_sysctl.h"
#include "inc/hw_types.h"
#include "driverlib/gpio.h"
#include "driverlib/ssi.h"
#include "inc/hw_nvic.h"
#include "inc/hw_ssi.h"
#include "inc/hw_timer.h"
#include "bitdefs.h"
//#include "PWM_Module.h"
#include "EventCheckers.h"
#include
#include
#include
#include
"Controlling_Motor.h"
"PeriodicInterruptPID.h"
"PWMGenerator.h"
"MasterSM.h"
#define ALL_BITS (0xff<<2)

/*----------------------------- Module Defines ----------------------------*/
// these times assume a 1.000mS/tick timing
#define ONE_SEC 976
#define HALF_SEC (ONE_SEC/2)
#define TWO_SEC (ONE_SEC*2)
#define FIVE_SEC (ONE_SEC*5)
#define DC_MOTOR_TIMER SERVICE1_TIMER
#define DC_MOTOR_TIMER_DELAY 10
#define NORMALIZING_CONST 4095
#define MOTOR_A (GPIO_PIN_1) //PB1 sets direction of Motor A (controlled by
PB6 PWM)
#define MOTOR_B (GPIO_PIN_2) //PB2 sets direction of Motor B (controlled by
PB7 PWM)
/*---------------------------- Module Functions ---------------------------*/
/* prototypes for private functions for this service.They should be functions
relevant to the behavior of this service
*/
bool InitControlMotor ( uint8_t Priority );
bool PostControlMotor( ES_Event ThisEvent );
ES_Event RunControlMotor( ES_Event ThisEvent );
void InitMotorDirectionLines(void);
void ControlMotor(int control);
void TurnLeft(int control);
void TurnRight(int control);
void SetDutyRightMotor(uint8_t duty);
void SetDutyLeftMotor(uint8_t duty);
void RightMotorForward(void);
void LeftMotorForward(void);
void RightMotorBackward(void);
void LeftMotorBackward(void);
void RotateCC25 (void);
/*---------------------------- Module Variables ---------------------------*/
// with the introduction of Gen2, we need a module level Priority variable
static uint8_t MyPriority;
/*------------------------------ Module Code ------------------------------*/
//Our InitControl Motor Function, calls all initializations related to
controling the motors
bool InitControlMotor( uint8_t Priority )
{
puts("starting init controlmotor\r\n");
ES_Event ThisEvent;
MyPriority = Priority;
_HW_Timer_Init(ES_Timer_RATE_1mS); //initialize and start our timer

END_MANEUVER_TIMER
InitPWMGenerator(); //Initializes port lines for motors (PB6 and PB7)
InitMotorDirectionLines();
InitPeriodicIntPID(); //Periodic Interrupt generates PID control value
// need to have initialized ADMultiService by this point
puts("ending init controlmotor\r\n");

if (ES_PostToService( MyPriority, ThisEvent) == true)
{
return true;
}else
{
return false;
}
/****************************************************************************
Function
PostControlMotor
Posts an event to this module
****************************************************************************/
bool PostControlMotor( ES_Event ThisEvent )
{
return ES_PostToService( MyPriority, ThisEvent);
}
/****************************************************************************
Function
RunControlMotor
Receives posts from the SPI module and generates the appropriate maneuvers
****************************************************************************/
ES_Event RunControlMotor( ES_Event ThisEvent )
{
//puts("Run Control Motor \r\n");
ES_Event ReturnEvent;
ReturnEvent.EventType = ES_NO_EVENT; // assume no errors
switch (ThisEvent.EventType){
// this triggers when the current maneuver is done
case ES_TIMEOUT:
if (ThisEvent.EventParam == END_MANEUVER_TIMER){
StopAndLock(); // stay still and wait for the
next command
}
break;
case EV_GAME_END:
StopAndLock();
//puts("motor stop\r\n");
break;
default:
//don't do anything for any other events
break;
}
return ReturnEvent;
}
/***************************************************************************
public functions
***************************************************************************/
//Stop, hold position, do not move or rotate
/***************************************************************************
private functions
***************************************************************************/
//this function initializes the digital Pins used for directional motor
control
void InitMotorDirectionLines(void)
{
puts("InitMotorDirectionLines \r\n");
/* Enable Ports on the Tiva
* Port F | Port E | Port D | Port C | Port B | Port A
* Bit 5
|
Bit 4 | Bit 3 | Bit 2 | Bit 1 | Bit 0
* example: HWREG(SYSCTL_RCGCGPIO) |= BIT1HI; // Enable GPIO Port B
*/
HWREG(SYSCTL_RCGCGPIO) |= BIT1HI; // Enable GPIO Port B
// wait a few clock cycles after enabling clock
uint8_t pause = HWREG(SYSCTL_RCGCGPIO);
/* Set Port pins to digital or analog function
* GPIO_O_DEN bit: 1 = Digital, 0 = Analog
*/
HWREG(GPIO_PORTB_BASE+GPIO_O_DEN) |= (MOTOR_A | MOTOR_B);
/* Set pin directions to Input or Output
* GPIO_O_DIR bit: 1 = Output, 0 = Input
*/
HWREG(GPIO_PORTB_BASE+GPIO_O_DIR) |= (MOTOR_A | MOTOR_B);
// Set both direction lines low (drive in forward direction)
HWREG(GPIO_PORTB_BASE+(GPIO_O_DATA + ALL_BITS)) &= ~(MOTOR_A |
MOTOR_B);
}
StopAndLock(); //set both motor DCs to zero
/*
* Takes in a control value and controls motors accordingly
*/
void ControlMotor(int control)
{
//printf("Control = %i \r\n", control);
//control = control/2;
//
//
//
//
//
//
//
//
// control is a value between 0 and 50

if ((control > -5) && (control < 5)){ // drive straight
ForwardHalf();
}else if (control <= -5){ // turn left
TurnLeft(control);
}else if (control >= 5){ // turn right
TurnRight(control);
}
uint8_t LeftDuty = GetControlLeftDuty();
uint8_t RightDuty = GetControlRightDuty();
RightMotorForward();
LeftMotorForward();
SetDutyLeftMotor(LeftDuty);
SetDutyRightMotor(RightDuty);
//
//
//
//
//
//
//
//
}
// If Left and Right duty are close to each other (+/- 5)

// then forward full speed
if ((LeftDuty - RightDuty <= 1) || (RightDuty - LeftDuty <= 1)){
ForwardFull();
}else{
SetDutyLeftMotor(LeftDuty);
SetDutyRightMotor(RightDuty);
}
//control is a positive value

void TurnRight(int control)
{
LeftMotorForward();
SetDutyLeftMotor(50);
SetDutyRightMotor(20);
//
//
//
//
//
//
//
}
if (control >= 100){

}else{
SetDutyRightMotor(50-(control>>1));
SetDutyLeftMotor(50+(control>>1));
}
// control is a negative value

void TurnLeft(int control)
{
LeftMotorForward();
//
//
//
//
//
//
//
//
}
control = -1*control;
if (control >= 100){
}else{
SetDutyRightMotor(50+(control>>1));
SetDutyLeftMotor(50-(control>>1));
}
//set both Motor Duty Cycles to zero

void StopAndLock (void)
{
}
// Rotate Clockwise Continuously
void RotateC_Continuous(void)
{
RightMotorBackward();
LeftMotorForward();
}
void RotateCC_Continuous(void)
{
LeftMotorBackward();
}
//Rotate Clockwise by 90 degrees (allow 6 sec to complete)
void RotateC90 (void)
{
LeftMotorForward();
ES_Timer_InitTimer(END_MANEUVER_TIMER, 1500);
}
//Rotate Clockwise by 45 degrees (allows 3 sec to complete)
void RotateC45 (void)
{
LeftMotorForward();
}
//Rotate Counter Clockwise by 90 degrees (allows 6 seconds to complete)

void RotateCC90 (void)
{
//
printf("We are in RotateCC90 Sub Module \n\r");
ES_Timer_InitTimer(END_MANEUVER_TIMER, 1500); //tuned based off our
Lab 8 motors
//
printf("We have finished RotateCC90 Sub Module \n\r");
}
{
ES_Timer_InitTimer(END_MANEUVER_TIMER, 625); //tuned based off our Lab
8 motors
}
{
ES_Timer_InitTimer(END_MANEUVER_TIMER, 300); //tuned based off our Lab
8 motors
}
//Drive forward half speed
void ForwardHalf (void)
{
LeftMotorForward();
}
//Drive forward full speed
void ForwardFull (void)
{
LeftMotorForward();
}
//Drive in reverse full speed

void ReverseFull (void)
{
}
//drive in reverse half speed
void ReverseHalf (void)
{
}
/***************************************************************************
Map Left and Right Motors to PWMA and PWM B
***************************************************************************/
/*
* Maps Motor A to Right Motor
*/
void SetDutyRightMotor(uint8_t duty)
{
if (duty <= 1){
SetBtoZero();
}else if (duty >= 100){
setPWM_B(99);
//SetBto100();
}else{
setPWM_B(duty);
}
//printf("Right %u,", duty);
}
/*
* Maps Motor B to Left Motor
*/
void SetDutyLeftMotor(uint8_t duty)
{
if (duty <= 1){
SetAtoZero();
}else if (duty >= 100){
setPWM_A(99);
//SetAto100();
}else{
setPWM_A(duty);
}
//printf(" Left %u\r\n", duty);
}
/***************************************************************************
Set Motor Directions
***************************************************************************/
/*
* Set left Motor to Forward Direction
*/
void LeftMotorForward()
{
HWREG(GPIO_PORTB_BASE+(GPIO_O_DATA + ALL_BITS)) &= ~(MOTOR_A);
}
/*
* Set right Motor to Forward Direction
*/
void RightMotorForward()
{
HWREG(GPIO_PORTB_BASE+(GPIO_O_DATA + ALL_BITS)) &= ~(MOTOR_B);
}
/*
* Set left Motor to Backward Direction
*/
void LeftMotorBackward()
{
HWREG(GPIO_PORTB_BASE+(GPIO_O_DATA + ALL_BITS)) |= (MOTOR_A);
}
/*
* Set right Motor to Backward Direction
*/
void RightMotorBackward()
{
HWREG(GPIO_PORTB_BASE+(GPIO_O_DATA + ALL_BITS)) |= (MOTOR_B);
}
/***************************************************************************
Test Harness for Controlling_Motor.c
***************************************************************************/
//#define TEST
//test harness to tune the maneuvers
#ifdef TEST
int main(void) {
TERMIO_Init();
_HW_Timer_Init(ES_Timer_RATE_1mS);
puts("\r\n In test harness for Motor Control\r\n");
InitPWM(); //initialize the PWM to ports B6 and B7
InitDirectionalControlPins(); //Initialize ports B0 and B1 as digital
outputs to control the motor direction
ForwardHalf();
//delay_millis(2000);
//StopAndLock();
return 0;
#endif
/*------------------------------- Footnotes -------------------------------*/
/*------------------------------ End of file ------------------------------*/
/****************************************************************************
Module
DCMotorService.c
Revision
1.0.1
Description
This is a file for implementing a simple service under the
Gen2 Events and Services Framework.
Notes
by Antonio X. Cerruto
****************************************************************************/
/*----------------------------- 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
*/
#define clrScrn()
printf("\x1b[2J")
#define ALL_BITS (0xFF<<2)
#include
#include
#include
#include
#include
#include
#include
#include
#include
#include
#include
<stdio.h>
<stdint.h>
"ES_Port.h"
"termio.h"
"inc/hw_memmap.h"
"inc/hw_types.h"
"inc/hw_gpio.h"
"inc/hw_sysctl.h"
"driverlib/gpio.h"
"inc/hw_pwm.h"
"bitdefs.h"
#include
#include
#include
#include
#include
#include
"ES_Configure.h"
"ES_Framework.h"
"ADCSWTrigger.h"
"ADMultiService.h"
"PWMGenerator.h"
"PeriodicInterruptPID.h"
#include "DCMotorService.h"
/*----------------------------- Module Defines ----------------------------*/
#define DC_MOTOR_TIMER SERVICE1_TIMER
#define DC_MOTOR_TIMER_DELAY 10
#define NORMALIZING_CONST 4095
#define MOTOR_A (GPIO_PIN_1) //PB1 sets direction of Motor A (controlled by
PB6 PWM)
#define MOTOR_B (GPIO_PIN_2) //PB2 sets direction of Motor B (controlled by
PB7 PWM)
*/
void setPWMduty(int);
void InitMotorDirectionLines(void);
int GetControlValue(void);
void DriveForward(int control);
void TurnRight(int control);
void TurnLeft(int control);
/*------------------------------ Module Code ------------------------------*/
/****************************************************************************
Function
InitDCMotorService
Parameters
uint8_t : the priorty of this service
Returns
bool, false if error in initialization, true otherwise
Description
Saves away the priority, and does any
other required initialization for this service
Notes
Author
Antonio X. Cerruto
****************************************************************************/
bool InitDCMotorService ( uint8_t Priority )
{
ES_Event ThisEvent;
InitPWMGenerator(); //Initializes port lines for motors (PB6 and PB7)
InitMotorDirectionLines();
InitPeriodicIntPID(); //Periodic Interrupt generates PID control value
// post the initial transition event
ThisEvent.EventType = ES_INIT;
if (ES_PostToService( MyPriority, ThisEvent) == true)
{
return true;
}else
{
return false;
}
}
/****************************************************************************
Function
PostDCMotorService
Parameters
EF_Event ThisEvent ,the event to post to the queue
Returns
bool false if the Enqueue operation failed, true otherwise
Description
Posts an event to this state machine's queue
Notes
Author
Antonio X. Cerruto
****************************************************************************/
bool PostDCMotorService( ES_Event ThisEvent )
{
}
/****************************************************************************
Function
RunDCMotorService
Parameters
ES_Event : the event to process
Returns
ES_Event, ES_NO_EVENT if no error ES_ERROR otherwise
Description
add your description here
Notes
Author
Antonio X. Cerruto
****************************************************************************/
ES_Event RunDCMotorService( ES_Event ThisEvent )
{
if((ThisEvent.EventType == ES_TIMEOUT) && (ThisEvent.EventParam ==

DC_MOTOR_TIMER)){
// read inductor values and calculate control value
//int control = GetControlPID(); // control value calculated
in periodic interrupt
int control = GetControlValue(); // control value calculated
within DCMotorService
// control is a value between 0

if ((control > -10) && (control
DriveForward(100);
}else if (control < 0){ // turn
TurnLeft(control);
}else if (control > 0){ // turn
TurnRight(control);
}
and 100
< 10)){ // drive straight
left
right
ES_Timer_InitTimer(DC_MOTOR_TIMER, DC_MOTOR_TIMER_DELAY);
return ReturnEvent;
/***************************************************************************
private functions
***************************************************************************/
void InitMotorDirectionLines(void)
{
* Bit 5
|
*/
*/
HWREG(GPIO_PORTB_BASE+GPIO_O_DEN) |= (MOTOR_A | MOTOR_B);
*/
HWREG(GPIO_PORTB_BASE+GPIO_O_DIR) |= (MOTOR_A | MOTOR_B);
// Set both direction lines low
HWREG(GPIO_PORTB_BASE+(GPIO_O_DATA + ALL_BITS)) &= ~(MOTOR_A |
MOTOR_B);
}
void DriveForward(int control)
{
}
void TurnLeft(int control)
{
}
void TurnRight(int control)
{
}
/*
* PWM_A corresponds to PB6 (Left Motor)
* PWM_B corresponds to PB7 (Right Motor)
* if control < 0, turn Left
* if control > 0, turn Right
* PB1 sets direction of Motor A (controlled by PB6 PWM)
* PB2 sets direction of Motor B (controlled by PB7 PWM)
*/
void setPWMduty(int control)
{
if((control > -150)&&(control < 150))
{
setPWM_A(90);
setPWM_B(90);
}else if((control <= -100)){ //Turn Left
setPWM_A(90);
setPWM_B(45);
}else if((control >= 100)){ //Turn Right
setPWM_A(45);
setPWM_B(90);
}
int GetControlValue(void)
{
uint32_t* data = getADCData();
int
int
int
int
OuterRight = 100+data[0]; //PE0

InnerRight = 0+data[1]; //PE1
InnerLeft = 0+data[2]; //PE2
OuterLeft = 0+data[3]; //PE3
printf("%i
%i
%i
%i", OuterLeft, InnerLeft, InnerRight,
OuterRight);
int control = 1*((OuterLeft + InnerLeft) - (InnerRight + OuterRight));
}
//scale control value
/*------------------------------- Footnotes -------------------------------*/

/*------------------------------ End of file ------------------------------*/
/****************************************************************************
Module
AlignmentControl.c
This module uses PID control to control the robot using DRS
Coordinates.
A waypoint is entered when the Align function is called.
The robot will turn until it is aligned with this point.
Then, the EV_ROBOT_ALIGNED event will be Posted.
Fuck you, Keil, for locking up when I'd written half this code.
Piece of shit software.
We use this file in two ways.
1. Call Align(xcoord, y coord). This inputs the waypoints we want to
navigate to.
2. Start the Alignment Periodic Timer (and the ISR with it).
This will set us rotating.
When we are aligned, it will turn itself off and Post an
event to the Master.
3. Transition into your Drive State in your state machine.
4. In your drive state, start the Drive Periodic Timer (and the
DriveISR with it).
This will start us driving towards the waypoint and
post an event when the coordinates are reached.
When the waypoint is reached it will post an event to
the Master and turn itself OFF.
****************************************************************************/
/*----------------------------- Include Files -----------------------------*/
// Basic includes for a program using the Events and Services Framework
#include <math.h>
#include <stdint.h>
#include "SPIDRS.h"
#include "GameStatus.h"
#include "Kart1DataSM.h"
#include "MasterMachine.h"
#include "Controlling_Motor.h"
#include "DSRControl.h"
#include "PlayingGameSM.h"
#include "ObstacleCrossingSM.h"
#include
#include
#include
#include
#include
#include
#include
#include
"ES_Port.h"
"inc/hw_memmap.h"
"inc/hw_gpio.h"
"inc/hw_pwm.h"
"inc/hw_sysctl.h"
"inc/hw_types.h"
"inc/hw_pwm.h"
"driverlib/gpio.h"
#define TicksPerMS 40000
#define ControlPriority 7
/*----------------------------- Module Level Variables
-----------------------------*/
int x_waypoint, y_waypoint, x_robotpos,y_robotpos,angle_robot,angle_desired;
static int tolerance = 15; //set this
float error=0;
float last_error=0;
float sum_error=0;
float error_control;
float kp=.7;
float kd=.0;
float ki=.05;
/*----------------------------- Function Prototypes
-----------------------------*/
void Align(int xcoord, int ycoord); //Enter the waypoints and start robot
rotating.
void InitAlignPeriodicTimer(void); //Initialize the Align Periodic Timer and
turns interrupts on.
void StartAlignPeriodicTimer(void); //Starts the Timer(and ISR with it).
void AlignISR(void); //ISR that checks whether we've aligned, posts event, and
stops motor.
void StopAlignPeriodicTimer(void); //Disables the periodic timer (and the ISR
with it).
void InitDrivePeriodicTimer(void); //Initialize the Drive Periodic Timer and
turns interrupts on.
void StartDrivePeriodicTimer(void); //Starts the Timer(and ISR with it).
void DriveISR(void); //ISR that checks whether we've Driveed, posts event, and
stops motor.
void StopDrivePeriodicTimer(void); //Disables the periodic timer (and the ISR
with it).
//uint8_t
//uint8_t
//uint8_t
//uint8_t
//uint8_t
//uint8_t
//uint8_t
//uint8_t
GetX_0
GetY_0
GetX_1
GetY_1
GetX_2
GetY_2
GetX_3
GetY_3
(void);
(void);
(void);
(void);
(void);
(void);
(void);
(void);
/*----------------------------- Functions -----------------------------*/

void Align(int xcoord, int ycoord){
x_waypoint = xcoord;
y_waypoint = ycoord;
}
void InitAlignPeriodicTimer(void){
volatile uint32_t Dummy; // use volatile to avoid over-optimization
// start by enabling the clock to the timer (Wide TIMER1)
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
// 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(WTIMER1_BASE+TIMER_O_CFG) = TIMER_CFG_16_BIT;
// set up timer A in periodic mode so that it repeats the time-outs
HWREG(WTIMER1_BASE+TIMER_O_TAMR) =
(HWREG(WTIMER1_BASE+TIMER_O_TAMR)& ~TIMER_TAMR_TAMR_M)|
TIMER_TAMR_TAMR_PERIOD;
// set timeout to 100mS
HWREG(WTIMER1_BASE+TIMER_O_TAILR) = TicksPerMS * 200;
// enable a local timeout interrupt
HWREG(WTIMER1_BASE+TIMER_O_IMR) |= TIMER_IMR_TATOIM;
// enable the Timer A in Wide TIMER1 interrupt in the NVIC
// it is interrupt number 94 so apppears in EN2 at bit 30
HWREG(NVIC_EN3) = BIT0HI;
// make sure interrupts are enabled globally
__enable_irq();
//Set priority low.
HWREG(NVIC_PRI24) |= (ControlPriority << 7);
}
void StartAlignPeriodicTimer(void){
// now kick the timer off by enabling it and enabling the timer to
// stall while stopped by the debugger
printf("Start Align periodic Timer. \n\r");
HWREG(WTIMER1_BASE+TIMER_O_CTL) |= (TIMER_CTL_TAEN |
TIMER_CTL_TASTALL);
}
void AlignISR(void){
HWREG(WTIMER1_BASE+TIMER_O_ICR) = TIMER_ICR_TATOCINT;
//puts("In my Align ISR \r\n");
//get out data depending on what our kart number is
uint8_t OUR_KART = WhatsOurKartNumber();

switch (OUR_KART) {
case 1:
x_robotpos = GetKart1X();
y_robotpos = GetKart1Y();
angle_robot = GetKart1Orientation();
break;
case 2:
break;
case 3:
break;
}
int x_delta = x_robotpos - x_waypoint;
int y_delta = y_waypoint - y_robotpos;
double theangle_desired = atan2(y_delta, x_delta);
theangle_desired = (theangle_desired * 180)/3.14;
angle_desired = (int) theangle_desired;
//convert the desired angle to 0 - 360 degrees
if (QueryPlayingGameSM() == OBSTACLE_CROSSING &&

(QueryObstacleCrossingSM() != REALIGNING_TO_WIRE)) {
angle_desired = angle_desired - 180;
}
//
//
//
if (angle_desired < 0) {
angle_desired = angle_desired + 360;
}
printf("XR, YR: %i, %i \r\n", x_robotpos, y_robotpos);
printf("XWP, YWP: %i, %i \r\n", x_waypoint, y_waypoint);
printf("XD, YD: %i, %i \r\n", x_delta, y_delta);
//printf("AR, AD: %i, %i \r\n", angle_robot, angle_desired);
int Cangle = abs(angle_robot + 360 - angle_desired);
int CCangle = abs(angle_desired - angle_robot);
if (Cangle < CCangle){
//printf("Rotate C \n\r");
RotateC_Continuous();
} else {
RotateCC_Continuous();
}
if ((angle_robot > (angle_desired-tolerance)) && (angle_robot <

(angle_desired+tolerance))){
ES_Event TheEvent;
StopAlignPeriodicTimer();
printf("Aligned Completed (ISR). Post EV_Align_Completed.
\n\r");
TheEvent.EventType = EV_ALIGN_COMPLETED;
PostMasterSM(TheEvent);
StopAndLock();
}
}
void StopAlignPeriodicTimer(void){
printf("Stop Align periodic timer \n\r");
HWREG(WTIMER1_BASE+TIMER_O_CTL) &= ~(TIMER_CTL_TAEN);
HWREG(WTIMER1_BASE+TIMER_O_CTL) &= ~(BIT0HI);
}
void InitDrivePeriodicTimer(void){
// start by enabling the clock to the timer (Wide TIMER1)
// make sure that timer (Timer A) is disabled before configuring
HWREG(WTIMER1_BASE+TIMER_O_CTL) &= ~TIMER_CTL_TBEN;
// the constant name derives from the 16/32 bit timer, but this is a 32/64
// set up timer A 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;
HWREG(WTIMER1_BASE+TIMER_O_TBILR) = TicksPerMS * 200;
HWREG(WTIMER1_BASE+TIMER_O_IMR) |= TIMER_IMR_TBTOIM;
// enable the Timer A in Wide TIMER1 interrupt in the NVIC

__enable_irq();
//Set priority low.
HWREG(NVIC_PRI24) |= (5 << 15);
}
void StartDrivePeriodicTimer(void){
HWREG(WTIMER1_BASE+TIMER_O_CTL) |= (TIMER_CTL_TBEN |
TIMER_CTL_TBSTALL);
sum_error = 0;
error = 0;
sum_error = 0;
}
void DriveISR(void){
static uint8_t CENTERING_VALUE;
PlayingGameState_t GameState = QueryPlayingGameSM();
if (GameState == OBSTACLE_CROSSING) {
switch (QueryObstacleCrossingSM()) {
case DRIVING_TO_FALL:
//puts("Setting PD vals to Drive to Fall.
\r\n");
CENTERING_VALUE = 95;
ki = 0;
kp = 0.5;
break;
case DRIVING_TO_FIELD:
//puts("Setting PD vals to Drive to Field.
\r\n");
break;
ki = 0;
kp = 0.5;
default:
break;
}
} else {
}
HWREG(WTIMER1_BASE+TIMER_O_ICR) = TIMER_ICR_TBTOCINT;
//puts("In my Drive ISR");
// Set x and y coordinates of positiong from which to shoot
ball
y_waypoint);
/*
to 0-360
//
//x_waypoint = 133;
//y_waypoint = 90;
printf("Way_Points: %i, %i \r\n", x_waypoint,
set gains, requested duty

calculate the desired angle using atan2
sample our actual angle and change to 0-360
calculate angle error = desired angle - actual angle, change
add the angle error to the sum of the error
RequestedDuty = (pGain * ((RPMError) + (iGain * SumError)));
*/
//Drives the robot according to the error.
//If the error is positive, the angle of the robot is greater and we need to
turn right.
//If error is negative, robto angle is smaller and we need to turn left.
//get out data depending on what our kart number is
uint8_t OUR_KART = WhatsOurKartNumber();
switch (OUR_KART) {
case 1:
//puts("Our kart Number is 1. \r\n");
break;
case 2:
break;
case 3:
break;
}
int x_delta = x_robotpos - x_waypoint;
int y_delta = y_waypoint-y_robotpos ;
if ((QueryPlayingGameSM() == OBSTACLE_CROSSING) &&
(QueryObstacleCrossingSM() != REALIGNING_TO_WIRE)) {
//puts("We're Driving Backwards \r\n");
angle_desired = atan2(y_delta, x_delta)*180/3.14;
//added this

}
if (angle_desired > 180) {
angle_desired = angle_desired - 180; //flip the
angle by 180 degrees
}
//printf("Backwards AR, AD: %i, %i \r\n", angle_robot,
angle_desired);
//printf("Xrobot, Yrobot, Xwaypoint, Ywaypoint: %i, %i,

%i, %i \r\n", x_robotpos, y_robotpos, x_waypoint, y_waypoint);
error = abs((float)angle_desired (float)angle_robot) ; //calculate the angle error
sum_error = sum_error + error;
error_control = kp*(error) + (ki*sum_error);
if (error_control>12){error_control=12;}
else if (error_control<1){error_control = 1;}
int ecd = (uint8_t) error_control;
//printf("Final Error is: %i \r\n", ecd);
if (QueryObstacleCrossingSM() == DRIVING_TO_FALL){
if (angle_robot > angle_desired){
//puts("AR>AD, Pulling from LEFT---\r\n");
//
//
SetDutyLeftMotor(CENTERING_VALUE+2-ecd);
SetDutyRightMotor(CENTERING_VALUE+ecd);
SetDutyRightMotor(99-ecd);
}
else if (angle_robot < angle_desired){
//puts("AR<AD, Pulling from
//
//
RIGHT---- \r\n");
//
//
SetDutyLeftMotor(CENTERING_VALUE+ecd);
SetDutyRightMotor(CENTERING_VALUE+2-ecd);
SetDutyLeftMotor(92-ecd);
}
//
//
}
else if (QueryObstacleCrossingSM() == DRIVING_TO_FIELD)

//puts("AR>AD, Pulling from LEFT---\r\n");
//
//
SetDutyLeftMotor(CENTERING_VALUE+2-ecd);
SetDutyRightMotor(CENTERING_VALUE+ecd);
SetDutyLeftMotor(92-ecd);
}
//puts("AR<AD, Pulling from
//
//
RIGHT---- \r\n");
//
//
SetDutyLeftMotor(CENTERING_VALUE+ecd);
SetDutyRightMotor(CENTERING_VALUE+2-ecd);
SetDutyRightMotor(99-ecd);
}
//
//
}
}
else {
angle_desired = atan2(y_delta, x_delta)*180/3.14;
//added this
}
//printf("Xrobot, Yrobot, Xwaypoint, Ywaypoint: %i, %i,
%i, %i \r\n", x_robotpos, y_robotpos, x_waypoint, y_waypoint);
//printf("AR, AD: %i, %i \r\n", angle_robot,
angle_desired);
error = (float)angle_desired - (float)angle_robot ;
//calculate the angle error
sum_error = sum_error + error;
error_control = kp*(error) + (ki*sum_error);
if (error_control>25){error_control=25;}
else if (error_control<1){error_control = 1;}
int ecd = (uint8_t) error_control;
LeftMotorForward();
SetDutyLeftMotor(CENTERING_VALUE+2+ecd);
SetDutyRightMotor(CENTERING_VALUE-ecd);
}

SetDutyLeftMotor(CENTERING_VALUE-ecd);
SetDutyRightMotor(CENTERING_VALUE+2+ecd);
}
}
if ((x_robotpos > (x_waypoint-tolerance)) && (x_robotpos <
(x_waypoint+tolerance))){
if ((y_robotpos > (y_waypoint-tolerance)) && (y_robotpos <
(y_waypoint+tolerance))){
//puts("waypoint reached----+++--\r\n");
StopDrivePeriodicTimer();
ES_Event TheEvent;
TheEvent.EventType = EV_POINT_REACHED;
StopAndLock();
}
}
}
void StopDrivePeriodicTimer(void){
HWREG(WTIMER1_BASE+TIMER_O_CTL) &= ~(TIMER_CTL_TBEN |
TIMER_CTL_TBSTALL);
}
/****************************************************************************
Module
ES_CheckEvents.c
Description
source file for the module to call the User event checking routines
Notes
Users should not modify the contents of this file.
History
When
Who
What/Why
-------------- ---------jec
out all user modifications into ES_Configure
10/16/11 12:32 jec
started coding
*****************************************************************************/
#include
#include
#include
#include
"ES_Configure.h"
"ES_Events.h"
"ES_General.h"
"ES_CheckEvents.h"
// Include the header files for the module(s) with your event checkers.
// This gets you the prototypes for the event checking functions.
#include EVENT_CHECK_HEADER
// Fill in this array with the names of your event checking functions
static CheckFunc * const ES_EventList[]={EVENT_CHECK_LIST };
// Implementation for public functions
/****************************************************************************
Function
ES_CheckUserEvents
Parameters
None
Returns
bool: true if any of the user event checkers returned true, false otherwise
Description
loop through the EF_EventList array executing the event checking functions
Notes
Author
J. Edward Carryer, 10/25/11, 08:55
****************************************************************************/
bool ES_CheckUserEvents( void )
{
uint8_t i;
// loop through the array executing the event checking functions
for ( i=0; i< ARRAY_SIZE(ES_EventList); i++) {
if ( ES_EventList[i]() == true )
break; // found a new event, so process it first
}
if ( i == ARRAY_SIZE(ES_EventList) ) // if no new events
return (false);
else
return(true);
}
/*------------------------------- Footnotes -------------------------------*/
/*------------------------------ End of file ------------------------------*/
/****************************************************************************
Module
FindingWireFromObstacleEndSM.c
****************************************************************************/
/*----------------------------- Include Files -----------------------------*/
*/
#include "FindingWireFromObstacleEnd.h"
/*----------------------------- Module Defines ----------------------------*/
// define constants for the states for this machine
// and any other local defines
#define ENTRY_STATE TURNING_CC45
/* 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
ES_Event RunFindingWireFromObstacleEndSM( ES_Event CurrentEvent );

void StartFindingWireFromObstacleEndSM ( ES_Event CurrentEvent );
FindingWireFromObstacleEndSMState_t QueryFindingWireFromObstacleEndSM ( void
);
static ES_Event DuringTurningCC45Obstacle( ES_Event Event);
static ES_Event DuringDrivingForwardFromObstacle( ES_Event Event);
// everybody needs a state variable, you may need others as well
static FindingWireFromObstacleEndSMState_t CurrentState;
/*------------------------------ Module Code ------------------------------*/
/****************************************************************************
Function
RunFindingWireFromObstacleEndSM
****************************************************************************/
ES_Event RunFindingWireFromObstacleEndSM( ES_Event CurrentEvent )
{
bool MakeTransition = false;/* are we making a state transition? */
FindingWireFromObstacleEndSMState_t NextState = CurrentState;
ES_Event EntryEventKind = { ES_ENTRY, 0 };// default to normal entry to new
state
ES_Event ReturnEvent = CurrentEvent; // assume we are not consuming event
switch ( CurrentState )
{
case TURNING_CC45 :
// If current state is state one
// Execute During function for state one. ES_ENTRY & ES_EXIT are
// processed here allow the lower level state machines to re-map
// or consume the event
CurrentEvent = DuringTurningCC45Obstacle(CurrentEvent);
//process any events
if ( CurrentEvent.EventType != ES_NO_EVENT ) //If an event is active
{
switch (CurrentEvent.EventType)
{
case ES_TIMEOUT : //If event is event one
(CurrentEvent.EventParam == END_MANEUVER_TIMER) {
if
/
/ Execute action function for state one : event one

NextState = DRIVING_FORWARD_OB;//Decide what the next state will be
/ for internal transitions, skip changing MakeTransition
MakeTransition = true; //mark that we are taking a transitiom
}
break;
transition events
default: //no other
break;
// repeat cases as required for relevant events
}
}
break;
// repeat state pattern as required for other states
case DRIVING_FORWARD_OB:
CurrentEvent =
DuringDrivingForwardFromObstacle(CurrentEvent);
if ( CurrentEvent.EventType !=
ES_NO_EVENT ) //If an event is active
{
{
default: //no
transition events. Upper level SM handles transition into following wire
break;
}
}
break;
}
//
If we are making a state transition
if (MakeTransition == true)
{
//
Execute exit function for current state
CurrentEvent.EventType = ES_EXIT;
RunFindingWireFromObstacleEndSM(CurrentEvent);
CurrentState = NextState; //Modify state variable
//
Execute entry function for new state
// this defaults to ES_ENTRY
RunFindingWireFromObstacleEndSM(EntryEventKind);
}
return(ReturnEvent);
}
/****************************************************************************
Function
StartFindingWireFromObstacleEndSM
****************************************************************************/
void StartFindingWireFromObstacleEndSM ( 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
RunFindingWireFromObstacleEndSM(CurrentEvent);
}
/****************************************************************************
Function
QueryFindingWireFromObstacleEndSM
****************************************************************************/
FindingWireFromObstacleEndSMState_t QueryFindingWireFromObstacleEndSM ( void )
{
return(CurrentState);
}
/***************************************************************************
private functions
***************************************************************************/
static ES_Event DuringTurningCC45Obstacle( 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) )
{
// implement any entry actions required for this state machine
RotateCC45();
// after that start any lower level machines that run in this state
//StartLowerLevelSM( Event );
// repeat the StartxxxSM() functions for concurrent state machines
// on the lower level
}
else if ( Event.EventType == ES_EXIT )
{
StopAndLock();
// on exit, give the lower levels a chance to clean up first
//RunLowerLevelSM(Event);
// repeat for any concurrently running state machines
// now do any local exit functionality
}else
// do the 'during' function for this state
{
// run any lower level state machine
// ReturnEvent = RunLowerLevelSM(Event);
// repeat for any concurrent lower level machines
// do any activity that is repeated as long as we are in this state
}
// return either Event, if you don't want to allow the lower level machine
// to remap the current event, or ReturnEvent if you do want to allow it.

}
static ES_Event DuringDrivingForwardFromObstacle( ES_Event Event)
{
{
ForwardHalf();
}
{
}else
{
}
}
/****************************************************************************
Module
FindingWireFromShootingSM.c
****************************************************************************/
/*----------------------------- Include Files -----------------------------*/
*/
#include "FindingWireFromShootingSM.h"
/*----------------------------- Module Defines ----------------------------*/

#define ENTRY_STATE TURNING_C45
*/
ES_Event RunFindingWireFromShootingSM( ES_Event CurrentEvent );
void StartFindingWireFromShootingSM ( ES_Event CurrentEvent );
FindingWireFromShootingSMState_t QueryFindingWireFromShootingSM ( void );
static ES_Event DuringTurningC45Shooting( ES_Event Event);
static ES_Event DuringDrivingBackwards( ES_Event Event);
static FindingWireFromShootingSMState_t CurrentState;
/*------------------------------ Module Code ------------------------------*/
/****************************************************************************
Function
RunFindingWireFromShootingSM
****************************************************************************/
ES_Event RunFindingWireFromShootingSM( ES_Event CurrentEvent )
{
FindingWireFromShootingSMState_t NextState = CurrentState;
state
{
case TURNING_C45 :
CurrentEvent = DuringTurningC45Shooting(CurrentEvent);
{
{
if
NextState = DRIVING_BACKWARDS;//Decide what the next state will be

}
break;
default: //no other

transition events
break;
}
}
break;
case DRIVING_BACKWARDS:
CurrentEvent =
DuringDrivingBackwards(CurrentEvent);
{
{
default: //no
break;
}
}
break;
}
//
{
//
RunFindingWireFromShootingSM(CurrentEvent);
//
RunFindingWireFromShootingSM(EntryEventKind);
}
}
/****************************************************************************
Function
StartFindingWireFromShootingSM
****************************************************************************/
void StartFindingWireFromShootingSM ( ES_Event CurrentEvent )
//
//
//
//
if
{
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
( ES_ENTRY_HISTORY != CurrentEvent.EventType )
}
RunFindingWireFromShootingSM(CurrentEvent);
/****************************************************************************
Function
QueryFindingWireFromShootingSM
****************************************************************************/
FindingWireFromShootingSMState_t QueryFindingWireFromShootingSM ( void )
{
}
/***************************************************************************
private functions
***************************************************************************/
static ES_Event DuringTurningC45Shooting( ES_Event Event)
{
{
RotateC45();
}
{
StopAndLock();
}else
{

}
static ES_Event DuringDrivingBackwards( ES_Event Event)

{
{
ReverseHalf();
}
{
}else
{
}
}
/****************************************************************************
Module
FindingWireFromStartSM.c
****************************************************************************/
/*----------------------------- Include Files -----------------------------*/
*/
#include "FindingWireFromStartSM.h"
/*----------------------------- Module Defines ----------------------------*/
#define ENTRY_STATE TURNING_CC_45
*/
ES_Event RunFindingWireFromStartSM( ES_Event CurrentEvent );
void StartFindingWireFromStartSM ( ES_Event CurrentEvent );
FindingWireFromStartSMState_t QueryFindingWireFromStartSM ( void );
static ES_Event DuringTurningCC45( ES_Event Event);
static ES_Event DuringDrivingForward( ES_Event Event);
static FindingWireFromStartSMState_t CurrentState;
/*------------------------------ Module Code ------------------------------*/
/****************************************************************************
Function
RunFindingWireFromStartSM
****************************************************************************/
ES_Event RunFindingWireFromStartSM( ES_Event CurrentEvent )
{
FindingWireFromStartSMState_t NextState = CurrentState;
state
{
case TURNING_CC_45 :
//puts("turning\r\n");
// Execute During function for state one. ES_ENTRY &
ES_EXIT are
CurrentEvent = DuringTurningCC45(CurrentEvent);
{
{

if
puts("gonna drive straight now\r\n");
//
Execute action function for state one : event one
NextState = DRIVING_FORWARD;//Decide what the next state will be

/

}
break;
default: //no other
transition events
break;
}
}
break;
case DRIVING_FORWARD:
DuringDrivingForward(CurrentEvent);
CurrentEvent =

{
{
default: //no
break;
}
}
break;
}
//
{
//
RunFindingWireFromStartSM(CurrentEvent);
//
RunFindingWireFromStartSM(EntryEventKind);
}
}
/****************************************************************************
Function
StartFindingWireFromStartSM
****************************************************************************/
void StartFindingWireFromStartSM ( ES_Event CurrentEvent )
{
// is started
{
}
RunFindingWireFromStartSM(CurrentEvent);
}
/****************************************************************************
Function
QueryFindingWireFromStartSM
****************************************************************************/
FindingWireFromStartSMState_t QueryFindingWireFromStartSM ( void )
{
}
/***************************************************************************
private functions
***************************************************************************/
static ES_Event DuringTurningCC45( ES_Event Event)
{
{
RotateCC45();
}
{
StopAndLock();
}else
{

}
static ES_Event DuringDrivingForward( ES_Event Event)

{
{
ForwardHalf();
}
{
}else
{
}
}
/****************************************************************************
Module
FindingWireSM.c
Revision
2.0.1
Description
This is a state machine for Finding Wire.
Notes
History
When
Who
What/Why
-------------- ---------2/15/15
DT
Begin Coding
2/19/2015
DT
Completed
InStartingZone
****************************************************************************/
/*----------------------------- Include Files -----------------------------*/
*/
#include "FindingWireSM.h"
#include "InStartingZoneService.h"
#include "FollowingWireSM.h"
/*----------------------------- Module Defines ----------------------------*/
#define ENTRY_STATE IN_STARTING_ZONE
*/
static ES_Event DuringFindingWireSM( ES_Event Event);
static FindingWireState_t CurrentState;
/*------------------------------ Module Code ------------------------------*/
/****************************************************************************
Function
RunFindingWireSM
Parameters
ES_Event: the event to process
Returns
ES_Event: an event to return
Description
Notes
uses nested switch/case to implement the machine.
Author
DT , 2/15/15
****************************************************************************/
ES_Event RunFindingWireSM( ES_Event CurrentEvent )
{
bool MakeTransition = false;
FindingWireState_t NextState = CurrentState;
volatile ES_Event EntryEventKind = { ES_ENTRY, 0 };// default to
normal entry to new state
volatile ES_Event ReturnEvent = CurrentEvent; // assume we are not consuming
event
//start by passing events to the lower level machines
CurrentEvent= DuringFindingWireSM(CurrentEvent);
//in the absence of an error the top level state machine should
//always return ES_No_Event
//
CurrentEvent.EventType= ES_NO_EVENT;
return(CurrentEvent);
}
/****************************************************************************
Function
StartFindingWireSM
Parameters
None
Returns
None
Description
Does any required initialization for this state machine
Notes
Author
DT 2/15/15
****************************************************************************/
void StartFindingWireSM ( ES_Event CurrentEvent )
{
// is started
{
}
RunFindingWireSM(CurrentEvent);
}
/****************************************************************************
Function
QueryTemplateSM
Parameters
None
Returns
TemplateState_t The current state of the Template state machine
Description
returns the current state of the Template state machine
Notes
Author
J. Edward Carryer, 2/11/05, 10:38AM
****************************************************************************/
FindingWireState_t QueryTemplateSM ( void )
{
}
/***************************************************************************
private functions
***************************************************************************/
static ES_Event DuringFindingWireSM( ES_Event Event)
{
//

{
printf("We are in the FindingWireSM with ES ENTRY\n\r");
//Call StartInStartingZone with ES_ENTRY
StartInStartingZone(Event);
//Call StartInObstacleEndingZone with ES_ENTRY
//Call StartInExitingShootingZone with ES_ENTRY
}
//Else If the Event.EventType = ES_EXIT (give the lower level

functions an opportunity to clean up)
{
//
printf("We are in the FindingWireSM with ES EXIT\n\r");
//Call RunInStartingZone with ES_ENTRY
RunInStartingZone(Event);
//Call RunInObstacleEndingZone with ES_ENTRY
//Call RunInExitShootingZone with ES_ENTRY
}else
{
//
printf("We are in the FindingWireSM with other
Event\n\r");
//Run the Lower Level State Machine InStartingZone
RunInStartingZone(Event);
InObstacleEndingZone
//Run the Lower Level State Machine
//Run the Lower Level State Machine InExitShootingZone

}
}
/****************************************************************************
Module
FollowingWire.c
Revision
1.0.0
Description
This is a state machine for Following Wire.
Notes
History
When
Who
-------------- --2/19/15
What/Why
-------DT
Begin Coding
****************************************************************************/
/*----------------------------- Include Files -----------------------------*/
*/
#include "PWM_Module.h"
#include "PeriodicInterruptPID.h"
/*----------------------------- Module Defines ----------------------------*/
behavior of this state machine*/
//static FollowingWireSM_t CurrentState;
/*------------------------------ Module Code ------------------------------*/

/****************************************************************************
Function
RunFollowingWireSM
Parameters
Returns
Description
This is our Events Services model for following the wire
Notes
Author
DT
2/19/15
****************************************************************************/
ES_Event RunFollowingWireSM( ES_Event CurrentEvent )
{
//Assume we are not consuming an event
ES_Event ReturnEvent = CurrentEvent;
if( CurrentEvent.EventType != ES_NO_EVENT) //If an
event is active
{
//
printf("Event is active\n\r");
switch(CurrentEvent.EventType)
{
int control;
case ES_ENTRY:
printf("We are following the
wire \n\r");
//Call following wire drive
function
//control = GetControlPID(); //
changed to turn right because initial turn onto tape is too sharp
//RotateC45(); // changed to
turn right because initial turn onto tape is too sharp
ES_Timer_InitTimer(FOLLOW_WIRE_TIMER, 500);
//consume this event
//ReturnEvent.EventType =
ES_NO_EVENT;
break;
FOLLOW_WIRE_TIMER) {
case ES_TIMEOUT:
if (CurrentEvent.EventParam ==
control = GetControlPID();
ControlMotor(control);
ES_Timer_InitTimer(FOLLOW_WIRE_TIMER, 10);
}
break;
default:
break;
}
/****************************************************************************
Module
GameStatus.c
Handles querying the game status from the DRS
****************************************************************************/
/*----------------------------- Include Files -----------------------------*/
*/
#include "SPIDRS.h"
#include <stdint.h>
/*----------------------------- Module Defines ----------------------------*/
#define ENTRY_STATE QueryingDRS
*/
ES_Event RunGameStatusSM( ES_Event CurrentEvent );
void StartGameStatusSM ( ES_Event CurrentEvent );
GameStatusState_t QueryGameStatusSM ( void );
void SendGameStatusQuery (void);
static ES_Event DuringQueryingDRS( ES_Event Event);
static ES_Event DuringReadData (ES_Event Event);
uint8_t AcquireKart1Laps (void);

void ReadTheData(void);
volatile static GameStatusState_t CurrentState;
static uint8_t Kart1GameStatusData;
static uint8_t Kart1Laps;
/*------------------------------ Module Code ------------------------------*/

/****************************************************************************
Function
RunGameStatusSM
****************************************************************************/
ES_Event RunGameStatusSM( ES_Event CurrentEvent )
{
volatile GameStatusState_t NextState = CurrentState;
state
{
case QueryingDRS :
//puts("GameStatusSM > Base Query\r\n");
//execute during function for BaseQuery
CurrentEvent = DuringQueryingDRS(CurrentEvent);
{
{
case EV_EOTinterrupt:
ReadData;
NextState =
MakeTransition =
true;
break;
default: //do nothing

break;
}
}
break;
CompletedExchange\r\n");
DuringReadData(CurrentEvent);
case ReadData:
// puts("GameStatusSM >
CurrentEvent =
{
{
This should remain here
default: //do nothing.
break;
}
break;
}
//
{
//
RunGameStatusSM(CurrentEvent);

//
CurrentEvent.EventType = ES_ENTRY;
RunGameStatusSM(EntryEventKind);
}
}
/****************************************************************************
Function
StartGameStatusSM
****************************************************************************/
void StartGameStatusSM ( ES_Event CurrentEvent )
{
//local variable to get debugger to display the value of currentevent
volatile ES_Event LocalEvent = CurrentEvent;
// is started
{
}
RunGameStatusSM(CurrentEvent);
}
/****************************************************************************
Function
QueryGameStatusSM
****************************************************************************/
GameStatusState_t QueryGameStatusSM ( void )
{
}
/***************************************************************************
private functions
***************************************************************************/
static ES_Event DuringQueryingDRS( ES_Event Event)
{
//local variable to get debugger to display the value of
currentevent
volatile ES_Event NewEvent = Event;
{
SendGameStatusQuery();
for (int i = 0; i < 7; i++){

SendPrimingByte();
}
}
{
}else
{

}
return(NewEvent);
static ES_Event DuringReadData (ES_Event Event)


{
ReadTheData();
ES_Timer_InitTimer(SPIDRS_TIMER, 5); // set the
SPIDRS timer for 5 ms
}
{
}else
{
}
return(NewEvent);
void ReadTheData(void)
{
for (int i = 1; i <= 8; i++) {
uint8_t data = grabData();
switch (i){
case 4: //reading Kart 1 Status Data
Kart1GameStatusData = data;
//printf("fourth byte: %u\r\n",Kart1Laps);
break;
//printf("fifth byte: %u\r\n",Kart2Laps);
break;
//printf("sixth byte: %u\r\n",Kart3Laps);
break;
default: //insignificant byte
break;
}
}
void SendGameStatusQuery (void) {
HWREG(SSI0_BASE + SSI_O_DR)= 0x3F; //Send a Query Byte to the

Command Generator
HWREG(SSI0_BASE + SSI_O_IM) |= SSI_IM_TXIM; //enable the local
interrupt
}
/*****************************************************************
Return Functions
-Allow other modules to access this module's information
*******************************************************************/
uint8_t AcquireKart1Laps (void)
{
Kart1Laps = Kart1GameStatusData & 0x07;
return Kart1Laps;
}
{
return Kart2Laps;
}
{
return Kart3Laps;
}
uint8_t QueryGameStatus (void) {
uint8_t GameStatusByte;
GameStatusByte = (Kart1GameStatusData >> 3) & 0x03;
return GameStatusByte;
}
uint8_t SeeIfKart1ShotMade (void) {
uint8_t Kart1MadeShot;
Kart1MadeShot = (Kart1GameStatusData >> 7) & 0x01;
return Kart1MadeShot;
}
}
}
uint8_t SeeIfKart1ObstacleCompleted (void) {
uint8_t Kart1ObstacleCompleted;
Kart1ObstacleCompleted = (Kart1GameStatusData >> 6) & 0x01;
return Kart1ObstacleCompleted;

}
}
/****************************************************************************
Module
InStartingZoneService.c
Revision
1.0.1
Description
This is a state machine for InStartingZone.
Notes
History
When
Who
What/Why
-------------- ---------2/15/15
2/18/15
Coding. Made the following Changes:
DT
DT
Begin Coding
Continued
- Modified PWM_Module
- Took Robot Control from Lab 8 and created Controlling_Motor.c and .h
2/19/15
DT
Completed
inStartingZone
****************************************************************************/
/*----------------------------- Include Files -----------------------------*/
*/
#include "PWM_Module.h"
/*----------------------------- Module Defines ----------------------------*/
#define ENTRY_STATE TURNING_90_CCW

behavior of this state machine*/

static InStartingZone_t CurrentState;
/*------------------------------ Module Code ------------------------------*/
/****************************************************************************
Function
RunInStartingZone
Parameters
Returns
Description
Notes
Author
DT
2/15/15
****************************************************************************/
ES_Event RunInStartingZone( ES_Event CurrentEvent )
{
//Create a boolean for MakeTransition
bool MakeTransition= false;
//Set the NextState equal to the CurrentState
InStartingZone_t NextState= CurrentState;
//Assume we are not consuming an event
//Switch between Current State
switch (CurrentState)
{
//
event is active
//
//If the case equals Turning90CCW

case TURNING_90_CCW :
printf("We are TURNING_90_CCW\n\r");
if( CurrentEvent.EventType != ES_NO_EVENT) //If an
{
printf("Event is activeW\n\r");
switch(CurrentEvent.EventType)
{
case EV_GAME_START:
printf("We are
TURNING_45_CCW\n\r");
//Call Turning90CCW motor

function
RotateCC45();
ES_Timer_InitTimer(HACKTIMER,
3500);
//
printf("We have finisqhed

//Set Next State equal to
Driving Forward
break;
case ES_TIMEOUT:
HACKTIMER){
printf("TIMEOUT, next
step Driving Forward.\n\r");
NextState =
DRIVING_FORWARD;
//Set MakeTransition=
True
MakeTransition= true;
ReturnEvent.EventType =
ES_NO_EVENT;
//
printf("We have finished

}
break;
default:
break;
}
break;
is active
//If the case equals Driving Forward

case DRIVING_FORWARD:
printf("Driving Forward > Forward Half.\n\r");
ForwardHalf();
if (CurrentEvent.EventType !=ES_NO_EVENT) //If an event
//
state\n\r");
DRIVING_FORWARD\n\r");
function
printf("Event is active in the DRIVING_FORWARD

//Create an event switch statement
{
case ES_ENTRY:
printf("We are now
//Call DriveForward motor
ForwardHalf();
//Set Next State equal to
Orienting kart
NextState = ORIENTING_KART;
//Set make Transition = true
MakeTransition = true;
ReturnEvent.EventType =
ES_NO_EVENT;
break;
default:
break;
}
}
//If the case equals Orienting Kart
case ORIENTING_KART:
{
//If the CurrentEvent is equal to the
TapeOrient Flag
case EV_TAPE_ORIENT:
printf("We are now ORIENTING KART
\n\r");
//Call PID for orienting the tape

int control = GetControlPID();
ES_Timer_InitTimer(ORIENT_TIMER, 10);
break;
case ES_TIMEOUT:
ORIENT_TIMER) {
int control = GetControlPID();

ES_Timer_InitTimer(ORIENT_TIMER,
10);
}
break;
default:
break;
}
default:
break;
//
}
printf("We have run all the switch cases\n\r");
//
//
{
//Execute exit functions for the current state
RunInStartingZone(CurrentEvent);
CurrentState= NextState;
printf("We have set CurrentState equal to Nextstate\n\r");
//Execute entry functions for the current state

CurrentEvent.EventType= ES_ENTRY;
}
//
//
//
//
//in the absence of an error the top level state machine should
//always return ES_No_Event
CurrentEvent.EventType= ES_NO_EVENT;
printf("We have run through the entire function \n\r");
//return the ReturnEvent
}
/****************************************************************************
Function
StartInStartingZone
Parameters
None
Returns
None
Description
Notes
Author
DT 2/15/15
****************************************************************************/
void StartInStartingZone ( ES_Event CurrentEvent )
{
// is started
{
}
}
/***************************************************************************
private functions
***************************************************************************/
/*
Module : IRSensingModule.c
Ported from Lab 8 code to check for the IR beam
Uses pin C4 to time the IR frequency
*/
#include "MasterSM.h"
#include <stdint.h>
#include <stdbool.h>
#include <stdio.h>
#define ControlPriority 7
void InitCaptureTimer(void);
void IRPeriodCaptureISR(void);
void StartIRPeriodCaptureISR(void);
uint32_t TimeNow;
uint32_t TimePast;
uint32_t Period;
uint32_t msperiod;
uint32_t Dummy;
uint32_t DutyCycle;
uint32_t RPM; //actual measured RPM
uint32_t DRPM; //Desired RPM from the Encoder
float hz_freq;
float f_msperiod;
void InitCaptureTimer(void){
//Enable the Clock to Wide Timer 0
//Kill Time For a Bit
//Enable the Clock to Port C (What does this mean)?
HWREG(SYSCTL_RCGCGPIO) |= SYSCTL_RCGCGPIO_R2;
//Disable Timer 0
HWREG(WTIMER0_BASE+TIMER_O_CTL) &= ~TIMER_CTL_TAEN;
//Set up Timer 0 in 32bit Wide Mode
HWREG(WTIMER0_BASE+TIMER_O_CFG) = TIMER_CFG_16_BIT; //why 16??
//To use the full count before turnover, write FFFFFFFF to the Interval Load
Register
HWREG(WTIMER0_BASE+TIMER_O_TAILR) = 0xffffffff;
//Set up Timer A in Capture Mode (TAMR=3, TAAMS = 0), For Rising Edge Time
(TACMR=1), Up Counting (TACDIR=0);
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));
//Modify the TAEvent bits to set the interrupt even to Rising Edge. We do this
by clearing the two TAEVENT bits.
HWREG(WTIMER0_BASE+TIMER_O_CTL) &= ~TIMER_CTL_TAEVENT_M;
//Now the Timer is Set Up in 32BitWide mode with the right Turnover Interval
and Input Capture Time Rising Edge is enabled.
//Set an Alternate Function on Port C Pin 4.
HWREG(GPIO_PORTC_BASE+GPIO_O_AFSEL) |= BIT4HI;
//Write 7 to the PortC base PCTL register to set the Alternate Function to
Capture/Compare.
HWREG(GPIO_PORTC_BASE+GPIO_O_PCTL) = (HWREG(GPIO_PORTC_BASE+GPIO_O_PCTL) &
0xFFF0FFFF) + (7<<16);
//Enable PORTC Pin4 as Digital IO.
HWREG(GPIO_PORTC_BASE+GPIO_O_DEN) |= (BIT4HI);
HWREG(GPIO_PORTC_BASE+GPIO_O_DIR) |= (BIT4HI);
//Enable a local capture interrupt //INTERRUPT ENABLE 1
HWREG(WTIMER0_BASE+TIMER_O_IMR) |= TIMER_IMR_CAEIM;
//Enable the Timer A in Wide Timer 0 interrupt in the NVIC //INTERRUPT ENABLE
2
HWREG(NVIC_EN2) |= BIT30HI;
//Enable Global Interrupts // INTERRUPT ENABLE 3
__enable_irq();
}
void IRPeriodCaptureISR(void){
static uint8_t timesTriggered = 0;
// start by clearing the source of the interrupt, the input capture
event
HWREG(WTIMER0_BASE+TIMER_O_ICR) = TIMER_ICR_CAECINT;
//puts("Encoder ISR \r\n");
TimeNow = HWREG(WTIMER0_BASE+TIMER_O_TAR);
Period = TimeNow-TimePast;
float f_period = (float)Period;
f_msperiod = (f_period)/40000;
//float ms_period_f = (float)msperiod;
float s_period_f = f_period/1000;
hz_freq = 1000/f_msperiod;
TimePast=TimeNow;
//printf("frequency %f\r\n", hz_freq);
if ((hz_freq > 1100) && (hz_freq < 1300))
{
timesTriggered++;
//puts("posting IR hi\r\n");
if (timesTriggered > 3) {
ES_Event TheEvent;
TheEvent.EventType = EV_IR_HIGH;
timesTriggered = 0;
}
}
}
void StartIRPeriodCaptureISR(void) {
//Start 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 StopIRPeriodCaptureISR (void) {
HWREG(WTIMER0_BASE+TIMER_O_CTL) &= ~(TIMER_CTL_TAEN |
TIMER_CTL_TASTALL);
}
/****************************************************************************
Module
Kart1DataSM.c
Home of Kart 1 position and orientation data
****************************************************************************/
/*----------------------------- Include Files -----------------------------*/
*/
#include "SPIDRS.h"
#include <stdint.h>
/*----------------------------- Module Defines ----------------------------*/
#define ENTRY_STATE Kart1QueryingDRS

#define BITS_PER_NIBBLE 8
*/
ES_Event RunKart1DataSM( ES_Event CurrentEvent );
void StartKart1DataSM ( ES_Event CurrentEvent );
Kart1DataState_t QueryKart1DataSM( void );
static ES_Event DuringKart1QueryingDRS( ES_Event Event);
static ES_Event DuringReadingKart1Data (ES_Event Event);
void SendKart1DataQuery (void);
uint16_t GetKart1X (void);
uint16_t GetKart1Y (void);
int16_t GetKart1Orientation (void);
static Kart1DataState_t CurrentState;
static uint16_t Kart1X_MSB;
static uint16_t Kart1X_LSB;
static uint16_t Kart1X;
static uint16_t Kart1Y_MSB;
static uint16_t Kart1Y_LSB;
static uint16_t Kart1Y;
static uint16_t Kart1Orientation_MSB;
static uint16_t Kart1Orientation_LSB;
static int16_t Kart1Orientation;
/*------------------------------ Module Code ------------------------------*/
/****************************************************************************
Function
RunKart1DataSM
****************************************************************************/
ES_Event RunKart1DataSM( ES_Event CurrentEvent )
{
Kart1DataState_t NextState = CurrentState;
state
{
case Kart1QueryingDRS:
//puts("Kart1DataSM > Kart1BaseQuery\r\n");
CurrentEvent = DuringKart1QueryingDRS(CurrentEvent);
{
{
NextState =
ReadingKart1Data;
MakeTransition =
true;
break;
break;
}
}
break;
case ReadingKart1Data:
//puts("Kart1DataSM >
CompletedKart1DataExchange\r\n");
CurrentEvent = DuringReadingKart1Data(CurrentEvent);
{
{
Terminal State
}
break;
break;
}
//
{
//
RunKart1DataSM(CurrentEvent);
//
RunKart1DataSM(EntryEventKind);
}
}
/****************************************************************************
Function
StartKart1DataSM
****************************************************************************/
void StartKart1DataSM ( ES_Event CurrentEvent )

{
// is started
{
}
}
/****************************************************************************
Function
QueryKart1DataSM
****************************************************************************/
Kart1DataState_t QueryKart1DataSM( void )
{
}
/***************************************************************************
private functions
***************************************************************************/
static ES_Event DuringKart1QueryingDRS( ES_Event Event)
{
currentevent
{
SendKart1DataQuery();
for (int i = 0; i < 7; i++){
SendPrimingByte();
}
}
{
}else
{

}
return(NewEvent);
static ES_Event DuringReadingKart1Data (ES_Event Event)

{
{
ReadKart1Data();

}
{
}else
{
//this is a terminal state
}
return(NewEvent);
void ReadKart1Data (void)

{
for (int i = 1; i <= 8; i++) {
switch (i) {
case 3:
Kart1X_MSB = data;
//printf("third byte: %u\r\n", Kart1X_MSB);
break;
case 4:
Kart1X_LSB = data;
//printf("fourth byte: %u\r\n", Kart1X_LSB);
break;
case 5:
Kart1Y_MSB = data;
//printf("fifth byte: %u\r\n", Kart1Y_MSB);
break;
case 6:
Kart1Y_LSB = data;
//printf("sixth byte: %u\r\n", Kart1Y_LSB);
break;
case 7:
Kart1Orientation_MSB = data;
//printf("seventh byte: %u\r\n", Kart1Orientation_MSB);
break;
case 8:
Kart1Orientation_LSB);
Kart1Orientation_LSB = data;
//printf("eighth byte: %u\r\n",
break;
default: //first two bytes are insignificant
break;
}
}
Kart1X = ConcatenateNibbles(Kart1X_MSB, Kart1X_LSB);
Kart1Y = ConcatenateNibbles(Kart1Y_MSB, Kart1Y_LSB);
Kart1Orientation = ConcatenateNibbles (Kart1Orientation_MSB,
if (Kart1Orientation<0){Kart1Orientation+=360;}
}
void SendKart1DataQuery (void)
{
HWREG(SSI0_BASE + SSI_O_DR)= 0xC3; //Send a Query Byte to the Command
Generator
interrupt
}
/*****************************************************************
Return Functions
*******************************************************************/
uint16_t GetKart1X (void)
{
return Kart1X;
}
uint16_t GetKart1Y (void)
{
return Kart1Y;
}
int16_t GetKart1Orientation (void)
{
return Kart1Orientation;
}
/****************************************************************************
Module
Kart2DataSM.c

****************************************************************************/
/*----------------------------- Include Files -----------------------------*/
*/
#include "SPIDRS.h"
#include <stdint.h>
/*----------------------------- Module Defines ----------------------------*/
*/
static
static
static
static
static
uint16_t Kart2Y_LSB;
uint16_t Kart2Y;
uint16_t Kart2Orientation_MSB;
uint16_t Kart2Orientation_LSB;
int16_t Kart2Orientation;
/*------------------------------ Module Code ------------------------------*/

/****************************************************************************
Function
RunKart2DataSM
****************************************************************************/
{
state
{
{
{
NextState =
ReadingKart2Data;
MakeTransition =
true;
break;
break;
}
}
break;
{
{
Terminal State
break;
break;
}
//
{
//

//
}
}
/****************************************************************************
Function
StartKart2DataSM
****************************************************************************/
{
// is started
{
}
}
/****************************************************************************
Function
QueryKart2DataSM
****************************************************************************/
{
}
/***************************************************************************
private functions
***************************************************************************/
{
currentevent
{
for (int i = 0; i < 7; i++){
SendPrimingByte();
}
}
{
}else
{

}
return(NewEvent);

{
{
ReadKart2Data();

}
{
}else
{
}
return(NewEvent);

{
for (int i = 1; i <= 8; i++) {
switch (i) {
case 3:
Kart2X_MSB = data;
break;
case 4:
break;
case 5:
Kart2X_LSB = data;
Kart2Y_MSB = data;
break;
case 6:
break;
case 7:
Kart2Y_LSB = data;
break;
case 8:
break;

break;
}
}
}
{
HWREG(SSI0_BASE + SSI_O_DR)= 0x5A; //Send a Query Byte to the Command
Generator
interrupt
}
/*****************************************************************
Return Functions
*******************************************************************/
{
return Kart2X;
}
{
return Kart2Y;
}
{
}
/****************************************************************************
Module
Kart3DataSM.c
****************************************************************************/
/*----------------------------- Include Files -----------------------------*/
*/
#include "SPIDRS.h"
#include <stdint.h>
/*----------------------------- Module Defines ----------------------------*/

*/
static uint16_t Kart3Y_LSB;
static uint16_t Kart3Y;
static uint16_t Kart3Orientation_MSB;
static uint16_t Kart3Orientation_LSB;
static int16_t Kart3Orientation;
/*------------------------------ Module Code ------------------------------*/
/****************************************************************************
Function
RunKart3DataSM
****************************************************************************/
{
state
{
{
{
NextState =
ReadingKart3Data;
MakeTransition =
true;
break;
break;
}
}
break;
{
{
Terminal State
break;
break;
}
//
{
//

//
}
}
/****************************************************************************
Function
StartKart3DataSM
****************************************************************************/
{
// is started
{
}
}
/****************************************************************************
Function
QueryKart3DataSM
****************************************************************************/
{
}
/***************************************************************************
private functions
***************************************************************************/
{
currentevent
{
for (int i = 0; i < 7; i++) {
SendPrimingByte();
}
}
{
}else
{

}
return(NewEvent);

{

{
ReadKart3Data();

}
{
}else
{
}
return(NewEvent);

{
for (int i = 1; i <= 8; i++) {
switch (i) {
case 3:
Kart3X_MSB = data;
break;
case 4:
break;
case 5:
Kart3X_LSB = data;
Kart3Y_MSB = data;
break;
case 6:
break;
case 7:
Kart3Y_LSB = data;
break;
case 8:
break;

break;
}
}
}
{
HWREG(SSI0_BASE + SSI_O_DR)= 0x7E; //Send a Query Byte to the Command
Generator
interrupt
}
/*****************************************************************
Return Functions
*******************************************************************/
{
return Kart3X;
}
{
return Kart3Y;
}
{
}
/****************************************************************************
Module
MapKeys.c
Revision
1.0.1
Description
This service maps keystrokes to events
Notes
History
When
-------------02/06/14 14:44
02/07/12 00:00
Who
--jec
jec
What/Why
-------tweaked to be a more generic key-mapper
converted to service for use with E&S Gen2
02/20/07 21:37 jec

converted to use enumerated type for events
02/21/05 15:38 jec
Began coding
****************************************************************************/
/*----------------------------- Include Files -----------------------------*/
#include <stdio.h>
#include <ctype.h>
#include "MapKeys.h"
#include "TopHSMTemplate.h"
#include "FindingWireSM.h"
#include "RobotingSM.h"
#include "PWMGenerator.h"
/*----------------------------- Module Defines ----------------------------*/
*/
/*------------------------------ Module Code ------------------------------*/
/****************************************************************************
Function
InitMapKeys
Parameters
Returns
bool, false if error in initialization, true otherwise
Description
Saves away the priority, and does any
other required initialization for this service
Notes
Author
****************************************************************************/
bool InitMapKeys ( uint8_t Priority )
{
//SetServoADuty(0);
puts("Map keys initialized\r\n");
return true;
}
/****************************************************************************
Function
PostMapKeys
Parameters
EF_Event ThisEvent ,the event to post to the queue
Returns
bool false if the Enqueue operation failed, true otherwise
Descriptionp0p
Notes
Author
****************************************************************************/
bool PostMapKeys( ES_Event ThisEvent )
{
}
/****************************************************************************
Function
RunMapKeys
Parameters
ES_Event : the event to process
Returns
ES_Event, ES_NO_EVENT if no error ES_ERROR otherwise
Description
maps keys to Events for HierMuWave Example
Notes
Author
****************************************************************************/
ES_Event RunMapKeys( ES_Event ThisEvent )
{
// printf("In the map keys function \r\n");
if ( ThisEvent.EventType == ES_NEW_KEY) // there was a key pressed
{
switch ( toupper(ThisEvent.EventParam))
{
// this sample is just a dummy so it posts a ES_NO_EVENT
case 'O' : ThisEvent.EventType = ES_NO_EVENT;
break;
// this variable posts the event ES
ENTRY.
case 'Q' : ThisEvent.EventType =
ES_ENTRY;
break;
EXIT.
ES_EXIT;
// this variable posts the event ES

case 'W' : ThisEvent.EventType =
break;
oriented.
EV_GAME_START;
// this variable posts the event tape

case 'E' : ThisEvent.EventType =
break;
case 'P' : ThisEvent.EventType =
EV_GAME_END;
break;
EV_TAPE_ORIENT;
case 'R' : ThisEvent.EventType =
break;
case 'T' : ThisEvent.EventType =
EV_WIRE_SENSED;
break;
EV_IN_SHOOTING_ZONE;
case 'A' : ThisEvent.EventType =
break;
case 'S' : ThisEvent.EventType =
EV_SHOT_ATTEMPTED;
break;
EV_SHOT_MADE;
case 'D' : ThisEvent.EventType =
break;
case 'F' : ThisEvent.EventType =
EV_IN_OBSTACLE_ZONE;
break;
EV_OBSTACLE_COMPLETED;
case 'G' : ThisEvent.EventType =
break;
case 'Z' : ThisEvent.EventType =
EV_BOTH_COMPLETED;
break;
case 'X' : ThisEvent.EventType =
EV_IR_HIGH;
break;
ThisEvent.EventType = ES_TIMEOUT;
case 'C' :
ThisEvent.EventParam = END_MANEUVER_TIMER;
break;
case 'V' :
ThisEvent.EventParam = END_SHOOTER_DC_TIMER;
break;
case 'B' :
ThisEvent.EventParam = END_SHOOTER_SERVO_TIMER;
break;
case 'L' :
ThisEvent.EventType = EV_ALIGN_COMPLETED;
break;
case 'K' :
ThisEvent.EventType = EV_POINT_REACHED;
StopDrivePeriodicTimer();
break;
case 'I' :
ThisEvent.EventType = EV_IR_HIGH;
break;
}
PostMasterSM(ThisEvent);
}
}
return ReturnEvent;
/****************************************************************************
Module
MasterSM.c
Revision
2.0.1
Description
This is our 218 B MasterSM
Notes
History
When
Who
What/Why
-------------- ---------2/15/15
DT
Started coding
****************************************************************************/
/*----------------------------- Include Files -----------------------------*/
*/
#include "TopHSMTemplate.h"
//include header files for lower level state machines to be called SPIDRS and
RobotingSM
#include "SPIDRS.h"
#include "IRSensingModule.h"
/*----------------------------- Module Defines ----------------------------*/
static ES_Event DuringMaster( ES_Event Event);
// 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 MasterState_t CurrentState;
// with the introduction of Gen2, we need a module level Priority var as well
/*------------------------------ Module Code ------------------------------*/
/****************************************************************************
Function
InitMasterSM
Parameters
Returns
boolean, False if error in initialization, True otherwise

Description
Saves away the priority, and starts
the top level state machine
Notes
Author
Team 19 2/15/15
****************************************************************************/
bool InitMasterSM ( uint8_t Priority )
{
puts("Master Init started\r\n");
ES_Event ThisEvent;
// save our priority
ThisEvent.EventType = ES_ENTRY;
_HW_Timer_Init(ES_Timer_RATE_1mS);
/*
Initialize all the things. ALL THE THINGS!!!!!!!!!!!!
*/
InitializeSPI(); //initialize the TIVA for SPI transfer
InitAlignPeriodicTimer(); //initialize the align periodic timer
InitDrivePeriodicTimer(); //initialize the drive periodic timer
InitCaptureTimer(); //initialize the period capture timer for the IR
beam
GetKartNumFromToggleSwitch();
InitializeGameStatusLED();
ReloadServo(); //make sure the ball shooter is cocked and loaded
SetFlywheelDuty(0); //make sure the flywheel isn't spinning

ES_Timer_InitTimer(SPIDRS_TIMER, 5); // kick off the DRS timer
// Start the Master State machine
puts("Master Init ended\r\n");
StartMasterSM( ThisEvent );
return true;
}
/****************************************************************************
Function
PostMasterSM
Parameters
ES_Event ThisEvent , the event to post to the queue
Returns
boolean False if the post operation failed, True otherwise
Description
Notes
Author
Team 19 2/15/15
****************************************************************************/
bool PostMasterSM( ES_Event ThisEvent )
{
}
/****************************************************************************
Function
RunMasterSM
Parameters
Returns
Description
the run function for the top level state machine
Notes
Author
Team 19 2/15/15
****************************************************************************/
ES_Event RunMasterSM( ES_Event CurrentEvent )
{
//
//
MasterState_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
CurrentEvent = DuringMaster(CurrentEvent); //Call our During Master
Function which will check Roboting and SPIDRS
}
/****************************************************************************
Function
StartMasterSM
Parameters
ES_Event CurrentEvent
Returns
nothing
Description
Notes
Author
Team 19 2/15/15
****************************************************************************/
void StartMasterSM ( ES_Event CurrentEvent )
{
state machines
unused var
// now we need to let the Run function init the lower level
// use LocalEvent to keep the compiler from complaining about
RunMasterSM(CurrentEvent);
return;
}
/***************************************************************************
private functions
***************************************************************************/
static ES_Event DuringMaster( ES_Event Event)
{
ES_Event ReturnEvent = Event; // assme no re-mapping or
comsumption
//
If the Event.EventType = ES_ENTRY
{
//StartSPIDRS with ES_ENTRY
StartSPIDRSSM(Event);
//Start Roboting with ES_ENTRY
StartRobotingSM(Event);
}
//
//Else If the Event.EventType = ES_EXIT (give the lower level
functions an opportunity to clean up)
{
//Run SPIDRS with ES_EXIT event
RunSPIDRSSM(Event);
//RunRoboting with ES_EXIT event
RunRobotingSM(Event);
//
//
//
}
//Else (do the during function)
else
{
//Run the Lower Level State Machine SPIDRS
RunSPIDRSSM(Event);
//Run the Lower Level State Machine Roboting
RunRobotingSM(Event);
}
//
//
// return either Event, if you don't want to allow the lower
level machine
//
// to remap the current event, or ReturnEvent if you do want to allow
it.
}
#include <stdint.h>
#include
#include
#include
#include
#include
#include
#include
#include
#include
#include
#include
#include
#include
#include
#include
<stdint.h>
<stdbool.h>
<stdio.h>
"driverlib/sysctl.h"
"driverlib/interrupt.h"
"driverlib/gpio.h"
"utils/uartstdio.h"
"inc/hw_types.h"
"inc/hw_gpio.h"
"inc/hw_sysctl.h"
"inc/hw_memmap.h"
"ES_Configure.h"
"ES_Framework.h"
"ES_Port.h"
"termio.h"
#include "ADMultiService.h"
// set Proportional and Integral Gains
#define pGain 1
#define OUTER_WEIGHT 0
/*
INDUCTIVE SENSING VALUES
*/
// 40,000 ticks per mS assumes a 40Mhz clock
static
static
static
static
static
int Control = 0;
float LeftDuty = 0;
float RightDuty = 0;
float BASE_VALUE = 15;
uint32_t* data;
static
static
static
static
float
float
float
float
OR_Range;
IR_Range;
OL_Range;
IL_Range;
// we will use Timer B in Wide Timer 0 to generate the interrupt

void InitPeriodicIntPID( void ){

// start by enabling the clock to the timer (Wide Timer 0)

// make sure that timer (Timer B) is disabled before configuring
HWREG(WTIMER0_BASE+TIMER_O_CTL) &= ~TIMER_CTL_TBEN;
// the consTBnt name derives from the 16/32 bit timer, but this is a 32/64
// 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;
HWREG(WTIMER0_BASE+TIMER_O_TBILR) = TicksPerMS * 30;
HWREG(WTIMER0_BASE+TIMER_O_IMR) |= TIMER_IMR_TBTOIM;
// enable the Timer B in Wide Timer 0 interrupt in the NVIC
// Set priority to one below the highest priority (0 is highest)
HWREG(NVIC_PRI23) |= 1<<29;
__enable_irq();
//HWREG(WTIMER0_BASE+TIMER_O_CTL) |= (TIMER_CTL_TBEN |
TIMER_CTL_TBSTALL);
}
void StartWireSensingISR (void) {
HWREG(WTIMER0_BASE+TIMER_O_CTL) |= (TIMER_CTL_TBEN | TIMER_CTL_TBSTALL);
}
void StopWireSensingISR (void) {
//stop the ISR
HWREG(WTIMER0_BASE+TIMER_O_CTL) &= ~(TIMER_CTL_TBEN | TIMER_CTL_TBSTALL);
}
void PeriodicIntPIDResponse( void ){
// start by clearing the source of the interrupt
HWREG(WTIMER0_BASE+TIMER_O_ICR) = TIMER_ICR_TBTOCINT;
/*Don't delete these
//static float OR_Range = 1260;
//static float IR_Range = 1220;
//static float IL_Range = 1825;

//static float OL_Range = 1910;
//static float OR_Max = 2500;
//static float OR_Min = 1240;
//static float IR_Max = 2600;
//static float IR_Min = 1380;
//static float IL_Max = 3200;
//static float IL_Min = 1375;
//static float OL_Max = 3450;
//static float OL_Min = 1540;
*/
static float OR_Max = 2000;
static float OR_Min = 1240;
static float IR_Max = 2000;
static float IR_Min = 1380;
static float IL_Max = 2000;
static float IL_Min = 1375;
static float OL_Max = 2000;
static float OL_Min = 1540;
OR_Range
IR_Range
OL_Range
IL_Range
=
=
=
=
OR_Max
IR_Max
OL_Max
IL_Max
OR_Min;
IR_Min;
OL_Min;
IL_Min;
data = getADCvalues();
int
int
int
int
OuterRight = 0+data[0]; //PE0

InnerRight = 0+data[1]; //PE1
InnerLeft = 0+data[2]; //PE2
OuterLeft = 0+data[3]; //PE3
//
if (((float)OuterRight > OR_Max) && ((float)OuterRight
= (float) OuterRight;
//
if (((float)OuterLeft > OL_Max) && ((float)OuterLeft <
(float) OuterLeft;
//
if (((float)InnerRight > IR_Max) && ((float)InnerRight
= (float)InnerRight;
//
if (((float)InnerLeft > IL_Max) && ((float)InnerLeft <
(float)InnerLeft;
//
//
if (((float)OuterRight < OR_Min) && ((float)OuterRight
= (float) OuterRight;
//
if (((float)OuterLeft < OL_Min) && ((float)OuterLeft >
(float) OuterLeft;
//
if (((float)InnerRight < IR_Min) && ((float)InnerRight
= (float)InnerRight;
//
if (((float)InnerLeft < IL_Min) && ((float)InnerLeft >
(float)InnerLeft;
< 2800)) OR_Max

3700)) OL_Max =
< 2800)) IR_Max
3400)) IL_Max =
> 1000)) OR_Min
1300)) OL_Min =
> 1100)) IR_Min
1200)) IL_Min =
if (((float)OuterRight > OR_Max)) OR_Max = (float) OuterRight;

if (((float)OuterLeft > OL_Max) ) OL_Max = (float) OuterLeft;
if (((float)InnerRight > IR_Max) ) IR_Max = (float)InnerRight;
if (((float)InnerLeft > IL_Max) ) IL_Max = (float)InnerLeft;
if (((float)OuterRight < OR_Min) ) OR_Min = (float) OuterRight;
if (((float)OuterLeft < OL_Min) ) OL_Min = (float) OuterLeft;

if (((float)InnerRight < IR_Min) ) IR_Min = (float)InnerRight;
if (((float)InnerLeft < IL_Min) ) IL_Min = (float)InnerLeft;
//Control = ((OuterRight + InnerRight) - (InnerLeft + OuterLeft)); //removed
the Outer Right and Outer Left sensors, just sensing in the two middle sensors
Control = pGain*((InnerRight) - (InnerLeft));
Control = (100*Control)/300;
float
float
float
float
OR_Percentage
IR_Percentage
OL_Percentage
IL_Percentage
=
=
=
=
((float)
((float)
((float)
((float)
OuterRight - OR_Min)/OR_Range;
InnerRight - IR_Min)/IR_Range;
OuterLeft - OL_Min)/OL_Range;
InnerLeft - IL_Min)/IL_Range;
//printf("OR, IR, OL, IL: %f, %f, %f, %f \r\n", OR_Percentage,

IR_Percentage, OL_Percentage, IL_Percentage);
float MaxDuty = 70;
float outerWeight = .6;
RightDuty = BASE_VALUE + (MaxDuty*outerWeight)*( 1 - OR_Percentage ) +
(MaxDuty*(1-outerWeight))* ( 1 - IR_Percentage);
LeftDuty = BASE_VALUE + (MaxDuty*outerWeight)*( 1 - OL_Percentage ) +
(MaxDuty*(1-outerWeight))* ( 1 - IL_Percentage);
//RightDuty = (70)*(1- OR_Percentage) + 20*(1-IR_Percentage);
//LeftDuty = (70)*(1- OL_Percentage) + 20*(1-IL_Percentage);
RightDuty = RightDuty - LeftDuty/2;
LeftDuty = LeftDuty - RightDuty/2;
//printf("LeftDuty, RightDuty: %u, %u \r\n", (uint8_t)LeftDuty,
(uint8_t) RightDuty);
//printf("%i
%i
%i
%i | Control = %i\n\r", OuterLeft,
InnerLeft, InnerRight, OuterRight, Control);
}
/*
* If returned value is negative, Turn Left
* If returned value is positive, Turn Right
*/
int GetControlPID( void ){
return Control;
}
uint8_t GetControlRightDuty (void) {
return (uint8_t) RightDuty;
}
uint8_t GetControlLeftDuty (void) {
return (uint8_t) LeftDuty;
}
/*
* FOR DEBUGGING
* Returns all four ADC channels read
*/
uint32_t* getADCData( void ){
return data;
}
/****************************************************************************
Module
PlayingGameSM.c
Revision
2.0.2
Description
This is a state machine for our Playing Game SM
Notes
History
When
Who
-------------- --02/15/15 01:39 DT
2/19/2015
respond to ES_ENTRY and
What/Why
-------started coding
DT
EV_ORIENTED,
-Activated code to
currently will leave in Following Wire state

****************************************************************************/
/*----------------------------- Include Files -----------------------------*/
*/
//include the .h file for the current module and any other necessary lower
level modules
#include "FindingWireFromStartSM.h"
#include "FindingWireFromObstacleEnd.h"
#include "FindingWireFromShootingSM.h"
#include "ObstacleCrossingSM.h"
#include "ShootingSM.h"
/*----------------------------- Module Defines ----------------------------*/
#define ENTRY_STATE FINDING_WIRE_FROM_START
ES_Event RunPlayingGameSM( ES_Event CurrentEvent );
void StartPlayingGameSM ( ES_Event CurrentEvent );
static
static
static
static
static
static
ES_Event
ES_Event
ES_Event
ES_Event
ES_Event
ES_Event
DuringFindingWireFromStart( ES_Event Event);

DuringFindingWireFromObstacleEnd( ES_Event Event);
DuringFindingWireFromShooting( ES_Event Event);
DuringFollowingWire( ES_Event Event);
DuringShooting( ES_Event Event);
DuringObstacleCrossing( ES_Event Event);

// away without it
static PlayingGameState_t CurrentState;
static PlayingGameState_t PrevState;
/*------------------------------ Module Code ------------------------------*/
/****************************************************************************
Function
RunPlayingGameSM
****************************************************************************/
ES_Event RunPlayingGameSM( ES_Event CurrentEvent )
{
PlayingGameState_t NextState = CurrentState;
//ES_Event ReturnEvent = { ES_NO_EVENT, 0 }; // assume no error
//Switch between CurrentStates
{
//Do this for Case Finding Wire
case FINDING_WIRE_FROM_START :
// Execute During function for DuringFindingWire. ES_ENTRY & ES_EXIT
are
// processed here allow the lowere level state machines to re-map

//printf("About to go into DUring Finding
Wire.\n\r");
CurrentEvent =
DuringFindingWireFromStart(CurrentEvent);
{
//
printf("We get into the switch event
state for Finding Wire\n\r");
{
case EV_WIRE_SENSED : //If event is ORIENTED
puts("transition to FollowingWire\r\n");
NextState
= FOLLOWING_WIRE;//Decide what the nexqrt state will be
// for internal transitions, skip changing MakeTransition
MakeTransition = true; //mark that we are taking a
transition
// optionally, consume or re-map this event for the upper
// level state machine
//ReturnEvent.EventType = ES_NO_EVENT;
break;
transition events
default: // no other
break;
}
break;
case FOLLOWING_WIRE :
//run the during for FollowingWire
CurrentEvent =
DuringFollowingWire (CurrentEvent);
{
//
{
case EV_IN_SHOOTING_ZONE : //If event is ORIENTED
// if last
state is not shooting
if(PrevState !=
SHOOTING){
puts("transition to Shooting\r\n");
//**** LOOK HERE!!!!!!!!
// make sure to put guard
condition if already made the shot
NextState = SHOOTING;//Decide what the nexqrt state will be

MakeTransition = true; //mark that we are taking a transition
/ optionally, consume or re-map this event for the upper
/ level state machine
/ReturnEvent.EventType = ES_NO_EVENT;
/
/
/
}
break;
case
EV_IN_OBSTACLE_ZONE:
puts("transition to ObstacleCrossing\r\n");
//**** LOOK HERE!!!!!!!!
// make sure to put guard condition if
already completed the obstacle
NextState
= OBSTACLE_CROSSING;//Decide what the nexqrt state will be
transition
// ReturnEvent.EventType = ES_NO_EVENT;
break;
default: //no other
transition events
break;
}
}
break;
case SHOOTING :
// run the during for shooting
//puts("Going into Shooting During Function.
\r\n");
CurrentEvent = DuringShooting
(CurrentEvent);
if ( CurrentEvent.EventType != ES_NO_EVENT )
//If an event is active
//
event state for Finding Wire\n\r");
{
printf("We get into the switch
(CurrentEvent.EventType)
switch
{
case ES_TIMEOUT
: //If event is ORIENTED

if (QueryShootingeSM() == ROTATING_TO_WIRE && (CurrentEvent.EventParam
== END_MANEUVER_TIMER)) {
puts("Shooting state in Playing Game is Complete. Switch State to Following
Wire State in Playing Game Module. \r\n");
// Execute action function for state one : event one
NextState = FOLLOWING_WIRE;//Decide what the nexqrt state will be


cases as required for relevant events

other transition events
}
break;
// repeat
default: // no
break;
}
break;
case FINDING_WIRE_FROM_SHOOTING:
//run the during
CurrentEvent =
DuringFindingWireFromShooting(CurrentEvent);
{
//
{
NextState
transition
break;
transition events
break;
}
}
break;
case OBSTACLE_CROSSING :
//run the during 0for ObstacleCrossing
CurrentEvent =
DuringObstacleCrossing (CurrentEvent);
{
//
event state for Finding Wire\n\r");
printf("We get into the switch

switch
{
case
EV_WIRE_SENSED : //If the wire is sensed

if (QueryObstacleCrossingSM() == FORWARD_TO_WIRE) { //make sure we
don't trip the wire after flying over
puts("transition to FindingWireFromObstacleEnd\r\n");
NextState = FOLLOWING_WIRE;//Decide what the nexqrt state will be
}
break;
// repeat
cases as required for relevant events

default: // no
other transition events
break;
}
break;
//
case FINDING_WIRE_FROM_OBSTACLE_END:
//
//
//run the during
//
CurrentEvent =
DuringFindingWireFromObstacleEnd(CurrentEvent);
//
//
if ( CurrentEvent.EventType != ES_NO_EVENT ) //If an event is
active
//
{
////
//
//
{
//
//
//
NextState
//
//
transition
//
//
//
//
break;
//
//
//
transition events
//
//
//
}
//
break;
}
break;

}
//
{
//
RunPlayingGameSM(CurrentEvent);
PrevState = CurrentState;
// Execute entry function for new state
}
// 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
}
/****************************************************************************
Function
StartPlayingGameSM
Starts the PlayingGame SM
****************************************************************************/
void StartPlayingGameSM ( ES_Event CurrentEvent )
{
// is started
{
}
// now we need to let the Run function init the lower level state machines
// use LocalEvent to keep the compiler from complaining about unused var
return;
}
/****************************************************************************
Function
QueryPlayingGameSM
****************************************************************************/
PlayingGameState_t QueryPlayingGameSM ( void )

{
}
/***************************************************************************
private functions
***************************************************************************/
//During Function for Finding Wire
static ES_Event DuringFindingWireFromStart( ES_Event Event)
{
//Set a NewEvent qual to RunFindingWire(Event)
//Set NewEvent equal to RunFindingWire(Event)
//Return(NewEvent)
{
//If the event equals ES_ENTRY event //set the initial state of
findingwire
//call StartFindingWire
StartFindingWireFromStartSM(Event);
}
{
//Else If the event equals ES_EXIT event gives all the lower level
functions a chance to clean up
RunFindingWireFromStartSM(Event);
}else
{
//Else , perform the during function for this state
ReturnEvent = RunFindingWireFromStartSM(Event);
}
}
//During Function for Following Wire
static ES_Event DuringFollowingWire( ES_Event Event)
{
printf("Entered the ES_ENTRY for following wire\n\r");

// implement any entry actions required for
this state machine
StartWireSensingISR();
RunFollowingWireSM(Event);
}
{
RunFollowingWireSM(Event);
StopWireSensingISR();
}else
{
ReturnEvent = RunFollowingWireSM(Event);
}
}
//During Function for Shooting
static ES_Event DuringShooting( ES_Event Event)
{
{
StartShootingSM( Event );
}
{
RunShootingSM(Event);
}else
{
ReturnEvent = RunShootingSM(Event);

}
//During Function for Obstacle Crossing

static ES_Event DuringObstacleCrossing( ES_Event Event)
{
{
StartObstacleCrossingSM( Event );
}
{
RunObstacleCrossingSM(Event);
}else
{
ReturnEvent = RunObstacleCrossingSM(Event);

}
static ES_Event DuringFindingWireFromShooting( ES_Event Event)

{

//Return(NewEvent)
{
findingwire
StartFindingWireFromShootingSM(Event);
}
{
RunFindingWireFromShootingSM(Event);
}else
{
ReturnEvent = RunFindingWireFromShootingSM(Event);
}
}
//static ES_Event DuringFindingWireFromObstacleEnd( ES_Event Event)
//{
//
//
//
//
//

//Return(NewEvent)
//
//
//
//
{
//
findingwire
//
//
StartFindingWireFromObstacleEndSM(Event);
//
}
//
//
{
//
//
RunFindingWireFromObstacleEndSM(Event);
//
//
}else
//
//
{
//
//
//
ReturnEvent = RunFindingWireFromObstacleEndSM(Event);
//
}
//
// return either Event, if you don't want to allow the lower level
machine
//
// to remap the current event, or ReturnEvent if you do want to allow
it.
//
//}
/****************************************************************************
Module
RobotingSM.c
Revision
2.0.1
Description
This is a state machine for our Roboting State
Notes
History
When
Who
-------------- --02/15/15 01:39 DT
What/Why
-------started coding
****************************************************************************/
/*----------------------------- Include Files -----------------------------*/
*/
/*----------------------------- Module Defines ----------------------------*/
#define ENTRY_STATE WAITING
static ES_Event DuringStateOne( ES_Event Event);
bool InitRobotingSM ( uint8_t Priority );
bool PostRobotingSM( ES_Event ThisEvent );
void StartRobotingSM( ES_Event ThisEvent );
ES_Event RunRobotingSM ( ES_Event CurrentEvent );
static ES_Event DuringPlayingGame( ES_Event Event);
static ES_Event DuringWaiting( ES_Event Event);

// away without it
static RobotingSM_t CurrentState;
// with the introduction of Gen2, we need a module level Priority var as well
/*------------------------------ Module Code ------------------------------*/
/****************************************************************************
Function
InitRobotingSM
Parameters
Returns
boolean, False if error in initialization, True otherwise
Description
Saves away the priority, and starts
the top level state machine
Notes
Author
Phillip Dupree, 2/15/2015, 21:18
****************************************************************************/
bool InitRobotingSM ( uint8_t Priority )
{
ES_Event ThisEvent;
// save our priority
ThisEvent.EventType = ES_ENTRY;
// Start the Master State machine
StartRobotingSM( ThisEvent );
return true;
/****************************************************************************
Function
PostRobotingSM
Parameters
ES_Event ThisEvent , the event to post to the queue
Returns
boolean False if the post operation failed, True otherwise
Description
Notes
Author
Phillip Dupree, 2/15/2015, 21:20
****************************************************************************/
bool PostRobotingSM( ES_Event ThisEvent )
{
}
/****************************************************************************
Function
RunRobotingSM
Parameters
Returns
Description
the run function for the top level state machine
Notes
Author
****************************************************************************/
ES_Event RunRobotingSM ( ES_Event CurrentEvent )
{
RobotingSM_t NextState = CurrentState;
state
// ES_Event ReturnEvent = { ES_NO_EVENT, 0 }; // assume no error
{
case WAITING :
// puts("here waiting\r\n");
// Execute Waiting During
//ES_ENTRY & ES_EXIT are processed here allow

the lowere level state machines to re-map // or consume the event
CurrentEvent = DuringWaiting(CurrentEvent);
be

{
{
case EV_CAUTION_DOWN : //Caution Flag Goes DOWN (resume game)
NextState = PLAYING_GAME;//Decide what the next state will
transition
entry
// if transitioning to a state with history change kind of

EntryEventKind.EventType = ES_ENTRY_HISTORY;
//ReturnEvent.EventType = ES_NO_EVENT; //I DON'T THINK WE

NEED TO CONSUME HERE
break;
case EV_GAME_START :
//Let the Chaos Begn.
puts("Game
Start. Going into Playing Game. \r\n");
be
HWREG(GPIO_PORTB_BASE+(GPIO_O_DATA + ALL_BITS)) |= (GPIO_PIN_3);

NextState = PLAYING_GAME;//Decide what the next state will
transition
entry

EntryEventKind.EventType = ES_ENTRY;
//ReturnEvent.EventType = ES_NO_EVENT; //I DON'T THINK WE

NEED TO CONSUME HERE
break;
default:
break;
}
}
break;
current state is state one
case PLAYING_GAME :
// If
// puts("in playing game\r\n");

// Execute During function for
state one. ES_ENTRY & ES_EXIT are
lowere level state machines to re-map
// processed here allow the

CurrentEvent =
DuringPlayingGame(CurrentEvent);

{
switch
{
EV_CAUTION_UP : //If event is event one
case
puts("going to waiting\r\n");
NextState = WAITING;//Decide what the next state will be
// if transitioning to a state with history change kind of entry
EntryEventKind.EventType = ES_ENTRY_HISTORY; //We definitely need history
here.
break;
/
/ repeat cases as required for relevant events

EV_GAME_END : //If event is event one
puts("going to waiting\r\n");
NextState = WAITING;//Decide what the next state will be
// if transitioning to a state with history change kind of entry
case
EntryEventKind.EventType = ES_ENTRY; //No History Needed.

break;
}
break;
}
//
{
//
RunRobotingSM(CurrentEvent);
// Execute entry function for new state
RunRobotingSM(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
}
/****************************************************************************
Function
StartRobotingSM
Parameters
ES_Event CurrentEvent
Returns
nothing
Description
Notes
Author
****************************************************************************/
void StartRobotingSM ( ES_Event CurrentEvent )
{

is started
//
//
//
if
{
}
// now we need to let the Run function init the lower level state machines
// use LocalEvent to keep the compiler from complaining about unused var
RunRobotingSM(CurrentEvent);
return;
}
void InitializeGameStatusLED(void)
{
* Bit 5
|
*/
*/
HWREG(GPIO_PORTB_BASE+GPIO_O_DEN) |= (GPIO_PIN_3);
*/
HWREG(GPIO_PORTB_BASE+GPIO_O_DIR) |= (GPIO_PIN_3);
// Set initial value of Game Status LED High

HWREG(GPIO_PORTB_BASE+(GPIO_O_DATA + ALL_BITS)) &= ~(GPIO_PIN_3);
/***************************************************************************
private functions
***************************************************************************/
static ES_Event DuringWaiting( ES_Event Event)
{
ES_Event ReturnEvent = Event;
{
//ForwardHalf(); //Turn Robot Drive motors off.
// perhaps stop motors and pause the maneuver timer

StopAndLock();
StopDrivePeriodicTimer(); //make sure the
interrupts are no longer firing
StopIRPeriodCaptureISR();
}
{
// Do Nothing
//No lower level state machines to run.
//perhaps reenable the end maneuver timer
}else
{
// No lower level state machines.
StopAndLock();
//ReverseFull(); //for testing
}
}
static ES_Event DuringPlayingGame( ES_Event Event)
{
{
StartPlayingGameSM(Event);
}
{
RunPlayingGameSM(Event);
}else
{
//printf("We are Going into Run Playing Game
(within Roboting)\n\r");
ReturnEvent = RunPlayingGameSM(Event);
}

}
/****************************************************************************
Module
ShootingSM.c
Revision
1.0.0
Description
This is a template file for implementing state machines.
Notes
History
When
Who
What/Why
-------------- ---------2/23/15
DT
started coding
****************************************************************************/
/*----------------------------- Include Files -----------------------------*/
*/
#include "HSMTemplate.h"
#include "ShootingSM.h"
#include "SweepingForBeaconSM.h"
/*----------------------------- Module Defines ----------------------------*/
#define ENTRY_STATE ALIGNING_TO_SHOOTING
static float PIXELS_PER_INCH = 2.33;
//static float ZONE_WIDTH= 15; //inches
static float TRACK_SIDE_LENGTH= 51; //inches
//static float DISTANCE_FROM_TAPE_TO_SHOOTZONE_START= 7.5; //inches
*/
static ES_Event DuringAligningToShooting( ES_Event Event);
static ES_Event DuringDrivingIntoShooting( ES_Event Event);
static ES_Event DuringSweepingForBeacon( ES_Event Event);

static ES_Event DuringLaunchingBall( ES_Event Event);
static ES_Event DuringAligningOutShooting( ES_Event Event);
static ES_Event DuringDrivingOutShooting( ES_Event Event);
static ShootingSM_t CurrentState;
/*------------------------------ Module Code ------------------------------*/
/****************************************************************************
Function
RunShootingSM
Parameters
Returns
Description
Notes
Author
David Tung
****************************************************************************/
ES_Event RunShootingSM( ES_Event CurrentEvent )
{
ShootingSM_t NextState = CurrentState;
state
//Switch between Current States
/* Current States are:
TurningCC90
DrivingForward
SweepingForIR
LaunchingBall
*/
{
//If the case equals TurningCC90
case ALIGNING_TO_SHOOTING :
// If current state
is Turning CC 90
//Call the during for TurnCC90
//puts("Aligning to Shooting \r\n");
CurrentEvent = DuringAligningToShooting(CurrentEvent);
{
case EV_ALIGN_COMPLETED :
puts("DRIVING
TO SHOOTING ZONE\r\n");
NextState
= DRIVING_INTO_SHOOTING;//Decide what the next state will be
transition
entry
//EntryEventKind.EventType = ES_ENTRY_HISTORY;
break;
break;
default:
}
}
break;
case DRIVING_INTO_SHOOTING:
//Call the during for DrivingForward
CurrentEvent =
DuringDrivingIntoShooting(CurrentEvent);
if (CurrentEvent.EventType != ES_NO_EVENT)
{
{
case EV_POINT_REACHED:
puts("Point
Reached. Going to Sweep for Beacon \r\n");
//Set the
Next State equal to SweepingForBeacon
//Set
MakeTransition = true
NextState= SWEEPING_FOR_BEACON;
ReturnEvent.EventType = ES_NO_EVENT; //consume the event
break;
default:
break;
}
break;
//If the case equals Sweeping For Beacon

case SWEEPING_FOR_BEACON:
//Call the during for SweepingForBeacon
CurrentEvent =
DuringSweepingForBeacon(CurrentEvent);
{
//Case EV_IR High
case EV_IR_HIGH:
puts("IR HIGH \r\n");
//Set the next state equal to
Launching Ball
NextState = LAUNCHING_BALL;
//Set MakeTransition = true;
break;
default:
break;
}
break;
//If the case equals Launching Ball
case LAUNCHING_BALL:
//call the during for Launching Ball
CurrentEvent =
DuringLaunchingBall(CurrentEvent);
//Switch Case for Event
{
{
case ES_TIMEOUT:
if
(CurrentEvent.EventParam == END_SHOOTER_DC_TIMER)
{
FireServo();
//FIRE!
ES_Timer_InitTimer(END_SHOOTER_SERVO_TIMER, 2000); //let the servo
have 2 seconds to push the ball through
printf("Servo
turned ON\n\r");
}
if
(CurrentEvent.EventParam == END_SHOOTER_SERVO_TIMER)
ReloadServo();
NextState =
ALIGNING_OUT_SHOOTING; //transition to align out of the shooting zone
MakeTransition =
true;
printf("LAUNCHING BALL MOTOR TURNED OFF\n\r");
}
default: //no transition
events. Upper level SM handles transition into Finding Wire From Shooting
break;
}
}
//If the Event param = Launch DC Motor
//Set the next state equal to Turn CC90
//Set Make Transition = true
break;
case ALIGNING_OUT_SHOOTING:
CurrentEvent =
DuringAligningOutShooting(CurrentEvent);
{
{
case EV_ALIGN_COMPLETED:
puts("Aligned
Completed. Driving Out of Shooting \r\n");
NextState= DRIVING_OUT_SHOOTING;
break;
default:
}
break;
break;
case DRIVING_OUT_SHOOTING:
CurrentEvent =
DuringDrivingOutShooting(CurrentEvent);
{
{
case EV_WIRE_SENSED:
NextState= ROTATING_TO_WIRE;
break;
default:
break;
}
}
break;
case ROTATING_TO_WIRE:
CurrentEvent =
DuringRotatingToWire(CurrentEvent);
{
{
//terminal state
default:
break;
}
}
break;
}
//
{
//
RunShootingSM(CurrentEvent);
//
RunShootingSM(EntryEventKind);
}
}
/****************************************************************************
Function
StartShootingSM
Parameters
None
Returns
None
Description
Notes
Author
David Tung
****************************************************************************/
void StartShootingSM ( ES_Event CurrentEvent )
{
// is started
{
}
RunShootingSM(CurrentEvent);
}
/****************************************************************************
Function
QueryShootingSM
Parameters
None
Returns
Description
Notes
Author
David Tung
****************************************************************************/
ShootingSM_t QueryShootingeSM ( void )
{
}
/***************************************************************************
private functions
***************************************************************************/
static ES_Event DuringAligningToShooting( ES_Event Event)
{
{
//We want to rotate 90 to face the shot bin ;
int x_waypoint = (int)( GetXShootingPoint());
//int y_waypoint = GetY_0() +
TRACK_SIDE_LENGTH*PIXELS_PER_INCH/2;
int y_waypoint = (int) GetYShootingPoint();
Align(x_waypoint, y_waypoint);
StartAlignPeriodicTimer();
printf("Aligning To Shooting Point\n\r");
}
{
printf("Aligned to Shooting Pt \n\r");
//StopAndLock();
//RunShootingSM(Event);
}else
{
//puts("I'm in the else during for alignshooter \r\n");
//if (Event.EventType == EV_ALIGN_COMPLETED)
{printf("?? \n\r"); }
//ReturnEvent = RunShootingSM(Event);
}
}
static ES_Event DuringDrivingIntoShooting( ES_Event Event)
{

StartDrivePeriodicTimer();
printf("Driving Forward to SHooting pt \n\r");
}
{
StopAndLock();
printf("Stopped and Locked in Shooting zone
\n\r");
}else
{

}
}
static ES_Event DuringSweepingForBeacon( ES_Event Event)
{
{
puts("starting to look for beacon\r\n");
StartIRPeriodCaptureISR(); //enable the ISR
that checks for the right IR frequency
StartSweepingForBeaconSM( Event );
}
{
RunSweepingForBeaconSM(Event);
StopIRPeriodCaptureISR(); //stops the ISR that

checks for the right IR frequency
puts("found the beacon\r\n");
//
}else
{
puts("finding the beacon\r\n");
ReturnEvent = RunSweepingForBeaconSM(Event);
}
static ES_Event DuringLaunchingBall( ES_Event Event)

{
{
RotateC_Continuous();
// after that start any lower level machines that run

in this state
}
else if (Event.EventType ==ES_TIMEOUT && Event.EventParam ==
END_MANEUVER_TIMER)
{
SetFlywheelDuty(62); //should set the flywheel
to ~3V
StopAndLock();
ES_Timer_InitTimer(END_SHOOTER_DC_TIMER,
10000); //allow the flywheel 5 seconds to speed up
printf("LAUNCHING BALL MOTOR TURNED ON\n\r");
}

SetFlywheelDuty(0);
//******************************TURN DC MOTOR OFF HERE
************************
// repeat for any concurrently running state
machines
}else
{
}
}
static ES_Event DuringAligningOutShooting( ES_Event Event)
{
{
int x_waypoint = GetX_0();// +
TRACK_SIDE_LENGTH*PIXELS_PER_INCH/2 + ZONE_WIDTH*PIXELS_PER_INCH/2;
int y_waypoint = GetYShootingPoint()+5;
//GetY_0() + (TRACK_SIDE_LENGTH*PIXELS_PER_INCH)/2 + 10*PIXELS_PER_INCH;
//int y_waypoint = GetY_1() 5*PIXELS_PER_INCH;
Align(x_waypoint, y_waypoint);
StartAlignPeriodicTimer();
printf("Aligning to leave shooting zone\n\r");
}
{
StopAndLock();
printf("Aligned to leave shooting zone \n\r");
}else
{
}
}
static ES_Event DuringDrivingOutShooting( ES_Event Event)
{
{
ForwardHalf();
printf("Driving out of shooting\n\r");
}
{
//StopAndLock();
printf("Exited the shooting zone \n\r");
}else
{
}
}
static ES_Event DuringRotatingToWire( ES_Event Event)
{
{
//RotateCC90();
//RotateCC45();
RotateCC25();
//ES_Timer_InitTimer(END_MANEUVER_TIMER, 200);
printf("Driving out of shooting\n\r");
}
{
//StopAndLock();
ForwardHalf();
}else
//
//
//
//
run any lower level state machine

ReturnEvent = RunLowerLevelSM(Event);
repeat for any concurrent lower level machines
do any activity that is repeated as long as we are in this state
}
}
/****************************************************************************
Module
SPIDRS.c
****************************************************************************/
/*----------------------------- Include Files -----------------------------*/
*/
#include "SPIDRS.h"
#include "MasterMachine.h"
#include
#include
#include
#include
#include
#include
#include
#include
#include
#include
#include
#include
#include
"ES_Port.h"
<stdint.h>
"inc/hw_memmap.h"
"inc/hw_gpio.h"
"inc/hw_pwm.h"
"inc/hw_sysctl.h"
"inc/hw_types.h"
"inc/hw_pwm.h"
"driverlib/gpio.h"
"driverlib/ssi.h"
"inc/hw_nvic.h"
"inc/hw_ssi.h"
"inc/hw_timer.h"

/*----------------------------- Module Defines ----------------------------*/
#define ENTRY_STATE GameStatus

*/
ES_Event RunSPIDRSSM( ES_Event CurrentEvent );
void StartSPIDRSSM ( ES_Event CurrentEvent );
SPIDRSState_t QueryTemplateSM ( void );
static ES_Event DuringGameStatus( ES_Event Event);
static ES_Event DuringKart1Data(ES_Event Event) ;
static ES_Event DuringKart2Data(ES_Event Event) ;
static ES_Event DuringKart3Data(ES_Event Event);
uint8_t data;
volatile static SPIDRSState_t CurrentState;
/*------------------------------ Module Code ------------------------------*/
/****************************************************************************
Function
RunSPIDRSSM
****************************************************************************/
ES_Event RunSPIDRSSM( ES_Event CurrentEvent )
{
//puts("Run SPIDRS.\r\n");
volatile SPIDRSState_t NextState = CurrentState;
volatile ES_Event EntryEventKind = { ES_ENTRY, 0 };// default to normal
entry to new state
volatile ES_Event ReturnEvent = CurrentEvent; // assume we are not
consuming event
{
case GameStatus :
//puts("SPIDRS >
GAMESTATUS.\r\n");
//execute during function for Game Status

CurrentEvent = DuringGameStatus(CurrentEvent);
{
{
case ES_TIMEOUT:
if
(CurrentEvent.EventParam == SPIDRS_TIMER) {
//puts("SPIDRS >
GAMESTATUSTIMEOUT.\r\n");
NextState
= Kart1Data;
}
break;

break;
}
}
break;
case Kart1Data :
//puts("SPIDRS > KART1Data \r\n");
CurrentEvent = DuringKart1Data(CurrentEvent);
{
{
case ES_TIMEOUT:
if
//puts("SPIDRS >
KART1TIMEOUT.\r\n");
NextState
= Kart2Data;
}
}
break;
}
break;
default:
break;
case Kart2Data :
//puts("SPIDRS > KART2DATA.\r\n");
{
{
case ES_TIMEOUT:
if
//puts("SPIDRS >
NextState
= Kart3Data;
}
break;
default:
break;
}
}
break;
case Kart3Data :
//puts("SPIDRS > KART3DATA.\r\n");
{
{
case ES_TIMEOUT:
if
//puts("SPIDRS >
NextState
= GameStatus;
//puts("I
want to restart the chain\r\n");
}
break;
default:
break;
}
}
break;
}
//
{
//
RunSPIDRSSM(CurrentEvent);
//
}
}
/****************************************************************************
Function
StartSPIDRSSM
****************************************************************************/
void StartSPIDRSSM ( ES_Event CurrentEvent )
{
//puts("Start SPIDRS.\r\n");
volatile ES_Event LocalEvent = CurrentEvent;
//
//
//
//
if
{
to implement entry to a history state or directly to a substate

is started
}
/****************************************************************************
Function
QuerySPIDRSSM
****************************************************************************/
SPIDRSState_t QuerySPIDRSSM ( void )
{
}
/***************************************************************************
private functions
***************************************************************************/
static ES_Event DuringGameStatus( ES_Event Event)
{
currentevent
{
StartGameStatusSM(Event); //Start the BaseQuerySM
}
{
NewEvent = RunGameStatusSM(Event);
}else
{
NewEvent = RunGameStatusSM(Event);

}
return(NewEvent);
static ES_Event DuringKart1Data(ES_Event Event) {

{
StartKart1DataSM(Event); //Start the BaseQuerySM
}
{
NewEvent = RunKart1DataSM(Event);
}else
{
}
return(NewEvent);
}
{
}

}else
{
}
return(NewEvent);
}
{
}
{
}else
{

}
return(NewEvent);
void InterruptResponseSPI(void)
{
//printf("Our ISR is working. \r\n");

HWREG(SSI0_BASE + SSI_O_IM) &= ~SSI_IM_TXIM;
interrupt
ES_Event PostEvent;
PostEvent.EventType = EV_EOTinterrupt;
PostMasterSM(PostEvent);
}
//disables the local
uint16_t ConcatenateNibbles(uint16_t MSB, uint16_t LSB)

{
MSB = MSB << BITS_PER_NIBBLE;
return (MSB | LSB);
}
uint8_t grabData(void){
uint8_t data = HWREG(SSI0_BASE + SSI_O_DR);
return data;
}
/*
* SPI Initialization Code
* TIVA pin - SSI Function
* ----------------------* PA2 - Clock
* PA3 - SS
* PA4 - RX
* PA5 - TX
*/
void InitializeSPI(void)
{
//puts("Run SPI Initialization. \r\n");
// use volatile to avoid over-optimization, used to kill time
volatile uint32_t Dummy;
//1. Enable the clock to the GPIO port port A for module 0
HWREG(SYSCTL_RCGCGPIO) |= SYSCTL_RCGCGPIO_R0;
//2. Enable the clock to SSI module
HWREG(SYSCTL_RCGCSSI) |= SYSCTL_RCGCSSI_R0;
//3. Wait for the GPIO port to be ready, ED did PRPIO in the PWM code
while ((HWREG(SYSCTL_PRGPIO) & SYSCTL_PRGPIO_R0) != SYSCTL_PRGPIO_R0);
//4. Program the GPIO to use the alternate functions on the SSI pins
HWREG(GPIO_PORTA_BASE + GPIO_O_AFSEL) |= (BIT2HI | BIT3HI | BIT4HI |
BIT5HI);
//5. Select the SSI alternate
MUX 2 for pins A2,A3,A4,A5
HWREG(GPIO_PORTA_BASE
functions on those pins. Looking at table 23-5

+
+
+
+
+
GPIO_O_PCTL)
GPIO_O_PCTL)
GPIO_O_PCTL)
GPIO_O_PCTL)
GPIO_O_PCTL)
&=
|=
|=
|=
|=
0xFF0000FF;
2<<8;
2<<12;
2<<16;
2<<20;
//6. Program the port lines for digital I/O

HWREG(GPIO_PORTA_BASE + GPIO_O_DEN) |= (BIT2HI | BIT3HI | BIT4HI |
BIT5HI | BIT6HI);
//7. Program the required data directions on the port lines, BIT4 is master so
set as output, BIT5 is slave so set as input
HWREG(GPIO_PORTA_BASE + GPIO_O_DIR) |= (BIT2HI);
//
HWREG(GPIO_PORTA_BASE + GPIO_O_DIR) &= ~(BIT4HI);
//
HWREG(GPIO_PORTF_BASE+(GPIO_O_DATA + ALL_BITS)) |= (GPIO_PIN_6);
//8. If using SPI mode 3, program the pull-up on the clock line and the
receive line
HWREG(GPIO_PORTA_BASE + (GPIO_O_PUR)) |= (GPIO_PIN_2);
HWREG(GPIO_PORTA_BASE + (GPIO_O_PUR)) |= (GPIO_PIN_4);
//
Set SS line (attached to pin A3) to be an open drain style
output
HWREG(GPIO_PORTA_BASE + GPIO_O_ODR) |= BIT3HI;
//9. Wait for the SSI0 to be ready, mentions this in lecture
while ((HWREG(SYSCTL_PRSSI) & SYSCTL_PRSSI_R0) != SYSCTL_PRSSI_R0);
//10. Make sure the the SSI is disabled before programming mode bits,
disabling SSE bit
HWREG(SSI0_BASE + SSI_O_CR1) &= ~SSI_CR1_SSE;
//11. Select master mode (MS) & TXRIS indicating End of Transmit (EOT)
HWREG(SSI0_BASE + SSI_O_CR1) |= ( SSI_CR1_EOT );
HWREG(SSI0_BASE + SSI_O_CR1) &= ~( SSI_CR1_MS);
//12. Configure the SSI clock source to the system clock
//
HWREG(SSI0_BASE + SSI_O_CC) |= SSI_CC_CS_SYSPLL;
HWREG(SSI0_BASE + SSI_O_CC) = (HWREG(SSI0_BASE + SSI_O_CC)
~SSI_CC_CS_M ) + SSI_CC_CS_SYSPLL;
&
//13. Configure the clock pre-scaler

HWREG(SSI0_BASE + SSI_O_CPSR) = ( HWREG(SSI0_BASE + SSI_O_CPSR) &
~SSI_CPSR_CPSDVSR_M) + 160; //
//14. Configure clock rate (SCR), phase & polarity (SPH, SPO), mode (FRF)
//data size (DSS)
//
HWREG(SSI0_BASE + SSI_O_CR0) = 64 << 15; //Updated line to write 64 to
bit 15 through 8
//HWREG(SSI0_BASE + SSI_O_CR0) &= ~( SSI_CR0_SCR_M); //Set SCR to 0.
Bitrate = Sysclock/32
HWREG(SSI0_BASE + SSI_O_CR0) |= 16 << 8; // set tje SCR to 16
HWREG(SSI0_BASE + SSI_O_CR0) |=
SSI_CR0_SPH;
HWREG(SSI0_BASE + SSI_O_CR0) |=
SSI_CR0_SPO;
HWREG(SSI0_BASE + SSI_O_CR0) = (HWREG(SSI0_BASE + SSI_O_CR0) &

~SSI_CR0_FRF_M) + SSI_CR0_FRF_MOTO;
// HWREG(SSI0_BASE + SSI_O_CR0) |= (SSI_CR0_FRF_MOTO);
HWREG(SSI0_BASE + SSI_O_CR0) |= ( SSI_CR0_DSS_8);
//15. Locally enable interrupts on TXRIS
HWREG(SSI0_BASE + SSI_O_IM) |= SSI_IM_TXIM;
HWREG(NVIC_EN0) |= BIT7HI;
__enable_irq();
//Set priority really,really, high.
HWREG(NVIC_PRI1) |= (0 << 31);
//16. Make sure that the SSI is enabled for operation

HWREG(SSI0_BASE + SSI_O_CR1) |= SSI_CR1_SSE;
}
void SendPrimingByte (void)
{
//puts("sent byte\r\n");
HWREG(SSI0_BASE + SSI_O_DR)= 0x00; //Send a Priming Byte to the
Command Generator
interrupt
}
/****************************************************************************
Module
SweepingForBeaconSM.c
Revision
1.0.0
Description
This is a template file for implementing state machines.
Notes
History
When
Who
What/Why
-------------- ---------2/24/15
DT
started coding
****************************************************************************/
/*----------------------------- Include Files -----------------------------*/
*/
#include "SweepingForBeaconSM.h"
/*----------------------------- Module Defines ----------------------------*/
#define ENTRY_STATE TURNING_CC45_SWEEPING
*/
static ES_Event DuringTurningCC45Sweeping( ES_Event Event);
static ES_Event DuringTurningCC90Sweeping( ES_Event Event);
static ES_Event DuringTurningC90Sweeping( ES_Event Event);
static SweepingForBeaconSM_t CurrentState;
/*------------------------------ Module Code ------------------------------*/
/****************************************************************************
Function
RunSweepingForBeaconSM
Parameters
Returns
Description
Notes
Author
David Tung
****************************************************************************/
ES_Event RunSweepingForBeaconSM( ES_Event CurrentEvent )
{
SweepingForBeaconSM_t NextState = CurrentState;
state
//Switch between Current States
/* Current States are:
TurningCC90
DrivingForward
SweepingForIR
LaunchingBall
*/
{
//If the case equals TurningCC90
case TURNING_CC45_SWEEPING :
// If current state
is Turning CC45
//Call the during for TurnCC45
CurrentEvent = DuringTurningCC45Sweeping(CurrentEvent);
{
{
case ES_TIMEOUT : //If event is ES TIMEOUT
if
(CurrentEvent.EventParam==END_MANEUVER_TIMER)
{
//Set Next State equal to Turning Clockwise 90
NextState
= TURNING_C90_SWEEPING;//Decide what the next state will be
transition
entry
}
break;
// repeat cases as required for relevant
default:
break;
}
}
break;
case TURNING_C90_SWEEPING:
CurrentEvent =
DuringTurningC90Sweeping(CurrentEvent);
{
{
//Case ES_TIMEOUT
//if the event param = End Maneuver
Timeout
case ES_TIMEOUT:
if
{
//Set the
//Set
NextState= TURNING_CC90_SWEEPING;
}
break;
default:
break;
}
break;
//If the case equals Sweeping For Beacon
case TURNING_CC90_SWEEPING:
//Call the during for SweepingForBeacon
CurrentEvent =
DuringTurningCC90Sweeping(CurrentEvent);
{
//Case EV_IR High
case ES_TIMEOUT:
if
{
//Set the
//Set
NextState= TURNING_C90_SWEEPING;
}//Set the next
state equal to Launching Ball

break;
default:
break;
}
break;

}
//
{
//
RunSweepingForBeaconSM(CurrentEvent);
//
RunSweepingForBeaconSM(EntryEventKind);
}
}
/****************************************************************************
Function
StartSweepingForBeaconSM
Parameters
None
Returns
None
Description
Notes
Author
David Tung
****************************************************************************/
void StartSweepingForBeaconSM ( ES_Event CurrentEvent )
{
// is started
{
}
RunSweepingForBeaconSM(CurrentEvent);
}
/****************************************************************************
Function
QueryShootingSM
Parameters
None
Returns
Description
Notes
Author
David Tung
****************************************************************************/
SweepingForBeaconSM_t QuerySweepingForBeaconSM ( void )
{
}
/***************************************************************************
private functions
***************************************************************************/
static ES_Event DuringTurningCC45Sweeping( ES_Event Event)
{
{
RotateCC45();
printf("Turned 45 in Sweeping for Beacon\n\r");
}
{
//We want to stop the turning ;
StopAndLock();
printf("Stopped and Locked in Sweeping for
Beacon \n\r");
}else
{
}
}
static ES_Event DuringTurningCC90Sweeping( ES_Event Event)
{

{
RotateCC90();
printf("Rotating CC 90 in Sweeping for
Beacon\n\r");
}
{
StopAndLock();
printf("Stopped and Locked in Sweeping for
Beacon \n\r");
}else
{

}
static ES_Event DuringTurningC90Sweeping( ES_Event Event)

{
{
RotateC90();
printf("Rotate clockwise 90 in Sweeping for
Beacon \n\r");
}

StopAndLock();
printf ("Stopped and Locked in Sweeping for
Beacon \n\r");
}else
{

}

Code

Enviado por

Dados do documento

Descrição original:

Título original

Direitos autorais

Formatos disponíveis

Compartilhar este documento

Compartilhar ou incorporar documento

Opções de compartilhamento

Você considera este documento útil?

Este conteúdo é inapropriado?

Direitos autorais:

Formatos disponíveis

Code

Enviado por

Direitos autorais:

Formatos disponíveis

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

- Changed all names to Controlling_Motor as this module is only for

#define ALL_BITS (0xff<<2)

_HW_Timer_Init(ES_Timer_RATE_1mS); //initialize and start our timer

puts("ending init controlmotor\r\n");

StopAndLock(); //set both motor DCs to zero

// control is a value between 0 and 50

// If Left and Right duty are close to each other (+/- 5)

//control is a positive value

if (control >= 100){

// control is a negative value

//set both Motor Duty Cycles to zero

//Rotate Counter Clockwise by 90 degrees (allows 6 seconds to complete)

//Drive in reverse full speed

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

if((ThisEvent.EventType == ES_TIMEOUT) && (ThisEvent.EventParam ==

// control is a value between 0

OuterRight = 100+data[0]; //PE0

//scale control value

/*------------------------------- Footnotes -------------------------------*/

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

uint8_t OUR_KART = WhatsOurKartNumber();

if (QueryPlayingGameSM() == OBSTACLE_CROSSING &&

if ((angle_robot > (angle_desired-tolerance)) && (angle_robot <

// make sure interrupts are enabled globally

// Set x and y coordinates of positiong from which to shoot

set gains, requested duty

//convert the desired angle to 0 - 360 degrees

//printf("Xrobot, Yrobot, Xwaypoint, Ywaypoint: %i, %i,

else if (QueryObstacleCrossingSM() == DRIVING_TO_FIELD)

if (angle_robot > angle_desired){

else if (angle_robot < angle_desired){

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

behavior of this state machine

ES_Event RunFindingWireFromObstacleEndSM( ES_Event CurrentEvent );

/ Execute action function for state one : event one

default: //no other

// to remap the current event, or ReturnEvent if you do want to allow it.

// define constants for the states for this machine

/ for internal transitions, skip changing MakeTransition

default: //no other

to implement entry to a history state or directly to a substate

// do any activity that is repeated as long as we are in this state

static ES_Event DuringDrivingBackwards( ES_Event Event)

case ES_TIMEOUT : //If event is event one

Execute action function for state one : event one

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

/ for internal transitions, skip changing MakeTransition

ES_NO_EVENT ) //If an event is active

// do any activity that is repeated as long as we are in this state

static ES_Event DuringDrivingForward( ES_Event Event)

// process ES_ENTRY, ES_ENTRY_HISTORY & ES_EXIT events

//Else If the Event.EventType = ES_EXIT (give the lower level

//Run the Lower Level State Machine

//Run the Lower Level State Machine InExitShootingZone

/*------------------------------ Module Code ------------------------------*/

uint8_t AcquireKart1Laps (void);

/*------------------------------ Module Code ------------------------------*/

default: //do nothing

default: //do nothing.

CurrentState = NextState; //Modify state variable

for (int i = 0; i < 7; i++){

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

/------------------------------- Footnotes -------------------------------/

/----------------------------- Functions -----------------------------/

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

/------------------------------ Module Code ------------------------------/

/------------------------------ Module Code ------------------------------/

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

/------------------------------ Module Code ------------------------------/

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