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

Module
TrackingTracking.c
Revision
2.0.1
Description
state machine for finding target based on x,y location
*/
/*----------------------------- Include Files -----------------------------*/
// Basic includes for a program using the Events and Services Framework
#include "ES_Configure.h"
#include "ES_Framework.h"
/* include header files for this state machine as well as any machines at the
next lower level in the hierarchy that are sub-machines to this machine
*/
#include "TargetTracking.h"
#include "DriveMotorService.h"
#include "MasterMachine.h"
#include "DeadReckoning.h"
#include "SendCommand.h"
#include "Location.h"
#include "PWMDrive.h"
#include
#include
#include
#include
#include
#include
#include
#include
#include
#include
#include

"inc/hw_types.h"
"inc/hw_memmap.h"
"inc/hw_gpio.h"
"inc/hw_sysctl.h"
"driverlib/sysctl.h"
"driverlib/gpio.h"
"driverlib/interrupt.h"
"utils/uartstdio.h"
"inc/hw_timer.h"
"inc/hw_nvic.h"
<math.h>

/*----------------------------- Module Defines ----------------------------*/

// define constants for the states for this machine
// and any other local defines
#define ALL_BITS (0xff<<2)
#define TOLERANCE 2
#define Pgain 1.5
#define TicksPerMS 40000
// 40,000 ticks per mS assumes a
40Mhz clock
#define ROTATE_SPEED 30
#define XY_TOLERANCE 5
#define ERROR_TOO_BIG 90
#define FAST_SPEED 50
#define OPEN_LOOP_TIME 1000
/*---------------------------- Module Functions ---------------------------*/
/* prototypes for private functions for this machine, things like during
functions, entry & exit functions.They should be functions relevant to the
behavior of this state machine
*/
static ES_Event DuringWaitTargetTracking( ES_Event Event);
static ES_Event DuringRotateToTarget( ES_Event Event);
static ES_Event DuringDriveToTarget( ES_Event Event);
static ES_Event DuringFastOntoRamp( ES_Event Event);
void TargetTrackingControl(void);
void InitTargetTrackingControl(void);

bool TurnDirection(void);
double TargetHeading(void);
bool TargetInSight(void);
bool TargetReached(void);
void SetTarget(double x, double y);
void SetTargetFromPosition(int Position);
/*---------------------------- Module Variables ---------------------------*/
// everybody needs a state variable, you may need others as well
static TargetTrackingState_t CurrentState;
static double XTarget;
static double YTarget;
static int NominalRPM;
/*------------------------------ Module Code ------------------------------*/
/****************************************************************************
Function
RunTargetTracking
Parameters
ES_Event: the event to process
Returns
ES_Event: an event to return
****************************************************************************/
ES_Event RunTargetTracking( ES_Event CurrentEvent )
{
bool MakeTransition = false;/* are we making a state transition? */
TargetTrackingState_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 (
{
case
//
//
//

CurrentState )
WaitTargetTracking :
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 = DuringWaitTargetTracking(CurrentEvent);
//process any events
if ( CurrentEvent.EventType != ES_NO_EVENT ) //If an event is active
{
switch (CurrentEvent.EventType)
{
case EV_TARGET_SET :
// Execute action function for state one : event one
be

entry

NextState = RotateToTarget;//Decide what the next state will

// for internal transitions, skip changing MakeTransition
MakeTransition = true; //mark that we are taking a transition
// if transitioning to a state with history change kind of
EntryEventKind.EventType = ES_ENTRY;
// optionally, consume or re-map this event for the upper
// level state machine
ReturnEvent.EventType = ES_NO_EVENT;
break;
default: break;
// repeat cases as required for relevant events

}
}
break;
// repeat state pattern as required for other states
case RotateToTarget :
// 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 = DuringRotateToTarget(CurrentEvent);
//process any events
if ( CurrentEvent.EventType != ES_NO_EVENT ) //If an event is active
{
if(CurrentEvent.EventType == EV_TARGET_IN_SIGHT && QueryLocationSM()
== GoingToPosition7) { //If event is event one
// Execute action function for state one : event one
NextState = FastOntoRamp;//Decide what the next state will be
// for internal transitions, skip changing MakeTransition
MakeTransition = true; //mark that we are taking a transition
// if transitioning to a state with history change kind of
entry
EntryEventKind.EventType = ES_ENTRY;
// optionally, consume or re-map this event for the upper
// level state machine
ReturnEvent.EventType = ES_NO_EVENT;
// repeat cases as required for relevant events
} else if(CurrentEvent.EventType == EV_TARGET_IN_SIGHT) { //If event
is event one
// Execute action function for state one : event one
NextState = DriveToTarget;//Decide what the next state will be
// for internal transitions, skip changing MakeTransition
MakeTransition = true; //mark that we are taking a transition
// if transitioning to a state with history change kind of
entry
EntryEventKind.EventType = ES_ENTRY;
// optionally, consume or re-map this event for the upper
// level state machine
ReturnEvent.EventType = ES_NO_EVENT;
// repeat cases as required for relevant events
}
}
break;
one

case DriveToTarget :

// If current state is state

// 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 = DuringDriveToTarget(CurrentEvent);
//process any events
if ( CurrentEvent.EventType != ES_NO_EVENT ) //If an event is active
{
// no transitions
}
break;
case FastOntoRamp :

// 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 = DuringFastOntoRamp(CurrentEvent);

//process any events

if ( CurrentEvent.EventType != ES_NO_EVENT ) //If an event is active
{
if(CurrentEvent.EventType == ES_TIMEOUT && CurrentEvent.EventParam
== OpenLoopTimer) {
// Execute action function for state one : event one
NextState = DriveToTarget;//Decide what the next state will be
// for internal transitions, skip changing MakeTransition
MakeTransition = true; //mark that we are taking a transition
// if transitioning to a state with history change kind of
entry
EntryEventKind.EventType = ES_ENTRY;
// optionally, consume or re-map this event for the upper
// level state machine
ReturnEvent.EventType = ES_NO_EVENT;
}
}
break;
}
//
If we are making a state transition
if (MakeTransition == true)
{
//
Execute exit function for current state
CurrentEvent.EventType = ES_EXIT;
RunTargetTracking(CurrentEvent);
CurrentState = NextState; //Modify state variable
//
Execute entry function for new state
// this defaults to ES_ENTRY
RunTargetTracking(EntryEventKind);
}
return(ReturnEvent);
}
/****************************************************************************
Function
StartTargetTracking
Parameters
None
Returns
None
****************************************************************************/
void StartTargetTracking ( 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 = WaitTargetTracking;
}
// call the entry function (if any) for the ENTRY_STATE
RunTargetTracking(CurrentEvent);
}
/****************************************************************************
Function
QueryTargetTrackingState
Parameters

None
Returns
TemplateState_t The current state of the Template state machine
****************************************************************************/
TargetTrackingState_t QueryTargetTrackingState ( void )
{
return(CurrentState);
}
/***************************************************************************
private functions
***************************************************************************/
static ES_Event DuringWaitTargetTracking( 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) )
{
printf("STATE: Wait Target Tracking \r\n");
// set next position from the event param
SetTargetFromPosition(Event.EventParam);
}
else if ( Event.EventType == ES_EXIT )
{
// do nothing
}else
// do the 'during' function for this state
{
// do nothing
}
// 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.
return(ReturnEvent);
}
static ES_Event DuringRotateToTarget( 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
printf("STATE: Rotate To Target \r\n");
if(TurnDirection()) {
//Turn CW
NewRPMCommandLeft(ROTATE_SPEED);
NewRPMCommandRight(-ROTATE_SPEED);
} else {
//Turn CCW
NewRPMCommandLeft(-ROTATE_SPEED);
NewRPMCommandRight(ROTATE_SPEED);

}
}
else if ( Event.EventType == ES_EXIT )
{
// do nothing

}else
// do the 'during' function for this state
{
// do nothing
}
// 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.
return(ReturnEvent);

static ES_Event DuringDriveToTarget( 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
printf("STATE: Drive To Target \r\n");
//Enable NVIC for Target Tracking Control
HWREG(NVIC_EN3) = BIT2HI;
}
else if ( Event.EventType == ES_EXIT )
{
//Disable NVIC for Target Tracking Control
printf("disabling nvic for target tracking control\r\n");
HWREG(NVIC_DIS3) = BIT2HI;

}else
// do the 'during' function for this state
{
// do nothing
}
// 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.
return(ReturnEvent);

static ES_Event DuringFastOntoRamp( 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) )
{
printf("STATE: Fast onto ramp \r\n");
NewRPMCommandLeft(FAST_SPEED);
NewRPMCommandRight(FAST_SPEED);
ES_Timer_InitTimer(OpenLoopTimer,OPEN_LOOP_TIME);
}
else if ( Event.EventType == ES_EXIT )
{
// do nothing
}else
// do the 'during' function for this state
{
// do nothing
}
// 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.

return(ReturnEvent);
}
//-----------------------------------------------------------------------------------------//SetTargetFromPosition
//Takes 1 parameters (Position), returns nothing
void SetTargetFromPosition(int Position) {
//Set XTarget and YTarget based on DRS readings of positions
switch(Position){
case 1:
XTarget = 108;
YTarget = 33;
break;
case 2:
XTarget = 108;
YTarget = 88;
break;
case 3:
XTarget = 108;
YTarget = 136;
break;
case 4:
XTarget = 177;
YTarget = 136;
break;
case 5:
XTarget = 221;
YTarget = 136;
break;
case 6:
XTarget = 220;
YTarget = 36;
break;
case 7:
XTarget = 177;
YTarget = 36;
break;
case 8:
XTarget = 137;
YTarget = 88;
break;
}
ES_Event PostEvent = {EV_TARGET_SET,0};
PostMaster(PostEvent);
}
//End SetTargetFromPosition
//-----------------------------------------------------------------------------------------//TargetReached
//Takes no parameter, returns bool
bool TargetReached(void) {
//if we are within a tolerance of the destination, return true
if(abs(XTarget - GetX()) < XY_TOLERANCE && abs(YTarget - GetY()) <
XY_TOLERANCE)
return true;
else
return false;
}

//End TargetReached
//-----------------------------------------------------------------------------------------//TargetInSight
//Takes no parameter, returns bool
bool TargetInSight(void) {
//if we are within a tolerance in terms of orientation, return true
if(abs(TargetHeading() - GetHeading()) < TOLERANCE)
return true;
else
return false;
}
//End TargetInSight
//-----------------------------------------------------------------------------------------//TargetHeading
//Takes no parameter, returns the Target heading between -180 deg and +180 deg
double TargetHeading(void) {
//get coordinates from DRS
double X = GetKartX();
double Y = GetKartY();
//calculate delta
double Dy = YTarget - Y;
double Dx = XTarget - X;
double ReturnVal;
//handle boundary cases
if(Dx < 0) {
if(Dy > 0)
ReturnVal = (atan(Dy/Dx)*180/3.14 + 180);
else
ReturnVal = (atan(Dy/Dx)*180/3.14 - 180);
} else {
ReturnVal = (atan(Dy/Dx)*180/3.14);
}
return ReturnVal;
}
//End TargetHeading
//-----------------------------------------------------------------------------------------//TurnDirection TODO correct to rotate in right direction
//Takes no parameter, returns true if turning CW is closer to target or false if
CCW is closer to target
bool TurnDirection(void) {
double Dummy = TargetHeading() - GetHeading();
if(Dummy > 180) {
Dummy -= 360;
} else if(Dummy < -180) {
Dummy += 360;
}

if(Dummy > 0)
return true; // turn clockwise
else
return false; // turn counter clockwise

//End TurnDirection
//-----------------------------------------------------------------------------------------//Uses Wide Timer 2A
//InitTargetTrackingControl
//Takes no parameter, returns nothing
void InitTargetTrackingControl(void) {
volatile uint32_t Dummy; // use volatile to avoid over-optimization
// start by enabling the clock to the timer (Wide Timer 1)
HWREG(SYSCTL_RCGCWTIMER) |= SYSCTL_RCGCWTIMER_R2;
// kill a few cycles to let the clock get going
Dummy = HWREG(SYSCTL_RCGCGPIO);
// make sure that timer (Timer A&B) is disabled before configuring
HWREG(WTIMER2_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 32 bit mode
HWREG(WTIMER2_BASE+TIMER_O_CFG) = TIMER_CFG_16_BIT;
// set up timer A & B in periodic mode so that it repeats the time-outs
HWREG(WTIMER2_BASE+TIMER_O_TAMR) =
(HWREG(WTIMER2_BASE+TIMER_O_TAMR)& ~TIMER_TAMR_TAMR_M)|
TIMER_TAMR_TAMR_PERIOD;
// set timeout to 4mS
HWREG(WTIMER2_BASE+TIMER_O_TAILR) = TicksPerMS * 4;
// enable a local timeout interrupt
HWREG(WTIMER2_BASE+TIMER_O_IMR) |= TIMER_IMR_TATOIM;
// make sure interrupts are enabled globally
__enable_irq();
// now kick the timer off by enabling it and enabling the timer to
// stall while stopped by the debugger
HWREG(WTIMER2_BASE+TIMER_O_CTL) |= (TIMER_CTL_TAEN | TIMER_CTL_TASTALL);
printf("InitTargetTrackingControl Complete\r\n");
}
//End InitTargetTrackingControl
//-----------------------------------------------------------------------------------------//TargetTrackingControl
//Takes no paramter, returns nothing
void TargetTrackingControl(void) {
HWREG(WTIMER2_BASE+TIMER_O_ICR) = TIMER_ICR_TATOCINT;
double Error = TargetHeading() - GetHeading();
if(Error > 180)
Error -= 360;
else if(Error < -180)
Error += 360;
// if you pass the target, then just go to the next target
if(Error > ERROR_TOO_BIG || Error < -ERROR_TOO_BIG) {
ES_Event ThisEvent = {EV_TARGET_REACHED,0};
PostMaster(ThisEvent);
}

// apply P control
double Adjustment = Pgain * Error;
//set a different speed when going over the ramp
if(QueryLocationSM() == GoingToPosition7) {
NominalRPM = 30;
} else {
NominalRPM = 35;
}
//apply adjustments to wheel speed
NewRPMCommandLeft(NominalRPM + Adjustment);
NewRPMCommandRight(NominalRPM - Adjustment);
}
//End TargetTrackingControl