ShootTargetSM.c
/****************************************************************************
Module
ShootTargetSM.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
//Dist scale used for driving forward/backwards scale
#define DistScale 24
/*---------------------------- 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 DuringWaitForFAC( ES_Event Event);
static ES_Event DuringWaitForAim( ES_Event Event);
static ES_Event DuringWaitForUncloak( ES_Event Event);
static ES_Event DuringWaitForLoad(ES_Event Event);
static ES_Event DuringWaitForShoot( ES_Event Event);
/*---------------------------- Module Variables ---------------------------*/
// everybody needs a state variable, you may need others as well
static ShootingState_t CurrentState = Ended;
static int TargetID;
static coord TargetCoord;
static coord OurBot;
static double LastShooterMotorPWM = 20;
static double ShooterMotorPWM;
static int currentSector = 1; //unused
static int count = 0;
/*------------------------------ 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 RunShootTargetSM( 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;
double angle;
if (CurrentEvent.EventType == GameEnd)
{
NextState = Ended;
MakeTransition = True;
EntryEventKind.EventType = ES_ENTRY;
ReturnEvent = CurrentEvent;
}
switch ( CurrentState )
{
case WaitForUncloak :
CurrentEvent = DuringWaitForUncloak(CurrentEvent);
//process any events
if ( CurrentEvent.EventType != ES_NO_EVENT ) //If an event is active
{
switch (CurrentEvent.EventType)
{
case ES_TIMEOUT :
if (CurrentEvent.EventParam == 2)
{
NextState = WaitForFAC;
MakeTransition = True;
EntryEventKind.EventType = ES_ENTRY;
ReturnEvent = CurrentEvent;
break;
}
}
}
break;
case WaitForFAC :
CurrentEvent = DuringWaitForFAC(CurrentEvent);
if ( CurrentEvent.EventType != ES_NO_EVENT ) //If an event is active
{
switch (CurrentEvent.EventType)
{
case UpdateFinish : //Update finished, find target
TargetID = getNextTarget(); //identify next target
TargetCoord = queryShip(TargetID);
if (TargetCoord.active == FALSE) //Mainly for enemy ship
{
updateFAC();
break;
}
OurBot = queryShip(15);
setShooterSpeed(); //Finds speed to set motor at
// NextState = WaitForInPosition;
NextState = WaitForLoad;
MakeTransition = True;
EntryEventKind.EventType = ES_ENTRY;
ReturnEvent = CurrentEvent;
break;
// repeat cases as required for relevant events
}
}
break;
case WaitForLoad :
// CurrentEvent = DuringWaitForInPosition(CurrentEvent);
CurrentEvent = DuringWaitForLoad(CurrentEvent);
//process any events
if ( CurrentEvent.EventType != ES_NO_EVENT ) //If an event is active
{
switch (CurrentEvent.EventType)
{
case ES_TIMEOUT :
if (CurrentEvent.EventParam == 2)
{
NextState = WaitForAim;
MakeTransition = True;
EntryEventKind.EventType = ES_ENTRY;
ReturnEvent = CurrentEvent;
break;
}
}
}
break;
case WaitForAim :
CurrentEvent = DuringWaitForAim(CurrentEvent);
//process any events
if ( CurrentEvent.EventType != ES_NO_EVENT ) //If an event is active
{
switch (CurrentEvent.EventType)
{
case UpdateFinish :
printf("OurBotr before aiming\r\n", OurBot.r);
aimTarget();
break;
case ES_TIMEOUT :
if (CurrentEvent.EventParam == 1) // rotation finished
{
if (count == 0)
{
ES_Timer_SetTimer(1,500); //Wait before updating FAC again
ES_Timer_StartTimer(1);
count = 1;
} else
{
updateFAC();
count = 0;
}
} else if (CurrentEvent.EventParam == 2)
{
NextState = WaitForShoot;
MakeTransition = True;
EntryEventKind.EventType = ES_ENTRY;
ReturnEvent = CurrentEvent;
}
break;
}
}
break;
case WaitForShoot :
CurrentEvent = DuringWaitForShoot(CurrentEvent);
//process any events
if ( CurrentEvent.EventType != ES_NO_EVENT ) //If an event is active
{
switch (CurrentEvent.EventType)
{
case ES_TIMEOUT :
if (CurrentEvent.EventParam == 2)
{
//Reset and send event back to tell master sm that it finished firing
NextState = Ended;
MakeTransition = True;
EntryEventKind.EventType = ES_ENTRY;
ReturnEvent = CurrentEvent;
resetLoad();
//CloakBot();
changeAmmo(-1);
currEvent.EventType = BallFired;
PostMasterSM(currEvent);
setLastShip(TargetID);
break;
}
}
}
break;
case Ended :
break;
// repeat state pattern as required for other states
}
// If we are making a state transition
if (MakeTransition == True)
{
// Execute exit function for current state
CurrentEvent.EventType = ES_EXIT;
RunShootTargetSM(CurrentEvent);
CurrentState = NextState; //Modify state variable
// Execute entry function for new state
// this defaults to ES_ENTRY
RunShootTargetSM(EntryEventKind);
}
return(CurrentEvent);
}
/****************************************************************************
Function
StartShootTargetSM
Parameters
None
Returns
None
Description
Does any required initialization for this state machine
Notes
Author
J. Edward Carryer, 2/18/99, 10:38AM
****************************************************************************/
void StartShootTargetSM ( ES_Event CurrentEvent )
{
// 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
printf("Going to Shoot\r\n");
CurrentState = WaitForUncloak;
// call the entry function (if any) for the ENTRY_STATE
RunShootTargetSM(CurrentEvent);
}
/****************************************************************************
Function
QueryShootTargetSM
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 QueryShootTargetSM ( void )
{
return(CurrentState);
}
/***************************************************************************
private functions
***************************************************************************/
static ES_Event DuringWaitForUncloak( ES_Event Event)
{
ES_Event ReturnEvent = Event; // assmes no re-mapping or comsumption
if (Event.EventType == ES_ENTRY )
{
printf("During Wait For Uncloak\r\n");
//Uncloak our bot
//UncloakBot();
//If only one bot left, wait for awhile before updating to see if it goes down
if (getColor() == 1 && queryBlueShips() == 1)
{
ES_Timer_SetTimer(2,1500);
ES_Timer_StartTimer(2);
} else if (getColor() == 2 && queryRedShips() == 1)
{
ES_Timer_SetTimer(2,1500);
ES_Timer_StartTimer(2);
} else
{
ES_Timer_SetTimer(2,400);
ES_Timer_StartTimer(2);
}
}
else if ( Event.EventType == ES_EXIT )
{
updateFAC();
}else
// do the 'during' function for this state
{
}
// 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 DuringWaitForAim( ES_Event Event)
{
ES_Event ReturnEvent = Event; // assmes no re-mapping or comsumption
if (Event.EventType == ES_ENTRY)
{
printf("During Wait For Aim\r\n");
// implement any entry actions required for this state machine
//Rotate bot to point to correct sector
if ((queryShip(getNextTarget()).sector == 2 || queryShip(getNextTarget()).sector == 5) )
{
if(getColor() == 1)
{
RotateToExact(225);
} else
{
RotateToExact(45);
}
} else if ((queryShip(getNextTarget()).sector == 3 || queryShip(getNextTarget()).sector == 6) )
{
if(getColor() == 1)
{
RotateToExact(315);
} else
{
RotateToExact(135);
}
} else
{
if(getColor() == 1)
{
RotateToExact(270);
} else
{
RotateToExact(90);
}
}
//Allow time for servo to aim
}
else if ( Event.EventType == ES_EXIT )
{
} else
{
}
return(ReturnEvent);
}
static ES_Event DuringWaitForLoad( ES_Event Event)
{
ES_Event ReturnEvent = Event; // assmes no re-mapping or comsumption
int WaitTime;
if (Event.EventType == ES_ENTRY)
{
printf("During Wait For Load\r\n");
// implement any entry actions required for this state machine
loadShooter();
//Wait time based on how much of a change on shooter motor
WaitTime = 150*abs((int)ShooterMotorPWM - (int)LastShooterMotorPWM)+500;
printf("waitime %d \r\n", WaitTime);
LastShooterMotorPWM = ShooterMotorPWM;
ES_Timer_SetTimer(2,WaitTime);
ES_Timer_StartTimer(2);
}
else if ( Event.EventType == ES_EXIT )
{
} else
{
}
return(ReturnEvent);
}
static ES_Event DuringWaitForFAC( ES_Event Event)
{
ES_Event ReturnEvent = Event; // assmes no re-mapping or comsumption
if (Event.EventType == ES_ENTRY)
{
printf("During Wait For FAC\r\n");
updateFAC();
}
else if ( Event.EventType == ES_EXIT )
{
} else
{
}
return(ReturnEvent);
}
static ES_Event DuringWaitForShoot( ES_Event Event)
{
ES_Event ReturnEvent = Event; // assmes no re-mapping or comsumption
ES_Event currEvent;
if (Event.EventType == ES_ENTRY )
{
//Push ball into launcher (mechanism?)
shootBall();
//Allow time to pass
ES_Timer_SetTimer(2,300);
ES_Timer_StartTimer(2);
}
else if ( Event.EventType == ES_EXIT )
{
//Cloak bot when not shooting
}else
{
}
return(ReturnEvent);
}
/*******************************
Public Functions
*******************************/
void aimTarget (void)
{
static int TargetID;
coord TargetCoord;
coord OurBot;
double angle;
int color = getColor();
int botangle = 0;
//***May want to define these in a state and not the function***//
TargetID = getNextTarget();
TargetCoord = queryShip(TargetID);
OurBot = queryShip(15);
printf("TargetID = %d\r\n", TargetID);
//Calculate angle in degrees
angle = atan(((double)(OurBot.y-TargetCoord.y))/((double)(OurBot.x-TargetCoord.x)))*180/3.14;
if (color == 1)
{
botangle = -270+OurBot.r*45/32;
} else
{
botangle = -90+OurBot.r*45/32;
}
printf("Target = %d, angle = %d, botangle = %d, x, y = %d %d, tx, ty = %d %d\r\n", TargetID, (int)(90 + angle), botangle, OurBot.x, OurBot.y, TargetCoord.x, TargetCoord.y);
Servo_SetAngle(AimServo, 90+angle+botangle);
ES_Timer_SetTimer(2,500);
ES_Timer_StartTimer(2);
//Change angle of launcher
/*if(TargetCoord.y > OurBot.y)
{
Servo_SetAngle(AimServo, 90-angle);
} else
{
Servo_SetAngle(AimServo, angle+90);
} */
}
void setShooterSpeed( void)
{
double Current_BatVoltage;
double ShootingDistance;
ShootingDistance = (double)sqrt((pow((OurBot.y-TargetCoord.y),2) + pow((OurBot.x-TargetCoord.x),2))); //Max should be about 190
Current_BatVoltage = (double)ADS12_ReadADPin(4);
ShooterMotorPWM = (CAL_BAT_VOLTAGE/Current_BatVoltage)*(ShootingDistance*SHOOTER_DISTANCE_SCALE_M + SHOOTER_DISTANCE_SCALE_B);
printf("PWM = %d, ShootingDist = %d\r\n", (int)ShooterMotorPWM, (int)ShootingDistance);
SetDuty(ShooterMotorPWM,1);
}
void shootBall( void)
{
Servo_SetAngle(LoadServo, 112);
}
void resetLoad( void)
{
Servo_SetAngle(LoadServo, 55);
}
void resetCurrentSector( void)
{
currentSector = 1;
}
Module
ShootTargetSM.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
//Dist scale used for driving forward/backwards scale
#define DistScale 24
/*---------------------------- 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 DuringWaitForFAC( ES_Event Event);
static ES_Event DuringWaitForAim( ES_Event Event);
static ES_Event DuringWaitForUncloak( ES_Event Event);
static ES_Event DuringWaitForLoad(ES_Event Event);
static ES_Event DuringWaitForShoot( ES_Event Event);
/*---------------------------- Module Variables ---------------------------*/
// everybody needs a state variable, you may need others as well
static ShootingState_t CurrentState = Ended;
static int TargetID;
static coord TargetCoord;
static coord OurBot;
static double LastShooterMotorPWM = 20;
static double ShooterMotorPWM;
static int currentSector = 1; //unused
static int count = 0;
/*------------------------------ 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 RunShootTargetSM( 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;
double angle;
if (CurrentEvent.EventType == GameEnd)
{
NextState = Ended;
MakeTransition = True;
EntryEventKind.EventType = ES_ENTRY;
ReturnEvent = CurrentEvent;
}
switch ( CurrentState )
{
case WaitForUncloak :
CurrentEvent = DuringWaitForUncloak(CurrentEvent);
//process any events
if ( CurrentEvent.EventType != ES_NO_EVENT ) //If an event is active
{
switch (CurrentEvent.EventType)
{
case ES_TIMEOUT :
if (CurrentEvent.EventParam == 2)
{
NextState = WaitForFAC;
MakeTransition = True;
EntryEventKind.EventType = ES_ENTRY;
ReturnEvent = CurrentEvent;
break;
}
}
}
break;
case WaitForFAC :
CurrentEvent = DuringWaitForFAC(CurrentEvent);
if ( CurrentEvent.EventType != ES_NO_EVENT ) //If an event is active
{
switch (CurrentEvent.EventType)
{
case UpdateFinish : //Update finished, find target
TargetID = getNextTarget(); //identify next target
TargetCoord = queryShip(TargetID);
if (TargetCoord.active == FALSE) //Mainly for enemy ship
{
updateFAC();
break;
}
OurBot = queryShip(15);
setShooterSpeed(); //Finds speed to set motor at
// NextState = WaitForInPosition;
NextState = WaitForLoad;
MakeTransition = True;
EntryEventKind.EventType = ES_ENTRY;
ReturnEvent = CurrentEvent;
break;
// repeat cases as required for relevant events
}
}
break;
case WaitForLoad :
// CurrentEvent = DuringWaitForInPosition(CurrentEvent);
CurrentEvent = DuringWaitForLoad(CurrentEvent);
//process any events
if ( CurrentEvent.EventType != ES_NO_EVENT ) //If an event is active
{
switch (CurrentEvent.EventType)
{
case ES_TIMEOUT :
if (CurrentEvent.EventParam == 2)
{
NextState = WaitForAim;
MakeTransition = True;
EntryEventKind.EventType = ES_ENTRY;
ReturnEvent = CurrentEvent;
break;
}
}
}
break;
case WaitForAim :
CurrentEvent = DuringWaitForAim(CurrentEvent);
//process any events
if ( CurrentEvent.EventType != ES_NO_EVENT ) //If an event is active
{
switch (CurrentEvent.EventType)
{
case UpdateFinish :
printf("OurBotr before aiming\r\n", OurBot.r);
aimTarget();
break;
case ES_TIMEOUT :
if (CurrentEvent.EventParam == 1) // rotation finished
{
if (count == 0)
{
ES_Timer_SetTimer(1,500); //Wait before updating FAC again
ES_Timer_StartTimer(1);
count = 1;
} else
{
updateFAC();
count = 0;
}
} else if (CurrentEvent.EventParam == 2)
{
NextState = WaitForShoot;
MakeTransition = True;
EntryEventKind.EventType = ES_ENTRY;
ReturnEvent = CurrentEvent;
}
break;
}
}
break;
case WaitForShoot :
CurrentEvent = DuringWaitForShoot(CurrentEvent);
//process any events
if ( CurrentEvent.EventType != ES_NO_EVENT ) //If an event is active
{
switch (CurrentEvent.EventType)
{
case ES_TIMEOUT :
if (CurrentEvent.EventParam == 2)
{
//Reset and send event back to tell master sm that it finished firing
NextState = Ended;
MakeTransition = True;
EntryEventKind.EventType = ES_ENTRY;
ReturnEvent = CurrentEvent;
resetLoad();
//CloakBot();
changeAmmo(-1);
currEvent.EventType = BallFired;
PostMasterSM(currEvent);
setLastShip(TargetID);
break;
}
}
}
break;
case Ended :
break;
// repeat state pattern as required for other states
}
// If we are making a state transition
if (MakeTransition == True)
{
// Execute exit function for current state
CurrentEvent.EventType = ES_EXIT;
RunShootTargetSM(CurrentEvent);
CurrentState = NextState; //Modify state variable
// Execute entry function for new state
// this defaults to ES_ENTRY
RunShootTargetSM(EntryEventKind);
}
return(CurrentEvent);
}
/****************************************************************************
Function
StartShootTargetSM
Parameters
None
Returns
None
Description
Does any required initialization for this state machine
Notes
Author
J. Edward Carryer, 2/18/99, 10:38AM
****************************************************************************/
void StartShootTargetSM ( ES_Event CurrentEvent )
{
// 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
printf("Going to Shoot\r\n");
CurrentState = WaitForUncloak;
// call the entry function (if any) for the ENTRY_STATE
RunShootTargetSM(CurrentEvent);
}
/****************************************************************************
Function
QueryShootTargetSM
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 QueryShootTargetSM ( void )
{
return(CurrentState);
}
/***************************************************************************
private functions
***************************************************************************/
static ES_Event DuringWaitForUncloak( ES_Event Event)
{
ES_Event ReturnEvent = Event; // assmes no re-mapping or comsumption
if (Event.EventType == ES_ENTRY )
{
printf("During Wait For Uncloak\r\n");
//Uncloak our bot
//UncloakBot();
//If only one bot left, wait for awhile before updating to see if it goes down
if (getColor() == 1 && queryBlueShips() == 1)
{
ES_Timer_SetTimer(2,1500);
ES_Timer_StartTimer(2);
} else if (getColor() == 2 && queryRedShips() == 1)
{
ES_Timer_SetTimer(2,1500);
ES_Timer_StartTimer(2);
} else
{
ES_Timer_SetTimer(2,400);
ES_Timer_StartTimer(2);
}
}
else if ( Event.EventType == ES_EXIT )
{
updateFAC();
}else
// do the 'during' function for this state
{
}
// 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 DuringWaitForAim( ES_Event Event)
{
ES_Event ReturnEvent = Event; // assmes no re-mapping or comsumption
if (Event.EventType == ES_ENTRY)
{
printf("During Wait For Aim\r\n");
// implement any entry actions required for this state machine
//Rotate bot to point to correct sector
if ((queryShip(getNextTarget()).sector == 2 || queryShip(getNextTarget()).sector == 5) )
{
if(getColor() == 1)
{
RotateToExact(225);
} else
{
RotateToExact(45);
}
} else if ((queryShip(getNextTarget()).sector == 3 || queryShip(getNextTarget()).sector == 6) )
{
if(getColor() == 1)
{
RotateToExact(315);
} else
{
RotateToExact(135);
}
} else
{
if(getColor() == 1)
{
RotateToExact(270);
} else
{
RotateToExact(90);
}
}
//Allow time for servo to aim
}
else if ( Event.EventType == ES_EXIT )
{
} else
{
}
return(ReturnEvent);
}
static ES_Event DuringWaitForLoad( ES_Event Event)
{
ES_Event ReturnEvent = Event; // assmes no re-mapping or comsumption
int WaitTime;
if (Event.EventType == ES_ENTRY)
{
printf("During Wait For Load\r\n");
// implement any entry actions required for this state machine
loadShooter();
//Wait time based on how much of a change on shooter motor
WaitTime = 150*abs((int)ShooterMotorPWM - (int)LastShooterMotorPWM)+500;
printf("waitime %d \r\n", WaitTime);
LastShooterMotorPWM = ShooterMotorPWM;
ES_Timer_SetTimer(2,WaitTime);
ES_Timer_StartTimer(2);
}
else if ( Event.EventType == ES_EXIT )
{
} else
{
}
return(ReturnEvent);
}
static ES_Event DuringWaitForFAC( ES_Event Event)
{
ES_Event ReturnEvent = Event; // assmes no re-mapping or comsumption
if (Event.EventType == ES_ENTRY)
{
printf("During Wait For FAC\r\n");
updateFAC();
}
else if ( Event.EventType == ES_EXIT )
{
} else
{
}
return(ReturnEvent);
}
static ES_Event DuringWaitForShoot( ES_Event Event)
{
ES_Event ReturnEvent = Event; // assmes no re-mapping or comsumption
ES_Event currEvent;
if (Event.EventType == ES_ENTRY )
{
//Push ball into launcher (mechanism?)
shootBall();
//Allow time to pass
ES_Timer_SetTimer(2,300);
ES_Timer_StartTimer(2);
}
else if ( Event.EventType == ES_EXIT )
{
//Cloak bot when not shooting
}else
{
}
return(ReturnEvent);
}
/*******************************
Public Functions
*******************************/
void aimTarget (void)
{
static int TargetID;
coord TargetCoord;
coord OurBot;
double angle;
int color = getColor();
int botangle = 0;
//***May want to define these in a state and not the function***//
TargetID = getNextTarget();
TargetCoord = queryShip(TargetID);
OurBot = queryShip(15);
printf("TargetID = %d\r\n", TargetID);
//Calculate angle in degrees
angle = atan(((double)(OurBot.y-TargetCoord.y))/((double)(OurBot.x-TargetCoord.x)))*180/3.14;
if (color == 1)
{
botangle = -270+OurBot.r*45/32;
} else
{
botangle = -90+OurBot.r*45/32;
}
printf("Target = %d, angle = %d, botangle = %d, x, y = %d %d, tx, ty = %d %d\r\n", TargetID, (int)(90 + angle), botangle, OurBot.x, OurBot.y, TargetCoord.x, TargetCoord.y);
Servo_SetAngle(AimServo, 90+angle+botangle);
ES_Timer_SetTimer(2,500);
ES_Timer_StartTimer(2);
//Change angle of launcher
/*if(TargetCoord.y > OurBot.y)
{
Servo_SetAngle(AimServo, 90-angle);
} else
{
Servo_SetAngle(AimServo, angle+90);
} */
}
void setShooterSpeed( void)
{
double Current_BatVoltage;
double ShootingDistance;
ShootingDistance = (double)sqrt((pow((OurBot.y-TargetCoord.y),2) + pow((OurBot.x-TargetCoord.x),2))); //Max should be about 190
Current_BatVoltage = (double)ADS12_ReadADPin(4);
ShooterMotorPWM = (CAL_BAT_VOLTAGE/Current_BatVoltage)*(ShootingDistance*SHOOTER_DISTANCE_SCALE_M + SHOOTER_DISTANCE_SCALE_B);
printf("PWM = %d, ShootingDist = %d\r\n", (int)ShooterMotorPWM, (int)ShootingDistance);
SetDuty(ShooterMotorPWM,1);
}
void shootBall( void)
{
Servo_SetAngle(LoadServo, 112);
}
void resetLoad( void)
{
Servo_SetAngle(LoadServo, 55);
}
void resetCurrentSector( void)
{
currentSector = 1;
}