Académique Documents
Professionnel Documents
Culture Documents
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
}
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
//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
}
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
}
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();
{
//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
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
}
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
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
}
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
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
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
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);
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;
}