MovementSM.c
/****************************************************************************
Module
MovementSM.c
Revision
2.0.1
Description
This is a template file for implementing state machines.
Notes
History
When Who What/Why
-------------- --- --------
02/13/13 02:32 mt ME218B Project
02/08/12 09:56 jec revisions for the Events and Services Framework Gen2
02/13/10 14:29 jec revised Start and run to add new kind of entry function
to make implementing history entry cleaner
02/13/10 12:29 jec added NewEvent local variable to During function and
comments about using either it or Event as the return
02/11/10 15:54 jec more revised comments, removing last comment in during
function that belongs in the run function
02/09/10 17:21 jec updated comments about internal transitions on During funtion
02/18/09 10:14 jec removed redundant call to RunLowerlevelSM in EV_Entry
processing in During function
02/20/07 21:37 jec converted to use enumerated type for events & states
02/13/05 19:38 jec added support for self-transitions, reworked
to eliminate repeated transition code
02/11/05 16:54 jec converted to implment hierarchy explicitly
02/25/03 10:32 jec converted to take a passed event parameter
02/18/99 10:19 jec built template from MasterMachine.c
02/14/99 10:34 jec Began Coding
****************************************************************************/
/*----------------------------- Include Files -----------------------------*/
// Basic includes for a program using the Events and Services Framework
#include "ES_Configure.h"
#include "ES_Framework.h"
#include "ES_ServiceHeaders.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
*/
/*----------------------------- Module Defines ----------------------------*/
// define constants for the states for this machine
// and any other local defines
#define DistScale 20
/*---------------------------- 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 DuringWaitForUncloakB( ES_Event Event);
static ES_Event DuringTurnAtTarget( ES_Event Event);
static ES_Event DuringMovingForward( ES_Event Event);
static ES_Event DuringAligning(ES_Event Event);
/*---------------------------- Module Variables ---------------------------*/
// everybody needs a state variable, you may need others as well
static MovementState_t CurrentState = End;
static coord TargetCoord;
static coord OurBot;
static int count = 0;
static int tape = 0;
static int angleInterval = 1;
static int dir = 0;
static double angle;
/*------------------------------ Module Code ------------------------------*/
/****************************************************************************
Function
RunShootTargetSM
Parameters
ES_Event: the event to process
Returns
ES_Event: an event to return
Description
add your description here
Notes
uses nested switch/case to implement the machine.
Author
Michelle T, 2/13/13
****************************************************************************/
ES_Event RunMovementSM( ES_Event CurrentEvent)
{
unsigned char MakeTransition = False;/* are we making a state transition? */
ShootingState_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
ES_Event currEvent;
static double newAngle;
double rotDiff;
static int pause = 0;
if (CurrentEvent.EventType == GameEnd)
{
NextState = End;
MakeTransition = True;
EntryEventKind.EventType = ES_ENTRY;
ReturnEvent = CurrentEvent;
}
switch(CurrentState)
{
case WaitForUncloakB :
CurrentEvent = DuringWaitForUncloakB(CurrentEvent);
//process any events
if ( CurrentEvent.EventType != ES_NO_EVENT ) //If an event is active
{
switch (CurrentEvent.EventType)
{
case ES_TIMEOUT :
if (CurrentEvent.EventParam == 3)
{
updateFAC();
}
break;
case UpdateFinish:
printf("switching turn at target");
NextState = TurnAtTarget;
MakeTransition = True;
EntryEventKind.EventType = ES_ENTRY;
ReturnEvent = CurrentEvent;
break;
}
}
break;
//First thing to do is to turn at the target location
case TurnAtTarget :
CurrentEvent = DuringTurnAtTarget(CurrentEvent);
//process any events
if ( CurrentEvent.EventType != ES_NO_EVENT ) //If an event is active
{
switch (CurrentEvent.EventType)
{
case ES_TIMEOUT :
if (CurrentEvent.EventParam == 1)
{
updateFAC();
}
break;
case UpdateFinish :
OurBot = queryShip(15);
newAngle = (OurBot.r * 1.4);
rotDiff = angle - newAngle; //changed from newAngle*angle to newAngle-angle
if (abs((int)rotDiff ) <= angleInterval) //If the angle is within the angle interval (2 for going to load, 4 for going to fire)
{
//Start next state
NextState = MovingForward;
MakeTransition = True;
EntryEventKind.EventType = ES_ENTRY;
ReturnEvent = CurrentEvent;
break;
} else //Keep trying to get closer
{
RotateTo(angle);
}
break;
}
}
break;
case MovingForward:
CurrentEvent = DuringMovingForward(CurrentEvent);
if ( CurrentEvent.EventType != ES_NO_EVENT ) //If an event is active
{
switch (CurrentEvent.EventType)
{
case ES_TIMEOUT :
if (CurrentEvent.EventParam == 4)
{
if (pause == 0) //Flag again to make sure FAC updates before we get information
{
ES_Timer_SetTimer(4,150);
ES_Timer_StartTimer(4);
pause = 1;
} else
{
updateFAC();
pause = 0;
}
}
break;
case UpdateFinish:
if (count == 1)
{
/*if (tape == 1)
{
printf("goign to tape sense\r\n");
NextState = TapeSense;
MakeTransition = True;
EntryEventKind.EventType = ES_ENTRY;
ReturnEvent = CurrentEvent;
break;
} else */
printf("goign to aligning\r\n");
NextState = Aligning;
MakeTransition = True;
EntryEventKind.EventType = ES_ENTRY;
ReturnEvent = CurrentEvent;
break;
} else
{
count = 1;
NextState = TurnAtTarget;
MakeTransition = True;
EntryEventKind.EventType = ES_ENTRY;
ReturnEvent = CurrentEvent;
break;
}
}
}
break;
/* Tape aligning code if ever needed, UNUSED IN ACTUAL PROJECT DUE TO US NOT NEEDING IT */
/* case TapeSense:
CurrentEvent = DuringTapeSense(CurrentEvent);
//process any events
if ( CurrentEvent.EventType != ES_NO_EVENT ) //If an event is active
{
switch (CurrentEvent.EventType)
{
case ES_TIMEOUT :
if (CurrentEvent.EventParam == 1)
{
}
break;
case UpdateFinish :
if (ADS12_ReadADPin(1) < 300)
{
if (ADS12_ReadADPin(2) < 300)
{
NextState = Aligning;
MakeTransition = True;
EntryEventKind.EventType = ES_ENTRY;
ReturnEvent = CurrentEvent;
break;
} else {
requestTapeAlign();
RotateCWInf();
}
} else {
requestMidTape();
RotateTo(180);
if (queryShip(15).y < 127)
{
DriveForward();
} else {
DriveReverse();
}
}
break;
case MidTape :
Stop();
if (ADS12_ReadADPin(2) < 300)
{
NextState = Aligning;
MakeTransition = True;
EntryEventKind.EventType = ES_ENTRY;
ReturnEvent = CurrentEvent;
break;
} else {
requestTapeAlign();
RotateCW(1080);
}
break;
case TapeAlign :
Stop();
NextState = Aligning;
MakeTransition = True;
EntryEventKind.EventType = ES_ENTRY;
ReturnEvent = CurrentEvent;
break;
}
}
break; */
//Final align, same as turn at target but goes to set angle given as input
case Aligning:
CurrentEvent = DuringAligning(CurrentEvent);
if ( CurrentEvent.EventType != ES_NO_EVENT ) //If an event is active
{
switch (CurrentEvent.EventType)
{
case ES_TIMEOUT :
if (CurrentEvent.EventParam == 1)
{
if (pause == 0)
{
ES_Timer_SetTimer(1,100);
ES_Timer_StartTimer(1);
pause = 1;
} else
{
updateFAC();
pause = 0;
}
}
break;
case UpdateFinish :
if ((queryShip(15).r*45)/32 >= TargetCoord.r - angleInterval && (queryShip(15).r*45)/32 <= TargetCoord.r + angleInterval)
{
printf("x = %d, y = %d, Angle = %d\r\n", OurBot.x, OurBot.y, OurBot.r*45/32);
NextState = End;
MakeTransition = True;
EntryEventKind.EventType = ES_ENTRY;
ReturnEvent = CurrentEvent;
currEvent.EventType = MovementFinished;
PostMasterSM(currEvent);
break;
} else
{
RotateTo(TargetCoord.r);
}
break;
}
}
break;
case End:
break;
}
// If we are making a state transition
if (MakeTransition == True)
{
// Execute exit function for current state
CurrentEvent.EventType = ES_EXIT;
RunMovementSM(CurrentEvent);
CurrentState = NextState; //Modify state variable
// Execute entry function for new state
// this defaults to ES_ENTRY
RunMovementSM(EntryEventKind);
}
return(CurrentEvent);
}
/****************************************************************************
Function
StartMovementSM
Parameters
None
Returns
None
Description
Does any required initialization for this state machine
Notes
Author
J. Edward Carryer, 2/18/99, 10:38AM
****************************************************************************/
void StartMovementSM ( ES_Event CurrentEvent, int x, int y, int r, int angleint, int direction /*,int sense */ ) //goes to x, y, turns to r, sets precision on angleint, direction 0 for forward, 1 for reverse
{
// local variable to get debugger to display the value of CurrentEvent
ES_Event LocalEvent = 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
TargetCoord.x = x;
TargetCoord.y = y;
TargetCoord.r = r;
//tape = sense;
count = 0;
dir = direction;
angleInterval = angleint;
if ( ES_ENTRY_HISTORY != CurrentEvent.EventType )
{
CurrentState = WaitForUncloakB;
}
// call the entry function (if any) for the ENTRY_STATE
RunMovementSM(CurrentEvent);
}
/****************************************************************************
Function
QueryMovementSM
Parameters
None
Returns
TemplateState_t The current state of the Template state machine
Description
returns the current state of the Template state machine
Notes
Author
J. Edward Carryer, 2/11/05, 10:38AM
****************************************************************************/
ShootingState_t QueryMovementSM ( void )
{
return(CurrentState);
}
/***************************************************************************
private functions
***************************************************************************/
static ES_Event DuringWaitForUncloakB( ES_Event Event)
{
ES_Event ReturnEvent = Event;
if (Event.EventType == ES_ENTRY )
{
//Uncloak our bot
UncloakBot();
printf("Enter WaitForUncloak\r\n");
//Allow time for servo to uncloak
ES_Timer_SetTimer(3,300);
ES_Timer_StartTimer(3);
}
else if ( Event.EventType == ES_EXIT )
{
}else
// do the 'during' function for this state
{
}
return(ReturnEvent);
}
static ES_Event DuringTurnAtTarget( ES_Event Event)
{
ES_Event ReturnEvent = Event; // assmes no re-mapping or comsumption
ES_Event currEvent;
if (Event.EventType == ES_ENTRY )
{
printf("Enter TurnAtTarget\r\n");
OurBot = queryShip(15);
angle = atan(((double)(OurBot.y-TargetCoord.y))/((double)(OurBot.x-TargetCoord.x)))*180/3.14;
// printf("%d\r\n", (int)angle);
//For knowing which direction to turn
if (TargetCoord.x >= OurBot.x)
{
angle = 270-angle;
if (dir == 1)
{
angle += 180;
if (angle >= 360) angle -= 360;
}
RotateTo((int)(angle));
} else
{
angle = 90-angle;
if (dir == 1)
{
angle += 180;
if (angle >= 360) angle -= 360;
}
RotateTo((int)(angle));
}
}
else if ( Event.EventType == ES_EXIT )
{
}else
// do the 'during' function for this state
{
}
return(ReturnEvent);
}
static ES_Event DuringMovingForward( ES_Event Event)
{
ES_Event ReturnEvent = Event; // assumes no re-mapping or consumption
ES_Event currEvent;
double distance;
if (Event.EventType == ES_ENTRY )
{
printf("Enter MovingForward\r\n");
distance = sqrt(pow((OurBot.x-TargetCoord.x),2)+pow((OurBot.y-TargetCoord.y), 2));
printf("dist %d\r\n", (int)distance);
// printf("%d\r\n", (int)distance);
if(distance == 0) distance = 1;
if(count == 1) //First one should go farther
{
distance *= 1.3;
}
//Drive bot forward to be close enough to target
if (dir == 1)
{
DriveReverse();
ES_Timer_SetTimer(4, ((int)distance)*DistScale*9/5);
ES_Timer_StartTimer(4);
} else
{
DriveForward();
ES_Timer_SetTimer(4, ((int)distance)*DistScale);
ES_Timer_StartTimer(4);
}
// printf("%d\r\n", ((int)distance)*DistScale);
}
else if ( Event.EventType == ES_EXIT )
{
}else
// do the 'during' function for this state
{
}
return(ReturnEvent);
}
static ES_Event DuringTapeSense( ES_Event Event)
{
ES_Event ReturnEvent = Event; // assumes no re-mapping or consumption
ES_Event currEvent;
if (Event.EventType == ES_ENTRY )
{
updateFAC();
}
else if ( Event.EventType == ES_EXIT )
{
}else
// do the 'during' function for this state
{
}
return(ReturnEvent);
}
static ES_Event DuringAligning( ES_Event Event)
{
ES_Event ReturnEvent = Event; // assmes no re-mapping or comsumption
ES_Event currEvent;
if (Event.EventType == ES_ENTRY )
{
RotateTo(TargetCoord.r);
}
else if ( Event.EventType == ES_EXIT )
{
}else
// do the 'during' function for this state
{
}
return(ReturnEvent);
}
/*******************************
Public Functions
*******************************/
void RotateTo(int angle)
{
static coord OurBot;
static double newAngle;
double rotDiff;
OurBot = queryShip(15);
// printf("Target Angle = %d\r\n", angle);
newAngle = (OurBot.r * 1.4);
// printf("New Angle = %3.2f\r\n", newAngle);
rotDiff = angle - newAngle;
// printf("rotDiff = %d\r\n", (int)rotDiff);
//Calculates rotation
if((rotDiff >= 0) && (rotDiff < 180))
{
RotateCCW((int)rotDiff, 0);
} else if (rotDiff >= 180)
{
RotateCW((int)(360-rotDiff), 0);
} else if ((rotDiff < 0) && (rotDiff > -180))
{
RotateCW((int)-rotDiff, 0);
} else if (rotDiff <= -180)
{
RotateCCW((int)(360+rotDiff), 0);
}
}
void RotateToExact(int angle)
{
static coord OurBot;
static double newAngle;
double rotDiff;
OurBot = queryShip(15);
printf("Target Angle = %d\r\n", angle);
newAngle = (OurBot.r * 1.4);
printf("New Angle = %3.2f\r\n", newAngle);
rotDiff = angle - newAngle;
printf("rotDiff = %d\r\n", (int)rotDiff);
if((rotDiff >= 0) && (rotDiff < 180))
{
RotateCCW((int)rotDiff, 1);
} else if (rotDiff >= 180)
{
RotateCW((int)(360-rotDiff), 1);
} else if ((rotDiff < 0) && (rotDiff > -180))
{
RotateCW((int)-rotDiff, 1);
} else if (rotDiff <= -180)
{
RotateCCW((int)(360+rotDiff), 1);
}
}
Module
MovementSM.c
Revision
2.0.1
Description
This is a template file for implementing state machines.
Notes
History
When Who What/Why
-------------- --- --------
02/13/13 02:32 mt ME218B Project
02/08/12 09:56 jec revisions for the Events and Services Framework Gen2
02/13/10 14:29 jec revised Start and run to add new kind of entry function
to make implementing history entry cleaner
02/13/10 12:29 jec added NewEvent local variable to During function and
comments about using either it or Event as the return
02/11/10 15:54 jec more revised comments, removing last comment in during
function that belongs in the run function
02/09/10 17:21 jec updated comments about internal transitions on During funtion
02/18/09 10:14 jec removed redundant call to RunLowerlevelSM in EV_Entry
processing in During function
02/20/07 21:37 jec converted to use enumerated type for events & states
02/13/05 19:38 jec added support for self-transitions, reworked
to eliminate repeated transition code
02/11/05 16:54 jec converted to implment hierarchy explicitly
02/25/03 10:32 jec converted to take a passed event parameter
02/18/99 10:19 jec built template from MasterMachine.c
02/14/99 10:34 jec Began Coding
****************************************************************************/
/*----------------------------- Include Files -----------------------------*/
// Basic includes for a program using the Events and Services Framework
#include "ES_Configure.h"
#include "ES_Framework.h"
#include "ES_ServiceHeaders.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
*/
/*----------------------------- Module Defines ----------------------------*/
// define constants for the states for this machine
// and any other local defines
#define DistScale 20
/*---------------------------- 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 DuringWaitForUncloakB( ES_Event Event);
static ES_Event DuringTurnAtTarget( ES_Event Event);
static ES_Event DuringMovingForward( ES_Event Event);
static ES_Event DuringAligning(ES_Event Event);
/*---------------------------- Module Variables ---------------------------*/
// everybody needs a state variable, you may need others as well
static MovementState_t CurrentState = End;
static coord TargetCoord;
static coord OurBot;
static int count = 0;
static int tape = 0;
static int angleInterval = 1;
static int dir = 0;
static double angle;
/*------------------------------ Module Code ------------------------------*/
/****************************************************************************
Function
RunShootTargetSM
Parameters
ES_Event: the event to process
Returns
ES_Event: an event to return
Description
add your description here
Notes
uses nested switch/case to implement the machine.
Author
Michelle T, 2/13/13
****************************************************************************/
ES_Event RunMovementSM( ES_Event CurrentEvent)
{
unsigned char MakeTransition = False;/* are we making a state transition? */
ShootingState_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
ES_Event currEvent;
static double newAngle;
double rotDiff;
static int pause = 0;
if (CurrentEvent.EventType == GameEnd)
{
NextState = End;
MakeTransition = True;
EntryEventKind.EventType = ES_ENTRY;
ReturnEvent = CurrentEvent;
}
switch(CurrentState)
{
case WaitForUncloakB :
CurrentEvent = DuringWaitForUncloakB(CurrentEvent);
//process any events
if ( CurrentEvent.EventType != ES_NO_EVENT ) //If an event is active
{
switch (CurrentEvent.EventType)
{
case ES_TIMEOUT :
if (CurrentEvent.EventParam == 3)
{
updateFAC();
}
break;
case UpdateFinish:
printf("switching turn at target");
NextState = TurnAtTarget;
MakeTransition = True;
EntryEventKind.EventType = ES_ENTRY;
ReturnEvent = CurrentEvent;
break;
}
}
break;
//First thing to do is to turn at the target location
case TurnAtTarget :
CurrentEvent = DuringTurnAtTarget(CurrentEvent);
//process any events
if ( CurrentEvent.EventType != ES_NO_EVENT ) //If an event is active
{
switch (CurrentEvent.EventType)
{
case ES_TIMEOUT :
if (CurrentEvent.EventParam == 1)
{
updateFAC();
}
break;
case UpdateFinish :
OurBot = queryShip(15);
newAngle = (OurBot.r * 1.4);
rotDiff = angle - newAngle; //changed from newAngle*angle to newAngle-angle
if (abs((int)rotDiff ) <= angleInterval) //If the angle is within the angle interval (2 for going to load, 4 for going to fire)
{
//Start next state
NextState = MovingForward;
MakeTransition = True;
EntryEventKind.EventType = ES_ENTRY;
ReturnEvent = CurrentEvent;
break;
} else //Keep trying to get closer
{
RotateTo(angle);
}
break;
}
}
break;
case MovingForward:
CurrentEvent = DuringMovingForward(CurrentEvent);
if ( CurrentEvent.EventType != ES_NO_EVENT ) //If an event is active
{
switch (CurrentEvent.EventType)
{
case ES_TIMEOUT :
if (CurrentEvent.EventParam == 4)
{
if (pause == 0) //Flag again to make sure FAC updates before we get information
{
ES_Timer_SetTimer(4,150);
ES_Timer_StartTimer(4);
pause = 1;
} else
{
updateFAC();
pause = 0;
}
}
break;
case UpdateFinish:
if (count == 1)
{
/*if (tape == 1)
{
printf("goign to tape sense\r\n");
NextState = TapeSense;
MakeTransition = True;
EntryEventKind.EventType = ES_ENTRY;
ReturnEvent = CurrentEvent;
break;
} else */
printf("goign to aligning\r\n");
NextState = Aligning;
MakeTransition = True;
EntryEventKind.EventType = ES_ENTRY;
ReturnEvent = CurrentEvent;
break;
} else
{
count = 1;
NextState = TurnAtTarget;
MakeTransition = True;
EntryEventKind.EventType = ES_ENTRY;
ReturnEvent = CurrentEvent;
break;
}
}
}
break;
/* Tape aligning code if ever needed, UNUSED IN ACTUAL PROJECT DUE TO US NOT NEEDING IT */
/* case TapeSense:
CurrentEvent = DuringTapeSense(CurrentEvent);
//process any events
if ( CurrentEvent.EventType != ES_NO_EVENT ) //If an event is active
{
switch (CurrentEvent.EventType)
{
case ES_TIMEOUT :
if (CurrentEvent.EventParam == 1)
{
}
break;
case UpdateFinish :
if (ADS12_ReadADPin(1) < 300)
{
if (ADS12_ReadADPin(2) < 300)
{
NextState = Aligning;
MakeTransition = True;
EntryEventKind.EventType = ES_ENTRY;
ReturnEvent = CurrentEvent;
break;
} else {
requestTapeAlign();
RotateCWInf();
}
} else {
requestMidTape();
RotateTo(180);
if (queryShip(15).y < 127)
{
DriveForward();
} else {
DriveReverse();
}
}
break;
case MidTape :
Stop();
if (ADS12_ReadADPin(2) < 300)
{
NextState = Aligning;
MakeTransition = True;
EntryEventKind.EventType = ES_ENTRY;
ReturnEvent = CurrentEvent;
break;
} else {
requestTapeAlign();
RotateCW(1080);
}
break;
case TapeAlign :
Stop();
NextState = Aligning;
MakeTransition = True;
EntryEventKind.EventType = ES_ENTRY;
ReturnEvent = CurrentEvent;
break;
}
}
break; */
//Final align, same as turn at target but goes to set angle given as input
case Aligning:
CurrentEvent = DuringAligning(CurrentEvent);
if ( CurrentEvent.EventType != ES_NO_EVENT ) //If an event is active
{
switch (CurrentEvent.EventType)
{
case ES_TIMEOUT :
if (CurrentEvent.EventParam == 1)
{
if (pause == 0)
{
ES_Timer_SetTimer(1,100);
ES_Timer_StartTimer(1);
pause = 1;
} else
{
updateFAC();
pause = 0;
}
}
break;
case UpdateFinish :
if ((queryShip(15).r*45)/32 >= TargetCoord.r - angleInterval && (queryShip(15).r*45)/32 <= TargetCoord.r + angleInterval)
{
printf("x = %d, y = %d, Angle = %d\r\n", OurBot.x, OurBot.y, OurBot.r*45/32);
NextState = End;
MakeTransition = True;
EntryEventKind.EventType = ES_ENTRY;
ReturnEvent = CurrentEvent;
currEvent.EventType = MovementFinished;
PostMasterSM(currEvent);
break;
} else
{
RotateTo(TargetCoord.r);
}
break;
}
}
break;
case End:
break;
}
// If we are making a state transition
if (MakeTransition == True)
{
// Execute exit function for current state
CurrentEvent.EventType = ES_EXIT;
RunMovementSM(CurrentEvent);
CurrentState = NextState; //Modify state variable
// Execute entry function for new state
// this defaults to ES_ENTRY
RunMovementSM(EntryEventKind);
}
return(CurrentEvent);
}
/****************************************************************************
Function
StartMovementSM
Parameters
None
Returns
None
Description
Does any required initialization for this state machine
Notes
Author
J. Edward Carryer, 2/18/99, 10:38AM
****************************************************************************/
void StartMovementSM ( ES_Event CurrentEvent, int x, int y, int r, int angleint, int direction /*,int sense */ ) //goes to x, y, turns to r, sets precision on angleint, direction 0 for forward, 1 for reverse
{
// local variable to get debugger to display the value of CurrentEvent
ES_Event LocalEvent = 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
TargetCoord.x = x;
TargetCoord.y = y;
TargetCoord.r = r;
//tape = sense;
count = 0;
dir = direction;
angleInterval = angleint;
if ( ES_ENTRY_HISTORY != CurrentEvent.EventType )
{
CurrentState = WaitForUncloakB;
}
// call the entry function (if any) for the ENTRY_STATE
RunMovementSM(CurrentEvent);
}
/****************************************************************************
Function
QueryMovementSM
Parameters
None
Returns
TemplateState_t The current state of the Template state machine
Description
returns the current state of the Template state machine
Notes
Author
J. Edward Carryer, 2/11/05, 10:38AM
****************************************************************************/
ShootingState_t QueryMovementSM ( void )
{
return(CurrentState);
}
/***************************************************************************
private functions
***************************************************************************/
static ES_Event DuringWaitForUncloakB( ES_Event Event)
{
ES_Event ReturnEvent = Event;
if (Event.EventType == ES_ENTRY )
{
//Uncloak our bot
UncloakBot();
printf("Enter WaitForUncloak\r\n");
//Allow time for servo to uncloak
ES_Timer_SetTimer(3,300);
ES_Timer_StartTimer(3);
}
else if ( Event.EventType == ES_EXIT )
{
}else
// do the 'during' function for this state
{
}
return(ReturnEvent);
}
static ES_Event DuringTurnAtTarget( ES_Event Event)
{
ES_Event ReturnEvent = Event; // assmes no re-mapping or comsumption
ES_Event currEvent;
if (Event.EventType == ES_ENTRY )
{
printf("Enter TurnAtTarget\r\n");
OurBot = queryShip(15);
angle = atan(((double)(OurBot.y-TargetCoord.y))/((double)(OurBot.x-TargetCoord.x)))*180/3.14;
// printf("%d\r\n", (int)angle);
//For knowing which direction to turn
if (TargetCoord.x >= OurBot.x)
{
angle = 270-angle;
if (dir == 1)
{
angle += 180;
if (angle >= 360) angle -= 360;
}
RotateTo((int)(angle));
} else
{
angle = 90-angle;
if (dir == 1)
{
angle += 180;
if (angle >= 360) angle -= 360;
}
RotateTo((int)(angle));
}
}
else if ( Event.EventType == ES_EXIT )
{
}else
// do the 'during' function for this state
{
}
return(ReturnEvent);
}
static ES_Event DuringMovingForward( ES_Event Event)
{
ES_Event ReturnEvent = Event; // assumes no re-mapping or consumption
ES_Event currEvent;
double distance;
if (Event.EventType == ES_ENTRY )
{
printf("Enter MovingForward\r\n");
distance = sqrt(pow((OurBot.x-TargetCoord.x),2)+pow((OurBot.y-TargetCoord.y), 2));
printf("dist %d\r\n", (int)distance);
// printf("%d\r\n", (int)distance);
if(distance == 0) distance = 1;
if(count == 1) //First one should go farther
{
distance *= 1.3;
}
//Drive bot forward to be close enough to target
if (dir == 1)
{
DriveReverse();
ES_Timer_SetTimer(4, ((int)distance)*DistScale*9/5);
ES_Timer_StartTimer(4);
} else
{
DriveForward();
ES_Timer_SetTimer(4, ((int)distance)*DistScale);
ES_Timer_StartTimer(4);
}
// printf("%d\r\n", ((int)distance)*DistScale);
}
else if ( Event.EventType == ES_EXIT )
{
}else
// do the 'during' function for this state
{
}
return(ReturnEvent);
}
static ES_Event DuringTapeSense( ES_Event Event)
{
ES_Event ReturnEvent = Event; // assumes no re-mapping or consumption
ES_Event currEvent;
if (Event.EventType == ES_ENTRY )
{
updateFAC();
}
else if ( Event.EventType == ES_EXIT )
{
}else
// do the 'during' function for this state
{
}
return(ReturnEvent);
}
static ES_Event DuringAligning( ES_Event Event)
{
ES_Event ReturnEvent = Event; // assmes no re-mapping or comsumption
ES_Event currEvent;
if (Event.EventType == ES_ENTRY )
{
RotateTo(TargetCoord.r);
}
else if ( Event.EventType == ES_EXIT )
{
}else
// do the 'during' function for this state
{
}
return(ReturnEvent);
}
/*******************************
Public Functions
*******************************/
void RotateTo(int angle)
{
static coord OurBot;
static double newAngle;
double rotDiff;
OurBot = queryShip(15);
// printf("Target Angle = %d\r\n", angle);
newAngle = (OurBot.r * 1.4);
// printf("New Angle = %3.2f\r\n", newAngle);
rotDiff = angle - newAngle;
// printf("rotDiff = %d\r\n", (int)rotDiff);
//Calculates rotation
if((rotDiff >= 0) && (rotDiff < 180))
{
RotateCCW((int)rotDiff, 0);
} else if (rotDiff >= 180)
{
RotateCW((int)(360-rotDiff), 0);
} else if ((rotDiff < 0) && (rotDiff > -180))
{
RotateCW((int)-rotDiff, 0);
} else if (rotDiff <= -180)
{
RotateCCW((int)(360+rotDiff), 0);
}
}
void RotateToExact(int angle)
{
static coord OurBot;
static double newAngle;
double rotDiff;
OurBot = queryShip(15);
printf("Target Angle = %d\r\n", angle);
newAngle = (OurBot.r * 1.4);
printf("New Angle = %3.2f\r\n", newAngle);
rotDiff = angle - newAngle;
printf("rotDiff = %d\r\n", (int)rotDiff);
if((rotDiff >= 0) && (rotDiff < 180))
{
RotateCCW((int)rotDiff, 1);
} else if (rotDiff >= 180)
{
RotateCW((int)(360-rotDiff), 1);
} else if ((rotDiff < 0) && (rotDiff > -180))
{
RotateCW((int)-rotDiff, 1);
} else if (rotDiff <= -180)
{
RotateCCW((int)(360+rotDiff), 1);
}
}