ShootTargetSM (Pseudo code)
Structure coord
Has x, y, r, active, sector (all ints)
Module Variables -
ShootingState_t CurrentState
static ShootingState_t CurrentState
static int TargetID
static coord TargetCoord
static coord OurBot
static double LastShooterMotorPWM
static double ShooterMotorPWM
static int currentSector
static int count
RunShootTargetSM
if CurrentEvent.EventType is GameEnd
Make NextState Ended
Set MakeTransition to True, Make EntryEventKind.EventType ES_ENTRY
switch CurrentState
case WaitForUncloak
Call DuringWaitForUncloak
if CurrentEvent is not ES_NO_EVENT
if Event is ES_TIMEOUT and EventParam is 2
Set NextState to WaitForFAC
Set MakeTransition to True, Make EntryEventKind.EventType ES_ENTRY
case WaitForFAC
Call DuringWaitForFAC
if CurrentEvent is not ES_NO_EVENT
switch (CurrentEvent.EventType)
case UpdateFinish // If FAC Update is finished
Call getNextTarget, assign to TargetID
Call queryShip(TargetID)
Set OurBot equal to queryShip(15);
Call setShooterSpeed
Set NextState to WaitForLoad
Set MakeTransition to True, Make EntryEventKind.EventType ES_ENTRY
case WaitForLoad
Call DuringWaitForLoad
if CurrentEvent is not ES_NO_EVENT
switch (CurrentEvent.EventType)
case ES_TIMEOUT
if EventParam is 2
NextState = WaitForAim
Set MakeTransition to True, Make EntryEventKind.EventType ES_ENTRY
case WaitForAim
Call DuringWaitForAim
if CurrentEvent is not ES_NO_EVENT
switch (CurrentEvent.EventType)
case UpdateFinish
call aimTarget function
case ES_TIMEOUT
if EventParam is 1 // rotation finished
if (count == 0)
Call ES_Timer_SetTimer, Timer 1, 500 ms //Wait before updating FAC again
Call ES_Timer_StartTimer, Timer 1
set count equal to 1
else
call updateFAC
set count equal to 0
else if (EventParam is 2)
Set NextState to WaitForShoot;
Set MakeTransition to True, Make EntryEventKind.EventType ES_ENTRY
case WaitForShoot
Call DuringWaitForShoot
if CurrentEvent is not ES_NO_EVENT
switch (CurrentEvent.EventType)
case ES_TIMEOUT
if EventParam is 2 // finished firing
Set NextState to Ended;
Set MakeTransition to True, Make EntryEventKind.EventType ES_ENTRY
call resetLoad
call changeAmmo(-1)
Set currEvent.EventType to BallFired
PostMasterSM(currEvent)
call setLastShip(TargetID)
case Ended
if MakeTransition is True // If we are making a state transition
Set CurrentEvent.EventType to ES_EXIT // Execute exit function for current state
Call RunShootTargetSM(CurrentEvent)
Set CurrentState to NextState //Modify state variable
RunShootTargetSM(EntryEventKind)// Execute entry function for new state, default ES_ENTRY
StartShootTargetSM(CurrentEvent)
Set LocalEvent equal to CurrentEvent
Set CurrentState to WaitForUncloak
RunShootTargetSM(CurrentEvent)
QueryShootTargetSM
return CurrentState
// Private Functions:
DuringWaitForUncloak(Event)
Set ReturnEvent equal to Event
if EventType is ES_ENTRY
// determine where to turn in order to hit ships in different sectors
if queryShip(getNextTarget).sector is equal to 2 or 5
if getColor is 1
RotateToExact(225)
else if getColor is 2
RotateToExact(45)
else if queryShip(getNextTarget).sector is equal to 3 or 6
if getColor is 1
RotateToExact(315)
else if getColor is 2
RotateToExact(135)
// else if sector 1 or 4 (in the middle), don't rotate
else if (EventType is ES_EXIT)
// do nothing
return(ReturnEvent)
DuringWaitForLoad(Event)
Set ReturnEvent equal to Event
int WaitTime
if EventType is ES_ENTRY
call loadShooter
// scale ramp up waittime based on change in launcher motor PWM
set WaitTime = to 150*abs((int)ShooterMotorPWM - (int)LastShooterMotorPWM)+500
set LastShooterMotorPWM equal to ShooterMotorPWM
call ES_Timer_SetTimer, Timer 2, WaitTime
call ES_Timer_StartTimer, Timer 2
else if (EventType is ES_EXIT)
// do nothing
return(ReturnEvent)
DuringWaitForFAC(Event)
Set ReturnEvent equal to Event
int WaitTime
if EventType is ES_ENTRY
call updateFAC
else if (EventType is ES_EXIT)
// do nothing
return(ReturnEvent)
DuringWaitForShoot(Event)
Set ReturnEvent equal to Event
int WaitTime
if EventType is ES_ENTRY
call shootBall
call ES_Timer_SetTimer, Timer 2, 300 ms
call ES_Timer_StartTimer, Timer 2
else if (EventType is ES_EXIT)
// do nothing
return(ReturnEvent)
// public functions:
void aimTarget (void)
static int TargetID
coord TargetCoord
coord OurBot
double angle
set int color equal to getColor()
set int botangle to
set TargetID equal to getNextTarget
set TargetCoord equal to queryShip(TargetID)
set OurBot equal to queryShip(15)
//Calculate angle in degrees
set angle equal to atan(((double)(OurBot.y-TargetCoord.y))/((double)(OurBot.x-TargetCoord.x)))*180/3.14;
if color is 1
set botangle = -270+OurBot.r*45/32;
else
set botangle = -90+OurBot.r*45/32;
call Servo_SetAngle(AimServo, 90+angle+botangle);
call ES_Timer_SetTimer, Timer 2, 500 ms
call ES_Timer_StartTimer, Timer 2
void setShooterSpeed( void)
double Current_BatVoltage
double ShootingDistance
set ShootingDistance equal to (double)sqrt((pow((OurBot.y-TargetCoord.y),2) + pow((OurBot.x-TargetCoord.x),2))); //Max should be about 190
set Current_BatVoltage equal to (double)ADS12_ReadADPin(4);
set ShooterMotorPWM equal to (CAL_BAT_VOLTAGE/Current_BatVoltage)*(ShootingDistance*SHOOTER_DISTANCE_SCALE_M + SHOOTER_DISTANCE_SCALE_B);
call SetDuty(ShooterMotorPWM,1);
void shootBall( void)
call Servo_SetAngle(LoadServo, 112)
void resetLoad( void)
call Servo_SetAngle(LoadServo, 55)
void resetCurrentSector( void)
set currentSector equal to 1
Has x, y, r, active, sector (all ints)
Module Variables -
ShootingState_t CurrentState
static ShootingState_t CurrentState
static int TargetID
static coord TargetCoord
static coord OurBot
static double LastShooterMotorPWM
static double ShooterMotorPWM
static int currentSector
static int count
RunShootTargetSM
if CurrentEvent.EventType is GameEnd
Make NextState Ended
Set MakeTransition to True, Make EntryEventKind.EventType ES_ENTRY
switch CurrentState
case WaitForUncloak
Call DuringWaitForUncloak
if CurrentEvent is not ES_NO_EVENT
if Event is ES_TIMEOUT and EventParam is 2
Set NextState to WaitForFAC
Set MakeTransition to True, Make EntryEventKind.EventType ES_ENTRY
case WaitForFAC
Call DuringWaitForFAC
if CurrentEvent is not ES_NO_EVENT
switch (CurrentEvent.EventType)
case UpdateFinish // If FAC Update is finished
Call getNextTarget, assign to TargetID
Call queryShip(TargetID)
Set OurBot equal to queryShip(15);
Call setShooterSpeed
Set NextState to WaitForLoad
Set MakeTransition to True, Make EntryEventKind.EventType ES_ENTRY
case WaitForLoad
Call DuringWaitForLoad
if CurrentEvent is not ES_NO_EVENT
switch (CurrentEvent.EventType)
case ES_TIMEOUT
if EventParam is 2
NextState = WaitForAim
Set MakeTransition to True, Make EntryEventKind.EventType ES_ENTRY
case WaitForAim
Call DuringWaitForAim
if CurrentEvent is not ES_NO_EVENT
switch (CurrentEvent.EventType)
case UpdateFinish
call aimTarget function
case ES_TIMEOUT
if EventParam is 1 // rotation finished
if (count == 0)
Call ES_Timer_SetTimer, Timer 1, 500 ms //Wait before updating FAC again
Call ES_Timer_StartTimer, Timer 1
set count equal to 1
else
call updateFAC
set count equal to 0
else if (EventParam is 2)
Set NextState to WaitForShoot;
Set MakeTransition to True, Make EntryEventKind.EventType ES_ENTRY
case WaitForShoot
Call DuringWaitForShoot
if CurrentEvent is not ES_NO_EVENT
switch (CurrentEvent.EventType)
case ES_TIMEOUT
if EventParam is 2 // finished firing
Set NextState to Ended;
Set MakeTransition to True, Make EntryEventKind.EventType ES_ENTRY
call resetLoad
call changeAmmo(-1)
Set currEvent.EventType to BallFired
PostMasterSM(currEvent)
call setLastShip(TargetID)
case Ended
if MakeTransition is True // If we are making a state transition
Set CurrentEvent.EventType to ES_EXIT // Execute exit function for current state
Call RunShootTargetSM(CurrentEvent)
Set CurrentState to NextState //Modify state variable
RunShootTargetSM(EntryEventKind)// Execute entry function for new state, default ES_ENTRY
StartShootTargetSM(CurrentEvent)
Set LocalEvent equal to CurrentEvent
Set CurrentState to WaitForUncloak
RunShootTargetSM(CurrentEvent)
QueryShootTargetSM
return CurrentState
// Private Functions:
DuringWaitForUncloak(Event)
Set ReturnEvent equal to Event
if EventType is ES_ENTRY
// determine where to turn in order to hit ships in different sectors
if queryShip(getNextTarget).sector is equal to 2 or 5
if getColor is 1
RotateToExact(225)
else if getColor is 2
RotateToExact(45)
else if queryShip(getNextTarget).sector is equal to 3 or 6
if getColor is 1
RotateToExact(315)
else if getColor is 2
RotateToExact(135)
// else if sector 1 or 4 (in the middle), don't rotate
else if (EventType is ES_EXIT)
// do nothing
return(ReturnEvent)
DuringWaitForLoad(Event)
Set ReturnEvent equal to Event
int WaitTime
if EventType is ES_ENTRY
call loadShooter
// scale ramp up waittime based on change in launcher motor PWM
set WaitTime = to 150*abs((int)ShooterMotorPWM - (int)LastShooterMotorPWM)+500
set LastShooterMotorPWM equal to ShooterMotorPWM
call ES_Timer_SetTimer, Timer 2, WaitTime
call ES_Timer_StartTimer, Timer 2
else if (EventType is ES_EXIT)
// do nothing
return(ReturnEvent)
DuringWaitForFAC(Event)
Set ReturnEvent equal to Event
int WaitTime
if EventType is ES_ENTRY
call updateFAC
else if (EventType is ES_EXIT)
// do nothing
return(ReturnEvent)
DuringWaitForShoot(Event)
Set ReturnEvent equal to Event
int WaitTime
if EventType is ES_ENTRY
call shootBall
call ES_Timer_SetTimer, Timer 2, 300 ms
call ES_Timer_StartTimer, Timer 2
else if (EventType is ES_EXIT)
// do nothing
return(ReturnEvent)
// public functions:
void aimTarget (void)
static int TargetID
coord TargetCoord
coord OurBot
double angle
set int color equal to getColor()
set int botangle to
set TargetID equal to getNextTarget
set TargetCoord equal to queryShip(TargetID)
set OurBot equal to queryShip(15)
//Calculate angle in degrees
set angle equal to atan(((double)(OurBot.y-TargetCoord.y))/((double)(OurBot.x-TargetCoord.x)))*180/3.14;
if color is 1
set botangle = -270+OurBot.r*45/32;
else
set botangle = -90+OurBot.r*45/32;
call Servo_SetAngle(AimServo, 90+angle+botangle);
call ES_Timer_SetTimer, Timer 2, 500 ms
call ES_Timer_StartTimer, Timer 2
void setShooterSpeed( void)
double Current_BatVoltage
double ShootingDistance
set ShootingDistance equal to (double)sqrt((pow((OurBot.y-TargetCoord.y),2) + pow((OurBot.x-TargetCoord.x),2))); //Max should be about 190
set Current_BatVoltage equal to (double)ADS12_ReadADPin(4);
set ShooterMotorPWM equal to (CAL_BAT_VOLTAGE/Current_BatVoltage)*(ShootingDistance*SHOOTER_DISTANCE_SCALE_M + SHOOTER_DISTANCE_SCALE_B);
call SetDuty(ShooterMotorPWM,1);
void shootBall( void)
call Servo_SetAngle(LoadServo, 112)
void resetLoad( void)
call Servo_SetAngle(LoadServo, 55)
void resetCurrentSector( void)
set currentSector equal to 1