You are on page 1of 40

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

Module
RaceStateMachine.c
Revision
1.0
Description
This is the RaceStateMachine for Team 9 ME218b final project. It is a substate machine of GameStateMachine.
Notes
const?
History
When
Who
What/Why
----------------------02/10/2015 16:33 Sky
Added racing motion and strategy
02/08/2015
20:50 Sky
Initial version
****************************************************************************/
#include "ES_Configure.h"
#include "ES_Framework.h"
#include "ES_DeferRecall.h"
#include
#include
#include
#include
#include
#include
#include

"inc/hw_memmap.h"
"inc/hw_types.h"
"inc/hw_gpio.h"
"inc/hw_sysctl.h"
"driverlib/sysctl.h"
"driverlib/pin_map.h" // Define PART_TM4C123GH6PM in project
"driverlib/gpio.h"

#include
#include
#include
#include
#include
#include
#include
#include
#include

"MasterMachine.h"
"GameStateMachine.h"
"RaceStateMachine.h"
"SPIStateMachine.h"
"Motion.h"
"Scan.h"
"EventCheckers.h"
"WheelRPMControl.h"
"Calculation.h"

/*******************************Module Defines**********************************
*/
#define SHOOT 0
#define OBSTACLE 1
#define MASTER 2
#define IN
0
#define OUT 1
#define SPEEDLV1 0
#define SPEEDLV2 1
#define SPEEDLV3 2
#define DelayTimeIn 100
#define DelayTimeOut 100
#define LEFT0 0
#define LEFT1 1
#define LEFT2 2

#define RIGHT0 3
#define RIGHT1 4
#define RIGHT2 5
#define LEFT
0
#define RIGHT 1
#define delayTime 100
/******************************Module Variables******************************/
static RaceState CurrentState, HistoryState;
static bool firstTime = true;
static bool DirectionRight;
static int16_t CalibrateInterval = 500;
static bool ToShoot = true;
static bool Goal = false;
static bool ToObstacle = true;
static int16_t DIRTOL = 25;
/******************************Constants*************************************/
/***************************Private Function Prototypes**********************/
static ES_Event DuringToRace(ES_Event);
static ES_Event DuringRegion1(ES_Event);
static ES_Event DuringRegion2(ES_Event);
static ES_Event DuringRegion3(ES_Event);
static ES_Event DuringRegion4(ES_Event);
static ES_Event DuringRegion5(ES_Event);
static ES_Event DuringRegion6(ES_Event);
static ES_Event DuringRegion7(ES_Event);
static ES_Event DuringRegion8(ES_Event);
static ES_Event DuringToShoot(ES_Event);
static ES_Event DuringToObstacle(ES_Event);
static ES_Event DuringToObstacleWaiting(ES_Event);
static void CheckDirection(void);
static bool ObstacleClear(void);
/******************************Public Functions******************************/
/*******************************************************
FUNCTION RunGaceSM
ARGUMENTS: ES_Event CurrentEvent
RETURN: ES_Event
DESCRIPTION: the Run function of Game state machine
*******************************************************/
ES_Event RunRaceSM(ES_Event CurrentEvent)
{
RaceState NextState;
bool MakeTransition;
ES_Event ReturnEvent;
ES_Event LocalEvent;
NextState = CurrentState;
MakeTransition = false;
ReturnEvent = CurrentEvent;
//switch currentstate
switch (CurrentState)
{
case Race_ToRace

{
LocalEvent = DuringToRace(CurrentEvent);
if (ES_NO_EVENT != LocalEvent.EventType) //if there is still an active event
{
switch (LocalEvent.EventType)
{
case ES_InRegion1

MakeTransition = true;
NextState = Race_Region1;
ReturnEvent.EventType = ES_NO_EVENT;
break;
}
case ES_InRegion2
MakeTransition = true;
NextState = Race_Region2;
ReturnEvent.EventType = ES_NO_EVENT;
break;
}
case ES_InRegion3
MakeTransition = true;
NextState = Race_Region3;
ReturnEvent.EventType = ES_NO_EVENT;
break;
}
case ES_InRegion4
MakeTransition = true;
NextState = Race_Region4;
ReturnEvent.EventType = ES_NO_EVENT;
break;
}

case ES_InRegion5

MakeTransition = true;
NextState = Race_Region5;
ReturnEvent.EventType = ES_NO_EVENT;
break;
}
case ES_InRegion6
MakeTransition = true;
NextState = Race_Region6;
ReturnEvent.EventType = ES_NO_EVENT;
break;
}
case ES_InRegion7
MakeTransition = true;
NextState = Race_Region7;
ReturnEvent.EventType = ES_NO_EVENT;
break;
}
case ES_InRegion8
MakeTransition = true;
NextState = Race_Region8;
ReturnEvent.EventType = ES_NO_EVENT;
break;
}
case ES_RightDirection

//stop rotation
Stop();
ES_Timer_InitTimer(Timer_delay, delayTime);
//moving forward
//MoveForward(SPEEDLV2);

ReturnEvent.EventType = ES_NO_EVENT;
break;
}
case ES_TIMEOUT

//if Direction Check Tim


er timeout,
//check direction and re
start Timer_CheckDirection
if (LocalEvent.EventParam == Timer_CheckDirection)
{
CheckDirection();
ES_Timer_InitTimer(Timer_CheckDirection, CalibrateInterval);
}
else if (LocalEvent.EventParam == Timer_RotateDelay)
{
NextState = Race_Region4;
MakeTransition = true;
}
//if Timer_Delay timeout, start moving forward
else if (LocalEvent.EventParam == Timer_delay)
{
MoveForward(SPEEDLV2);
}
ReturnEvent.EventType = ES_NO_EVENT;
break;
}
default
: break;
}
}
break;

}
case Race_Region1

{
//pass down
the event to any lower level state machine
LocalEvent = DuringRegion1(CurrentEvent);
if (ES_NO_EVENT != LocalEvent.EventType) //if there is still an active event
{
switch (LocalEvent.EventType)
{
case ES_InRegion2

NextState = Race_Region2;
MakeTransition = true;
ReturnEvent.EventType = ES_NO_EVENT;
break;
}
case ES_TIMEOUT

//if Direction Check Time


r timeout,
//check direction and re
start Timer_CheckDirection
if (LocalEvent.EventParam == Timer_CheckDirection)
{
CheckDirection();
ES_Timer_InitTimer(Timer_CheckDirection, CalibrateInterval);
}
//if Timer_Delay timeout, start moving forward
else if (LocalEvent.EventParam == Timer_delay)
{
MoveForward(SPEEDLV2);
}
else
{

//start rotation
Stop();
TurnLeft(SPEEDLV2);
}

ReturnEvent.EventType = ES_NO_EVENT;
break;
}
case ES_WrongDirection

Stop();
//calibrate the direction
if (LocalEvent.EventParam == LEFT0)
CalibrateForward(SPEEDLV1, LEFT);
else if (LocalEvent.EventParam == LEFT1)
CalibrateForward(SPEEDLV2, LEFT);
else if (LocalEvent.EventParam == LEFT2)
RotateLeft();
else if (LocalEvent.EventParam == RIGHT0)
CalibrateForward(SPEEDLV1, RIGHT);
else if (LocalEvent.EventParam == RIGHT1)
CalibrateForward(SPEEDLV2, RIGHT);
else
RotateRight();
ReturnEvent.EventType = ES_NO_EVENT;
break;
}
case ES_RightDirection

//stop rotation
Stop();
ES_Timer_InitTimer(Timer_delay, delayTime);

ReturnEvent.EventType = ES_NO_EVENT;
break;
}
default
: break;
}
}
break;
}
case Race_Region2

{
//pass down
the event to any lower level state machine
LocalEvent = DuringRegion2(CurrentEvent);
if (ES_NO_EVENT != LocalEvent.EventType) // if there is still an active event
{
switch (LocalEvent.EventType)
{
case ES_InRegion3

NextState = Race_Region3;
MakeTransition = true;
ReturnEvent.EventType = ES_NO_EVENT;
break;
}
case ES_RightDirection

//stop rotation
Stop();
ES_Timer_InitTimer(Timer_delay, delayTime);
ReturnEvent.EventType = ES_NO_EVENT;
break;
}

case ES_WrongDirection

Stop();
//calibrate the direction
if (LocalEvent.EventParam == LEFT0)
CalibrateForward(SPEEDLV1, LEFT);
else if (LocalEvent.EventParam == LEFT1)
CalibrateForward(SPEEDLV2, LEFT);
else if (LocalEvent.EventParam == LEFT2)
RotateLeft();
else if (LocalEvent.EventParam == RIGHT0)
CalibrateForward(SPEEDLV1, RIGHT);
else if (LocalEvent.EventParam == RIGHT1)
CalibrateForward(SPEEDLV2, RIGHT);
else
RotateRight();
ReturnEvent.EventType = ES_NO_EVENT;
break;
}
case ES_TIMEOUT

//if Direction Check Time


r timeout,
//check direction and re
start Timer_CheckDirection
if (LocalEvent.EventParam == Timer_CheckDirection)
{
CheckDirection();
ES_Timer_InitTimer(Timer_CheckDirection, CalibrateInterval);
}
else if (LocalEvent.EventParam == Timer_delay)
{
MoveForward(SPEEDLV2);

}
ReturnEvent.EventType = ES_NO_EVENT;
break;
}
default
:
break;
}
}
break;
}
case Race_Region3

{
//pass down t
he event to any lower level state machine
LocalEvent = DuringRegion3(CurrentEvent);
if (ES_NO_EVENT != LocalEvent.EventType) //if there is still an active event
{
switch (LocalEvent.EventType)
{
case ES_InRegion4

NextState = Race_Region4;
MakeTransition = true;
ReturnEvent.EventType = ES_NO_EVENT;
break;
}
case ES_TIMEOUT

//if Direction Check Time


r timeout,
//check direction and re
start Timer_CheckDirection
if (LocalEvent.EventParam == Timer_CheckDirection)
{
CheckDirection();
ES_Timer_InitTimer(Timer_CheckDirection, CalibrateInterval);

}
else if (LocalEvent.EventParam == Timer_delay)
{
MoveForward(SPEEDLV2);
}
else
{
//start rotation
Stop();
TurnLeft(SPEEDLV2);
}
ReturnEvent.EventType = ES_NO_EVENT;
break;
}
case ES_WrongDirection

Stop();
//calibrate the direction
if (LocalEvent.EventParam == LEFT0)
CalibrateForward(SPEEDLV1, LEFT);
else if (LocalEvent.EventParam == LEFT1)
CalibrateForward(SPEEDLV2, LEFT);
else if (LocalEvent.EventParam == LEFT2)
RotateLeft();
else if (LocalEvent.EventParam == RIGHT0)
CalibrateForward(SPEEDLV1, RIGHT);
else if (LocalEvent.EventParam == RIGHT1)
CalibrateForward(SPEEDLV2, RIGHT);
else
RotateRight();
ReturnEvent.EventType = ES_NO_EVENT;

break;
}
case ES_RightDirection

//stop rotation
Stop();
ES_Timer_InitTimer(Timer_delay, delayTime);
//moving forward
//MoveForward(SPEEDLV2);
ReturnEvent.EventType = ES_NO_EVENT;
break;
}
default
: break;
}
}
break;
}
case Race_Region4

{
//pass down t
he event to any lower level state machine
LocalEvent = DuringRegion4(CurrentEvent);
if (ES_NO_EVENT != LocalEvent.EventType) // if there is still an active event
{
switch (LocalEvent.EventType)
{
case ES_InRegion5
//if (!ToShoot)
//{
NextState = Race_Region5;
MakeTransition = true;
ReturnEvent.EventType = ES_NO_EVENT;

//}
break;
}
case ES_InRegion10

if (ToShoot)
{
NextState = Race_ToShoot;
MakeTransition = true;
ReturnEvent.EventType = ES_NO_EVENT;
}
break;
}
case ES_RightDirection

//stop rotation
Stop();
ES_Timer_InitTimer(Timer_delay, delayTime);
//moving forward
//MoveForward(SPEEDLV2);
ReturnEvent.EventType = ES_NO_EVENT;
break;
}
case ES_WrongDirection

Stop();
//calibrate the direction
if (LocalEvent.EventParam == LEFT0)
CalibrateForward(SPEEDLV1, LEFT);
else if (LocalEvent.EventParam == LEFT1)
CalibrateForward(SPEEDLV2, LEFT);
else if (LocalEvent.EventParam == LEFT2)
RotateLeft();

else if (LocalEvent.EventParam == RIGHT0)


CalibrateForward(SPEEDLV1, RIGHT);
else if (LocalEvent.EventParam == RIGHT1)
CalibrateForward(SPEEDLV2, RIGHT);
else
RotateRight();
ReturnEvent.EventType = ES_NO_EVENT;
break;
}
case ES_TIMEOUT

{
//if Direction Che

ck Timer timeout,
//check direction a
nd restart Timer_CheckDirection
if (LocalEvent.EventParam == Timer_CheckDirection)
{
CheckDirection();
ES_Timer_InitTimer(Timer_CheckDirection, CalibrateInterval);
}
else if (LocalEvent.EventParam == Timer_delay)
{
MoveForward(SPEEDLV2);
}
ReturnEvent.EventType = ES_NO_EVENT;
break;
}
default
:
break;
}
}
break;
}

case Race_Region5

{
//pass down
the event to any lower level state machine
LocalEvent = DuringRegion5(CurrentEvent);
if (ES_NO_EVENT != LocalEvent.EventType) //if there is still an active event
{
switch (LocalEvent.EventType)
{
case ES_InRegion6

NextState = Race_Region6;
MakeTransition = true;
ReturnEvent.EventType = ES_NO_EVENT;
break;
}
case ES_TIMEOUT

//if Direction Check Time


r timeout,
//check direction and re
start Timer_CheckDirection
if (LocalEvent.EventParam == Timer_CheckDirection)
{
CheckDirection();
ES_Timer_InitTimer(Timer_CheckDirection, CalibrateInterval);
}
else if (LocalEvent.EventParam == Timer_delay)
{
MoveForward(SPEEDLV2);
}
else
{
//start rotation
Stop();

TurnLeft(SPEEDLV2);
}
ReturnEvent.EventType = ES_NO_EVENT;
break;
}
case ES_WrongDirection

Stop();
//calibrate the direction
if (LocalEvent.EventParam == LEFT0)
CalibrateForward(SPEEDLV1, LEFT);
else if (LocalEvent.EventParam == LEFT1)
CalibrateForward(SPEEDLV2, LEFT);
else if (LocalEvent.EventParam == LEFT2)
RotateLeft();
else if (LocalEvent.EventParam == RIGHT0)
CalibrateForward(SPEEDLV1, RIGHT);
else if (LocalEvent.EventParam == RIGHT1)
CalibrateForward(SPEEDLV2, RIGHT);
else
RotateRight();
ReturnEvent.EventType = ES_NO_EVENT;
break;
}
case ES_RightDirection

//stop rotation
Stop();
ES_Timer_InitTimer(Timer_delay, delayTime);
//moving forward
//MoveForward(SPEEDLV2);
ReturnEvent.EventType = ES_NO_EVENT;

break;
}
default
: break;
}
}
break;
}
case Race_Region6

{
//pass down
the event to any lower level state machine
LocalEvent = DuringRegion6(CurrentEvent);
if (ES_NO_EVENT != LocalEvent.EventType) // if there is still an active event
{
switch (LocalEvent.EventType)
{
case ES_InRegion7

NextState = Race_Region7;
MakeTransition = true;
ReturnEvent.EventType = ES_NO_EVENT;
break;
}
case ES_RightDirection

//stop rotation
Stop();
ES_Timer_InitTimer(Timer_delay, delayTime);
//moving forward
//MoveForward(SPEEDLV2);
ReturnEvent.EventType = ES_NO_EVENT;
break;
}

case ES_WrongDirection

Stop();
//calibrate the direction
if (LocalEvent.EventParam == LEFT0)
CalibrateForward(SPEEDLV1, LEFT);
else if (LocalEvent.EventParam == LEFT1)
CalibrateForward(SPEEDLV2, LEFT);
else if (LocalEvent.EventParam == LEFT2)
RotateLeft();
else if (LocalEvent.EventParam == RIGHT0)
CalibrateForward(SPEEDLV1, RIGHT);
else if (LocalEvent.EventParam == RIGHT1)
CalibrateForward(SPEEDLV2, RIGHT);
else
RotateRight();
ReturnEvent.EventType = ES_NO_EVENT;
break;
}
case ES_TIMEOUT

//if Direction Check Tim


er timeout,
//check direction and re
start Timer_CheckDirection
if (LocalEvent.EventParam == Timer_CheckDirection)
{
CheckDirection();
ES_Timer_InitTimer(Timer_CheckDirection, CalibrateInterval);
}
else if (LocalEvent.EventParam == Timer_delay)
{
MoveForward(SPEEDLV2);

}
ReturnEvent.EventType = ES_NO_EVENT;
break;
}
case ES_InPosition

Stop();
//SetTarget(13);
//Check if there are other karts on obstacle
if (ObstacleClear())
{
NextState = Race_ToObstacle;
MakeTransition = true;
ReturnEvent.EventType = ES_NO_EVENT;
}
else
{
NextState = Race_ToObstacle;
MakeTransition = true;
//ES_Timer_InitTimer(Timer_ObstacleCheck, 500);
//ReturnEvent.EventType = ES_NO_EVENT;
}
break;
}
default
:
break;
}
}
break;
}
case Race_Region7

{
//pass down t
he event to any lower level state machine

LocalEvent = DuringRegion7(CurrentEvent);
if (ES_NO_EVENT != LocalEvent.EventType) //if there is still an active event
{
switch (LocalEvent.EventType)
{
case ES_InRegion8

NextState = Race_Region8;
MakeTransition = true;
ReturnEvent.EventType = ES_NO_EVENT;
break;
}
case ES_TIMEOUT

//if Direction Check Time


r timeout,
//check direction and re
start Timer_CheckDirection
if (LocalEvent.EventParam == Timer_CheckDirection)
{
CheckDirection();
ES_Timer_InitTimer(Timer_CheckDirection, CalibrateInterval);
}
else if (LocalEvent.EventParam == Timer_delay)
{
MoveForward(SPEEDLV2);
}
else
{
Stop();
TurnLeft(SPEEDLV2);
}
ReturnEvent.EventType = ES_NO_EVENT;

break;
}
case ES_WrongDirection

Stop();
//calibrate the direction
if (LocalEvent.EventParam == LEFT0)
CalibrateForward(SPEEDLV1, LEFT);
else if (LocalEvent.EventParam == LEFT1)
CalibrateForward(SPEEDLV2, LEFT);
else if (LocalEvent.EventParam == LEFT2)
RotateLeft();
else if (LocalEvent.EventParam == RIGHT0)
CalibrateForward(SPEEDLV1, RIGHT);
else if (LocalEvent.EventParam == RIGHT1)
CalibrateForward(SPEEDLV2, RIGHT);
else
RotateRight();
ReturnEvent.EventType = ES_NO_EVENT;
break;
}
case ES_RightDirection

//stop rotation
Stop();
ES_Timer_InitTimer(Timer_delay, delayTime);
//moving forward
//MoveForward(SPEEDLV2);
ReturnEvent.EventType = ES_NO_EVENT;
break;
}
default

: break;
}
}
break;
}
case Race_Region8

{
//pass down
the event to any lower level state machine
LocalEvent = DuringRegion8(CurrentEvent);
if (ES_NO_EVENT != LocalEvent.EventType) // if there is still an active event
{
switch (LocalEvent.EventType)
{
case ES_InRegion1

NextState = Race_Region1;
MakeTransition = true;
ReturnEvent.EventType = ES_NO_EVENT;
break;
}
case ES_TIMEOUT

//if Direction Check Tim


er timeout,
//check direction and re
start Timer_CheckDirection
if (LocalEvent.EventParam == Timer_CheckDirection)
{
CheckDirection();
ES_Timer_InitTimer(Timer_CheckDirection, CalibrateInterval);
}
else if (LocalEvent.EventParam == Timer_delay)
{
MoveForward(SPEEDLV2);

}
ReturnEvent.EventType = ES_NO_EVENT;
break;
}
case ES_RightDirection

//stop rotation
Stop();
ES_Timer_InitTimer(Timer_delay, delayTime);
//moving forward
//MoveForward(SPEEDLV2);
ReturnEvent.EventType = ES_NO_EVENT;
break;
}
case ES_WrongDirection

Stop();
//calibrate the direction
if (LocalEvent.EventParam == LEFT0)
CalibrateForward(SPEEDLV1, LEFT);
else if (LocalEvent.EventParam == LEFT1)
CalibrateForward(SPEEDLV2, LEFT);
else if (LocalEvent.EventParam == LEFT2)
RotateLeft();
else if (LocalEvent.EventParam == RIGHT0)
CalibrateForward(SPEEDLV1, RIGHT);
else if (LocalEvent.EventParam == RIGHT1)
CalibrateForward(SPEEDLV2, RIGHT);
else
RotateRight();
ReturnEvent.EventType = ES_NO_EVENT;
break;

}
default
: break;
}
}
break;
}
case Race_ToShoot

{
//pass down t
he event to any lower level state machine
LocalEvent = DuringToShoot(CurrentEvent);
if (ES_NO_EVENT != LocalEvent.EventType) // if there is still an active event
{
if (CurrentEvent.EventType == ES_InPosition)
{
//ES_Timer_StopTimer(Timer_CheckDirection);
Stop();
ES_Event ThisEvent;
ThisEvent.EventType = ES_ToShoot;
PostToMasterSM(ThisEvent);
ReturnEvent.EventType = ES_NO_EVENT;
}
}
break;
}
case Race_ToObstacle

{
//pass down t

he event to any lower level state machine


LocalEvent = DuringToObstacle(CurrentEvent);
if (ES_NO_EVENT != LocalEvent.EventType)
{
switch (LocalEvent.EventType)
{

case ES_RightDirection :

Stop();
ES_Event ThisEvent;
ThisEvent.EventType = ES_ToObstacle;
PostToMasterSM(ThisEvent);
ReturnEvent.EventType = ES_NO_EVENT;
break;
}
case ES_WrongDirection :

Stop();
//calibrate the direction
if (LocalEvent.EventParam == LEFT0)
RotateLeft();
else if (LocalEvent.EventParam == LEFT1)
RotateLeft();
else if (LocalEvent.EventParam == LEFT2)
RotateLeft();
else if (LocalEvent.EventParam == RIGHT0)
RotateRight();
else if (LocalEvent.EventParam == RIGHT1)
RotateRight();
else
RotateRight();
ReturnEvent.EventType = ES_NO_EVENT;
break;
}
case ES_TIMEOUT
if (LocalEvent.EventParam == Timer_CheckDirection)
{
CheckDirection();

ES_Timer_InitTimer(Timer_CheckDirection, CalibrateInterval);
}
ReturnEvent.EventType = ES_NO_EVENT;
break;
}
default
break;

}
}
}
case Race_ToObstacleWaiting

: {
//pass d

own the event to any lower level state machine


LocalEvent = DuringToObstacleWaiting(CurrentEvent);
if (ES_NO_EVENT != LocalEvent.EventType)
{
switch (LocalEvent.EventType)
{
case ES_TIMEOUT

if (LocalEvent.EventParam == Timer_ObstacleCheck)
{
if (ObstacleClear())
{
NextState = Race_ToObstacle;
MakeTransition = true;
ReturnEvent.EventType = ES_NO_EVENT;
}
else
{
ES_Timer_InitTimer(Timer_ObstacleCheck, 500);
ReturnEvent.EventType = ES_NO_EVENT;
}

}
break;
}
default

: break;

}
}
}
default
: break;
}
//if we are making transition
if (MakeTransition == true)
{
//execute exit function for current state
LocalEvent.EventType = ES_EXIT;
RunRaceSM(LocalEvent);
//execute entry function for next state
CurrentState = NextState;
LocalEvent.EventType = ES_ENTRY;
RunRaceSM(LocalEvent);
}
return ReturnEvent;
}
/*******************************************************
FUNCTION StartRaceSM
ARGUMENTS: ES_Event CurrentEvent
RETURN: no return
DESCRIPTION: start Race State machine
*******************************************************/
void StartRaceSM(ES_Event Event)
{
volatile ES_Event LocalEvent;
LocalEvent = Event;
if (LocalEvent.EventType == ES_ENTRY)
{
CurrentState = Race_ToRace;
HistoryState = CurrentState;
RunRaceSM(LocalEvent);
}
else if (LocalEvent.EventType == ES_ENTRY_HISTORY)
{
//enter history state
CurrentState = HistoryState;
RunRaceSM(LocalEvent);
}
}
/*******************************************************
FUNCTION QueryRaceState
ARGUMENTS: no argument

RETURN: RaceState
DESCRIPTION: query the current state
*******************************************************/
RaceState QueryRaceState(void)
{
return CurrentState;
}
/*******************************************************
FUNCTION ResetShootStatus
ARGUMENTS: no argument
RETURN: no return
DESCRIPTION: reset the flag for shoot to true
*******************************************************/
void ResetShootStatus(void)
{
ToShoot = true;
}
/*******************************************************
FUNCTION ResetFirstTime
ARGUMENTS: no argument
RETURN: no return
DESCRIPTION: reset firstTime to true
*******************************************************/
void ResetFirstTime(void)
{
firstTime = true;
}
/*******************************************************
FUNCTION SetObstacleStatus
ARGUMENTS: no argument
RETURN: no return
DESCRIPTION: set the flag for obstacle to false
*******************************************************/
void SetObstacleStatus(void)
{
ToObstacle = false;
}
/*******************************************************
FUNCTION SetObstacleStatus
ARGUMENTS: no argument
RETURN: no return
DESCRIPTION: set the flag for obstacle to false
*******************************************************/
void SetShootStatus(void)
{
ToShoot = false;
}
/*************************************Private Functions*************************
*************/
/*******************************************************
FUNCTION DuringToRace
ARGUMENTS: ES_Event CurrentEvent
RETURN: ES_Event
DESCRIPTION: during function for Race_ToRace state

*******************************************************/
static ES_Event DuringToRace(ES_Event CurrentEvent)
{
ES_Event LocalEvent;
LocalEvent = CurrentEvent;
if ((LocalEvent.EventType == ES_ENTRY)||
(LocalEvent.EventType == ES_ENTRY_HISTORY))
{
ResetFirstTime();
switch (LocalEvent.EventParam)
{
case SHOOT

printf("Shoot to Race\n\r");
//set CurrentTarget in Region 4
SetTarget(12);
RotateRight();
//Check direction
CheckDirection();
ES_Timer_InitTimer(Timer_CheckDirection, CalibrateInterval);
ToShoot = false;
break;
}
case OBSTACLE
//set CurrentTarget in Region 2
ToObstacle = false;
ES_Event ThisEvent;
ThisEvent.EventType = ES_InRegion2;
PostToMasterSM(ThisEvent);
//move forward
MoveForward(SPEEDLV2);
break;
}
case MASTER

: {

if ((LocalEvent.EventType == ES_ENTRY)||
(LocalEvent.EventType == ES_ENTRY_HISTORY))
{

: {

ToShoot = true;
ToObstacle = true;
Goal = false;
}
MoveForward(SPEEDLV2);
break;
}
default

: break;

}
}
else if (CurrentEvent.EventType == ES_EXIT)
{
//nothing to do here
HistoryState = CurrentState;
}
else
{
//nothing to do here
}
return LocalEvent;
}
/*******************************************************
FUNCTION DuringRegion1
ARGUMENTS: ES_Event CurrentEvent
RETURN: ES_Event
DESCRIPTION: during function for Race_Region1 state
*******************************************************/
static ES_Event DuringRegion1(ES_Event CurrentEvent)
{
ES_Event LocalEvent;
LocalEvent = CurrentEvent;
if ((LocalEvent.EventType == ES_ENTRY)||
(LocalEvent.EventType == ES_ENTRY_HISTORY))
{
ResetFirstTime();
SetTarget(1);
printf("Enter Region1!!!\n\r");
MoveForward(SPEEDLV2);
//Check direction
CheckDirection();
ES_Timer_InitTimer(Timer_CheckDirection, CalibrateInterval);
}
else if (CurrentEvent.EventType == ES_EXIT)
{
HistoryState = CurrentState;
}
else
{
//nothing to do here
}
return LocalEvent;
}
/*******************************************************
FUNCTION DuringRegion2

ARGUMENTS: ES_Event CurrentEvent


RETURN: ES_Event
DESCRIPTION: during function for Race_Region2 state
*******************************************************/
static ES_Event DuringRegion2(ES_Event CurrentEvent)
{
ES_Event LocalEvent;
LocalEvent = CurrentEvent;
if ((LocalEvent.EventType == ES_ENTRY)||
(LocalEvent.EventType == ES_ENTRY_HISTORY))
{
ResetFirstTime();
SetTarget(2);
printf("Enter Region2!!!\n\r");
MoveForward(SPEEDLV2);
//Check direction
CheckDirection();
ES_Timer_InitTimer(Timer_CheckDirection, CalibrateInterval);
}
else if (CurrentEvent.EventType == ES_EXIT)
{
HistoryState = CurrentState;
}
else
{
//nothing to do here
}
return LocalEvent;
}
/*******************************************************
FUNCTION DuringRegion3
ARGUMENTS: ES_Event CurrentEvent
RETURN: ES_Event
DESCRIPTION: during function for Race_Region3 state
*******************************************************/
static ES_Event DuringRegion3(ES_Event CurrentEvent)
{
ES_Event LocalEvent;
LocalEvent = CurrentEvent;
if ((LocalEvent.EventType == ES_ENTRY)||
(LocalEvent.EventType == ES_ENTRY_HISTORY))
{
//ToShoot = true;
ResetFirstTime();
SetTarget(3);
printf("Enter Region3!!!\n\r");
MoveForward(SPEEDLV2);
//Check direction
CheckDirection();
ES_Timer_InitTimer(Timer_CheckDirection, CalibrateInterval);
}
else if (CurrentEvent.EventType == ES_EXIT)
{
HistoryState = CurrentState;
}
else
{
//nothing to do here
}

return LocalEvent;
}
/*******************************************************
FUNCTION DuringRegion4
ARGUMENTS: ES_Event CurrentEvent
RETURN: ES_Event
DESCRIPTION: during function for Race_Region4 state
*******************************************************/
static ES_Event DuringRegion4(ES_Event CurrentEvent)
{
ES_Event LocalEvent;
LocalEvent = CurrentEvent;
if ((LocalEvent.EventType == ES_ENTRY)||
(LocalEvent.EventType == ES_ENTRY_HISTORY))
{
printf("Enter Region4!!!\n\r");
ResetFirstTime();
if (!ToShoot)
{
SetTarget(4);
//MoveForward(SPEEDLV2);
}
else
{
SetTarget(10);
printf("Go to Shooting Zone!!!!\n\r");
MoveForward(SPEEDLV2);
}
//Check direction
CheckDirection();
ES_Timer_InitTimer(Timer_CheckDirection, CalibrateInterval);
}
else if (CurrentEvent.EventType == ES_EXIT)
{
HistoryState = CurrentState;
}
else
{
//nothing to do here
}
return LocalEvent;
}
/*******************************************************
FUNCTION DuringRegion5
ARGUMENTS: ES_Event CurrentEvent
RETURN: ES_Event
DESCRIPTION: during function for Race_Region5 state
*******************************************************/
static ES_Event DuringRegion5(ES_Event CurrentEvent)
{
ES_Event LocalEvent;
LocalEvent = CurrentEvent;
if ((LocalEvent.EventType == ES_ENTRY)||
(LocalEvent.EventType == ES_ENTRY_HISTORY))
{
ResetFirstTime();
SetTarget(5);
printf("Enter Region5!!!\n\r");

MoveForward(SPEEDLV2);
//Check direction
CheckDirection();
ES_Timer_InitTimer(Timer_CheckDirection, CalibrateInterval);
}
else if (CurrentEvent.EventType == ES_EXIT)
{
HistoryState = CurrentState;
}
else
{
//nothing to do here
}
return LocalEvent;
}
/*******************************************************
FUNCTION DuringRegion6
ARGUMENTS: ES_Event CurrentEvent
RETURN: ES_Event
DESCRIPTION: during function for Race_Region6 state
*******************************************************/
static ES_Event DuringRegion6(ES_Event CurrentEvent)
{
ES_Event LocalEvent;
LocalEvent = CurrentEvent;
if ((LocalEvent.EventType == ES_ENTRY)||
(LocalEvent.EventType == ES_ENTRY_HISTORY))
{
//if (!Goal) ToShoot = true;
ToShoot = true;
ResetFirstTime();
if (!ToObstacle) SetTarget(6);
else SetTarget(13);
printf("Enter Region6!!!\n\r");
MoveForward(SPEEDLV2);
//Check direction
CheckDirection();
ES_Timer_InitTimer(Timer_CheckDirection, CalibrateInterval);
}
else if (CurrentEvent.EventType == ES_EXIT)
{
HistoryState = CurrentState;
}
else
{
//nothing to do here
}
return LocalEvent;
}
/*******************************************************
FUNCTION DuringRegion7
ARGUMENTS: ES_Event CurrentEvent
RETURN: ES_Event
DESCRIPTION: during function for Race_Region7 state
*******************************************************/
static ES_Event DuringRegion7(ES_Event CurrentEvent)
{
ES_Event LocalEvent;

LocalEvent = CurrentEvent;
if ((LocalEvent.EventType == ES_ENTRY)||
(LocalEvent.EventType == ES_ENTRY_HISTORY))
{
ResetFirstTime();
SetTarget(7);
printf("Enter Region7!!!\n\r");
//start moving forward
MoveForward(SPEEDLV2);
//Check direction
CheckDirection();
ES_Timer_InitTimer(Timer_CheckDirection, CalibrateInterval);
}
else if (CurrentEvent.EventType == ES_EXIT)
{
HistoryState = CurrentState;
}
else
{
//nothing to do here
}
return LocalEvent;
}
/*******************************************************
FUNCTION DuringRegion8
ARGUMENTS: ES_Event CurrentEvent
RETURN: ES_Event
DESCRIPTION: during function for Race_Region8 state
*******************************************************/
static ES_Event DuringRegion8(ES_Event CurrentEvent)
{
ES_Event LocalEvent;
LocalEvent = CurrentEvent;
if ((LocalEvent.EventType == ES_ENTRY)||
(LocalEvent.EventType == ES_ENTRY_HISTORY))
{
ResetFirstTime();
SetTarget(8);
printf("Enter Region8!!!\n\r");
//start moving forward
MoveForward(SPEEDLV2);
//Check direction
CheckDirection();
ES_Timer_InitTimer(Timer_CheckDirection, CalibrateInterval);
}
else if (CurrentEvent.EventType == ES_EXIT)
{
HistoryState = CurrentState;
}
else
{
//nothing to do here
}
return LocalEvent;
}
/*******************************************************
FUNCTION DuringToShoot
ARGUMENTS: ES_Event CurrentEvent

RETURN: ES_Event
DESCRIPTION: during function for Race_ToShoot state
*******************************************************/
static ES_Event DuringToShoot(ES_Event CurrentEvent)
{
ES_Event LocalEvent;
LocalEvent = CurrentEvent;
if ((LocalEvent.EventType == ES_ENTRY)||
(LocalEvent.EventType == ES_ENTRY_HISTORY))
{
ResetFirstTime();
SetTarget(10);
printf("Enter Region10!!!\n\r");
//start moving forward
MoveForward(SPEEDLV2);
//Check direction
CheckDirection();
ES_Timer_InitTimer(Timer_CheckDirection, CalibrateInterval);
}
else if (CurrentEvent.EventType == ES_EXIT)
{
HistoryState = CurrentState;
}
else
{
//nothing to do here
}
return LocalEvent;
}
/*******************************************************
FUNCTION DuringToObstacle
ARGUMENTS: ES_Event CurrentEvent
RETURN: ES_Event
DESCRIPTION: during function for Race_ToObstacle state
*******************************************************/
static ES_Event DuringToObstacle(ES_Event CurrentEvent)
{
ES_Event LocalEvent;
LocalEvent = CurrentEvent;
if ((LocalEvent.EventType == ES_ENTRY)||
(LocalEvent.EventType == ES_ENTRY_HISTORY))
{
ResetFirstTime();
SetTarget(14);
printf("Go to Obstacle!!!\n\r");
//start moving forward
//MoveForward(SPEEDLV1);
//Check direction
CheckDirection();
ES_Timer_InitTimer(Timer_CheckDirection, CalibrateInterval);
}
else if (CurrentEvent.EventType == ES_EXIT)
{
HistoryState = CurrentState;
}
else
{

//nothing to do here
}
return LocalEvent;
}
/*******************************************************
FUNCTION DuringToObstacleWaiting
ARGUMENTS: ES_Event CurrentEvent
RETURN: ES_Event
DESCRIPTION: during function for Race_ToObstacleWaiting state
*******************************************************/
static ES_Event DuringToObstacleWaiting(ES_Event CurrentEvent)
{
ES_Event LocalEvent;
LocalEvent = CurrentEvent;
if ((LocalEvent.EventType == ES_ENTRY)||
(LocalEvent.EventType == ES_ENTRY_HISTORY))
{
}
else if (CurrentEvent.EventType == ES_EXIT)
{
HistoryState = CurrentState;
}
else
{
//nothing to do here
}
return LocalEvent;
}
/*******************************************************
FUNCTION CheckDirection
ARGUMENTS: no argument
RETURN: no return
DESCRIPTION: check the kart orientation to see if the orientation
is correct
*******************************************************/
static void CheckDirection(void)
{
int16_t Difference;
uint8_t self = QuerySelf();
KartStatusType Kart;
//switch self number
switch (self)
{
case 1
:
Kart = QueryKart1Status(
);
break;
case 2

Kart = QueryKart2Status(

case 3

Kart = QueryKart3Status(

break;

);
break;
);
break;
default
}
int16_t TargetDirection;

//calculate TargetDirection
int16_t BaseAngle = 0;
int8_t VectorNum;
RaceState LocalState = CurrentState;
printf("CurrentState = %d\n\r", LocalState);
//switch current state to get base angle and base vector
switch (LocalState)
{
case
Race_ToRace
:
VectorNum = 0;
BaseAngle = 90;
break;
case

Race_Region1

VectorNum = 2;

case

Race_Region2

: VectorNum = 0;

case

Race_Region3

: VectorNum = 0;

case

Race_Region4

: VectorNum = 0;

case

Race_Region5

: VectorNum = 1;

Race_Region6

VectorNum = 1;

Race_Region7

VectorNum = 2;

Race_Region8

VectorNum = 2;

Race_ToShoot

VectorNum = 1;

BaseAngle = 270;
break;
BaseAngle = 90;
break;
BaseAngle = 90;
break;
BaseAngle = 90;
break;
BaseAngle = 180;
break;
case
BaseAngle = 180;
break;
case
BaseAngle = 270;
break;
case
BaseAngle = 270;
break;
case

BaseAngle = 180;
case Race_ToObstacle :VectorNum = 2;
BaseAngle = 270;

break;
default

break;
}
//query to get base vector
Vector Boundary = QueryVector(VectorNum);
Vector KartToTarget;
//get current target coordinates
Target CurrentTarget = QueryTarget();
printf("CurrentTarget: %d, %d\n\r", CurrentTarget.x, CurrentTarget.y);
printf("KartPosition: %d, %d \n\r", Kart.PositionX, Kart.PositionY);
KartToTarget.x = CurrentTarget.x-Kart.PositionX;
KartToTarget.y = CurrentTarget.y-Kart.PositionY;
printf("Vectors: %d, %d
%d, %d\n\r", KartToTarget.x, KartToTarget.y,
Boundary.x, Boundary.y);
double Angle = CalcAngle(KartToTarget, Boundary);
printf("Angle = %f\n\r", Angle);
printf("BaseAngle = %d\n\r", BaseAngle);
TargetDirection = (int16_t)(Angle+BaseAngle);
if (TargetDirection > 360) TargetDirection -= 360;
if (TargetDirection < 0)
TargetDirection += 360;
printf("TargetDirection = %d, KartOrientation = %d\n\r", TargetDirection
, Kart.Orientation);
//set tolerance for direction
if (CurrentState == Race_ToObstacle) DIRTOL = 10;
else DIRTOL = 15;
if (LocalState == Race_Region2) TargetDirection = 98;
//if the orientation is no correct
if ((abs(TargetDirection-Kart.Orientation)>DIRTOL)&&(abs(TargetDirection
-Kart.Orientation)<(360-DIRTOL)))
{
Difference = TargetDirection-Kart.Orientation;
if (firstTime)
{
DirectionRight = true;
firstTime = false;
}
//if (DirectionRight)
//{
DirectionRight = false;
ES_Event ThisEvent;
ThisEvent.EventType = ES_WrongDirection;
CalibrateInterval = 100;
if ((Difference < -180)||((Difference > 0)&&(Difference
< 180)))
{
if (Difference < -180) Difference += 360;
//SetEncoderCount(Difference);
if (Difference >= 80) ThisEvent.EventParam = L
EFT2;
else if (Difference >= 60) ThisEvent.EventParam
= LEFT1;
else ThisEvent.EventParam = LEFT0;
//printf("RotateLeft!\n\r");
}
else
{
if (Difference > 180) Difference -= 360;
Difference = abs(Difference);
//SetEncoderCount(Difference);

if (Difference >= 80)

ThisEvent.EventParam = R

IGHT2;
else if (Difference >= 50) ThisEvent.EventParam
= RIGHT1;
else ThisEvent.EventParam = RIGHT0;
//printf("RotateRight!\n\r");
}
PostToMasterSM(ThisEvent);
//}
}
//if the orientation is correct
else
{
if (firstTime)
{
DirectionRight = false;
firstTime = false;
}
if (!DirectionRight)
{
CalibrateInterval = 1000;
DirectionRight = true;
ES_Event ThisEvent;
ThisEvent.EventType = ES_RightDirection;
PostToMasterSM(ThisEvent);
}
}
}
/*******************************************************
FUNCTION ObstacleClear
ARGUMENTS: no argument
RETURN: true if obstacle is clear
DESCRIPTION: during function for Race_ToObstacleWaiting state
*******************************************************/
static bool ObstacleClear(void)
{
bool ReturnVal = true;
uint8_t self = QuerySelf();
KartStatusType Kart;
//Kart 1
if (self != 1)
{
Kart = QueryKart1Status();
if (onObstacle(Kart.PositionX, Kart.PositionY))
ReturnVal = false;
}
//Kart 2
if (self != 2)
{
Kart = QueryKart2Status();
if (onObstacle(Kart.PositionX, Kart.PositionY))
ReturnVal = false;
}
//Kart 3
if (self != 3)
{
Kart = QueryKart3Status();
if (onObstacle(Kart.PositionX, Kart.PositionY))
ReturnVal = false;

}
return ReturnVal;
}