Racestatemachine

/****************************************************************************
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 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
break;
}
case ES_InRegion3
break;
}
case ES_InRegion4
break;
}
case ES_InRegion5
break;
}
case ES_InRegion6
break;
}
case ES_InRegion7
break;
}
case ES_InRegion8
break;
}
case ES_RightDirection
//stop rotation
Stop();
ES_Timer_InitTimer(Timer_delay, delayTime);
//moving forward
//MoveForward(SPEEDLV2);
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)
{
}
//if Timer_Delay timeout, start moving forward
else if (LocalEvent.EventParam == Timer_delay)
{
MoveForward(SPEEDLV2);
}
break;
}
default
: break;
}
}
break;
}
case Race_Region1
{
//pass down
the event to any lower level state machine
LocalEvent = DuringRegion1(CurrentEvent);
{
{
case ES_InRegion2
break;
}
case ES_TIMEOUT
//if Direction Check Time

r timeout,
{
CheckDirection();
}
//if Timer_Delay timeout, start moving forward
{
}
else
{
//start rotation
Stop();
TurnLeft(SPEEDLV2);
}
break;
}
case ES_WrongDirection
Stop();
//calibrate the direction
if (LocalEvent.EventParam == LEFT0)
CalibrateForward(SPEEDLV1, LEFT);
else if (LocalEvent.EventParam == LEFT1)
RotateLeft();
else if (LocalEvent.EventParam == RIGHT0)
CalibrateForward(SPEEDLV1, RIGHT);
else
RotateRight();
break;
}
//stop rotation
Stop();
break;
}
default
: break;
}
}
break;
}
case Race_Region2
{
//pass down
if (ES_NO_EVENT != LocalEvent.EventType) // if there is still an active event
{
{
case ES_InRegion3
break;
}
//stop rotation
Stop();
break;
}
Stop();
RotateLeft();
else
RotateRight();
break;
}
case ES_TIMEOUT

r timeout,
{
CheckDirection();
}
{
}
break;
}
default
:
break;
}
}
break;
}
case Race_Region3
{
//pass down t
he event to any lower level state machine
{
{
case ES_InRegion4
break;
}
case ES_TIMEOUT

r timeout,
{
CheckDirection();
}
{
}
else
{
//start rotation
Stop();
TurnLeft(SPEEDLV2);
}
break;
}
Stop();
RotateLeft();
else
RotateRight();
break;
}
//stop rotation
Stop();
//moving forward
break;
}
default
: break;
}
}
break;
}
case Race_Region4
{
//pass down t
{
{
case ES_InRegion5
//if (!ToShoot)
//{
//}
break;
}
case ES_InRegion10
if (ToShoot)
{
NextState = Race_ToShoot;
}
break;
}
//stop rotation
Stop();
//moving forward
break;
}
Stop();
RotateLeft();

else
RotateRight();
break;
}
case ES_TIMEOUT
{
//if Direction Che
ck Timer timeout,
//check direction a
nd restart Timer_CheckDirection
{
CheckDirection();
}
{
}
break;
}
default
:
break;
}
}
break;
}
case Race_Region5
{
//pass down
{
{
case ES_InRegion6
break;
}
case ES_TIMEOUT

r timeout,
{
CheckDirection();
}
{
}
else
{
//start rotation
Stop();
TurnLeft(SPEEDLV2);
}
break;
}
Stop();
RotateLeft();
else
RotateRight();
break;
}
//stop rotation
Stop();
//moving forward
break;
}
default
: break;
}
}
break;
}
case Race_Region6
{
//pass down
{
{
case ES_InRegion7
break;
}
//stop rotation
Stop();
//moving forward
break;
}
Stop();
RotateLeft();
else
RotateRight();
break;
}
case ES_TIMEOUT

er timeout,
{
CheckDirection();
}
{
}
break;
}
case ES_InPosition
Stop();
//SetTarget(13);
//Check if there are other karts on obstacle
if (ObstacleClear())
{
NextState = Race_ToObstacle;
}
else
{
//ES_Timer_InitTimer(Timer_ObstacleCheck, 500);
//ReturnEvent.EventType = ES_NO_EVENT;
}
break;
}
default
:
break;
}
}
break;
}
case Race_Region7
{
//pass down t
{
{
case ES_InRegion8
break;
}
case ES_TIMEOUT

r timeout,
{
CheckDirection();
}
{
}
else
{
Stop();
TurnLeft(SPEEDLV2);
}
break;
}
Stop();
RotateLeft();
else
RotateRight();
break;
}
//stop rotation
Stop();
//moving forward
break;
}
default
: break;
}
}
break;
}
case Race_Region8
{
//pass down
{
{
case ES_InRegion1
break;
}
case ES_TIMEOUT

er timeout,
{
CheckDirection();
}
{
}
break;
}
//stop rotation
Stop();
//moving forward
break;
}
Stop();
RotateLeft();
else
RotateRight();
break;
}
default
: break;
}
}
break;
}
case Race_ToShoot
{
//pass down t
LocalEvent = DuringToShoot(CurrentEvent);
{
if (CurrentEvent.EventType == ES_InPosition)
{
//ES_Timer_StopTimer(Timer_CheckDirection);
Stop();
ES_Event ThisEvent;
ThisEvent.EventType = ES_ToShoot;
PostToMasterSM(ThisEvent);
}
}
break;
}
case Race_ToObstacle
{
//pass down t

LocalEvent = DuringToObstacle(CurrentEvent);
if (ES_NO_EVENT != LocalEvent.EventType)
{
{
case ES_RightDirection :
Stop();
ES_Event ThisEvent;
ThisEvent.EventType = ES_ToObstacle;
break;
}
case ES_WrongDirection :
Stop();
RotateLeft();
RotateLeft();
RotateLeft();
RotateRight();
RotateRight();
else
RotateRight();
break;
}
case ES_TIMEOUT
{
CheckDirection();
}
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)
{
{
case ES_TIMEOUT
if (LocalEvent.EventParam == Timer_ObstacleCheck)
{
if (ObstacleClear())
{
}
else
{
ES_Timer_InitTimer(Timer_ObstacleCheck, 500);
}
}
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;
}
return ReturnEvent;
}
/*******************************************************
FUNCTION StartRaceSM
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;
}
else if (LocalEvent.EventType == ES_ENTRY_HISTORY)
{
//enter history state
CurrentState = HistoryState;
}
}
/*******************************************************
FUNCTION QueryRaceState
ARGUMENTS: no argument
RETURN: RaceState
DESCRIPTION: query the current state
*******************************************************/
RaceState QueryRaceState(void)
{
return CurrentState;
}
/*******************************************************
FUNCTION ResetShootStatus
RETURN: no return
DESCRIPTION: reset the flag for shoot to true
*******************************************************/
void ResetShootStatus(void)
{
ToShoot = true;
}
/*******************************************************
FUNCTION ResetFirstTime
RETURN: no return
DESCRIPTION: reset firstTime to true
*******************************************************/
void ResetFirstTime(void)
{
firstTime = true;
}
/*******************************************************
FUNCTION SetObstacleStatus
RETURN: no return
DESCRIPTION: set the flag for obstacle to false
*******************************************************/
void SetObstacleStatus(void)
{
ToObstacle = false;
}
/*******************************************************
FUNCTION SetObstacleStatus
RETURN: no return
DESCRIPTION: set the flag for obstacle to false
*******************************************************/
void SetShootStatus(void)
{
ToShoot = false;
}
/*************************************Private Functions*************************
*************/
/*******************************************************
FUNCTION DuringToRace
RETURN: ES_Event
DESCRIPTION: during function for Race_ToRace state
*******************************************************/
static ES_Event DuringToRace(ES_Event CurrentEvent)
{
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();
ToShoot = false;
break;
}
case OBSTACLE
//set CurrentTarget in Region 2
ToObstacle = false;
ES_Event ThisEvent;
ThisEvent.EventType = ES_InRegion2;
//move forward
break;
}
case MASTER
: {
{
: {
ToShoot = true;
ToObstacle = true;
Goal = false;
}
break;
}
default
: break;
}
}
else if (CurrentEvent.EventType == ES_EXIT)
{
//nothing to do here
}
else
{
}
return LocalEvent;
}
/*******************************************************
FUNCTION DuringRegion1
RETURN: ES_Event
DESCRIPTION: during function for Race_Region1 state
*******************************************************/
static ES_Event DuringRegion1(ES_Event CurrentEvent)
{
{
ResetFirstTime();
SetTarget(1);
printf("Enter Region1!!!\n\r");
//Check direction
CheckDirection();
}
{
}
else
{
}
return LocalEvent;
}
/*******************************************************

RETURN: ES_Event
*******************************************************/
{
{
ResetFirstTime();
SetTarget(2);
//Check direction
CheckDirection();
}
{
}
else
{
}
return LocalEvent;
}
/*******************************************************
RETURN: ES_Event
*******************************************************/
{
{
//ToShoot = true;
ResetFirstTime();
SetTarget(3);
//Check direction
CheckDirection();
}
{
}
else
{
}
return LocalEvent;
}
/*******************************************************
RETURN: ES_Event
*******************************************************/
{
{
ResetFirstTime();
if (!ToShoot)
{
SetTarget(4);
}
else
{
SetTarget(10);
printf("Go to Shooting Zone!!!!\n\r");
}
//Check direction
CheckDirection();
}
{
}
else
{
}
return LocalEvent;
}
/*******************************************************
RETURN: ES_Event
*******************************************************/
{
{
ResetFirstTime();
SetTarget(5);
//Check direction
CheckDirection();
}
{
}
else
{
}
return LocalEvent;
}
/*******************************************************
RETURN: ES_Event
*******************************************************/
{
{
//if (!Goal) ToShoot = true;
ToShoot = true;
ResetFirstTime();
if (!ToObstacle) SetTarget(6);
else SetTarget(13);
//Check direction
CheckDirection();
}
{
}
else
{
}
return LocalEvent;
}
/*******************************************************
RETURN: ES_Event
*******************************************************/
{
{
ResetFirstTime();
SetTarget(7);
//start moving forward
//Check direction
CheckDirection();
}
{
}
else
{
}
return LocalEvent;
}
/*******************************************************
RETURN: ES_Event
*******************************************************/
{
{
ResetFirstTime();
SetTarget(8);
//Check direction
CheckDirection();
}
{
}
else
{
}
return LocalEvent;
}
/*******************************************************
FUNCTION DuringToShoot
RETURN: ES_Event
DESCRIPTION: during function for Race_ToShoot state
*******************************************************/
static ES_Event DuringToShoot(ES_Event CurrentEvent)
{
{
ResetFirstTime();
SetTarget(10);
//Check direction
CheckDirection();
}
{
}
else
{
}
return LocalEvent;
}
/*******************************************************
FUNCTION DuringToObstacle
RETURN: ES_Event
DESCRIPTION: during function for Race_ToObstacle state
*******************************************************/
static ES_Event DuringToObstacle(ES_Event CurrentEvent)
{
{
ResetFirstTime();
SetTarget(14);
printf("Go to Obstacle!!!\n\r");
//Check direction
CheckDirection();
}
{
}
else
{
}
return LocalEvent;
}
/*******************************************************
FUNCTION DuringToObstacleWaiting
RETURN: ES_Event
DESCRIPTION: during function for Race_ToObstacleWaiting state
*******************************************************/
static ES_Event DuringToObstacleWaiting(ES_Event CurrentEvent)
{
{
}
{
}
else
{
}
return LocalEvent;
}
/*******************************************************
FUNCTION CheckDirection
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
case 3
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");
}
//}
}
//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;
}
}
}
/*******************************************************
FUNCTION ObstacleClear
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)
{
ReturnVal = false;
}
//Kart 3
if (self != 3)
{
ReturnVal = false;
}
return ReturnVal;
}

Racestatemachine

Diunggah oleh

Informasi Dokumen

Judul Asli

Hak Cipta

Format Tersedia

Bagikan dokumen Ini

Bagikan atau Tanam Dokumen

Opsi Berbagi

Apakah menurut Anda dokumen ini bermanfaat?

Apakah konten ini tidak pantas?

Hak Cipta:

Format Tersedia

Racestatemachine

Diunggah oleh

Hak Cipta:

Format Tersedia

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

//if Direction Check Tim

//if Direction Check Time

//if Direction Check Time

//if Direction Check Time

else if (LocalEvent.EventParam == RIGHT0)

//if Direction Check Time

//if Direction Check Tim

//if Direction Check Time

//if Direction Check Tim

he event to any lower level state machine

own the event to any lower level state machine

ARGUMENTS: ES_Event CurrentEvent

if (Difference >= 80)

Anda mungkin juga menyukai