ShootPowerSM (Pseudo code)
Structure coord
Has x, y, r, active, sector (all ints)
Module Variables -
static PowerState_t CurrentState
static coord TargetCoord
static coord OurBot
static int angleForPower
static int servo
static coord PowerStation
static int SweepCount
RunShootPowerSM
initialize and set unsigned char MakeTransitition to False
initialize and set ShootingState_t NextState equal to CurrentState
set ES_Event EntryEventKind equal to { ES_ENTRY, 0 }// default to normal entry to new state
ES_Event ReturnEvent equal to CurrentEvent // assume we are not consuming event
ES_Event currEvent
int angle
if CurrentEvent.EventType is GameEnd
Make NextState Ended
Set MakeTransition to True, Make EntryEventKind.EventType ES_ENTRY
switch CurrentState
case SpinForAlign
Call DuringSpinForAlign, set CurrentEvent equal to returned event
if CurrentEvent is Aligned
call stop() // stop turning
set NextState to WaitForLoader
set MakeTransition to True, Make EntryEventKind.EventType ES_ENTRY
case WaitForServo
Call DuringWaitForServo, set CurrentEvent equal to returned event
if CurrentEvent is ES_TIMEOUT and EventParam is 5
set NextState to WaitForLoader
set MakeTransition to True, Make EntryEventKind.EventType ES_ENTRY
case WaitForLoader
Call DuringWaitForLoader , set CurrentEvent equal to returned event
if CurrentEvent is ES_TIMEOUT and EventParam is 2
set NextState to Shooting
set MakeTransition to True, Make EntryEventKind.EventType ES_ENTRY
case Shooting
Call DuringShooting , set CurrentEvent equal to returned event
if CurrentEvent is ES_TIMEOUT and EventParam is 2
if CurrentEvent is not ES_NO_EVENT
call resetLoad()
call changeAmmot(-1)
set currEvent.EventType equal to BallFired
PostMasterSM(currEvent)
set NextState to EndState
set MakeTransition to True, Make EntryEventKind.EventType ES_ENTRY
case EndState
// do nothing
StartShootPowerSM(CurrentEvent)
Set LocalEvent equal to CurrentEvent
if ES_ENTRY_HISTORY is not equal to CurrentEvent.EventType
if angleForPower is equal to 0
set CurrentState to SpinForAlign
else
set CurrentState to SpinForAlign
RunShootTargetSM(CurrentEvent)
QueryShootTargetSM
return CurrentState
// Private Functions:
DuringSpinForAlign
ES_Event ReturnEvent = Event; // assmes no re-mapping or comsumption
initialize and set static int updown equal to 1
inititalize and set static int flag equal to 0
if Event.EventType equals ES_ENTRY
call UncloakBot()
set servo equal to 55
call Servo_SetAngle(AimServo, servo)
call ES_Timer_SetTimer, Timer 5, 800 ms
call ES_Timer_StartTimer, Timer 5
call updateFAC()
else if Event.EventType equals ES_EXIT
set OurBot equal to queryShip(15)
call setShooterSpeed_PowerSM
set SweepCount equal to 0
else if Event.EventType equals ES_TIMEOUT and Event.EventParam equals 1 // finished rotating
if flag equals 0 //Wait for 600 ms before updating the FAC, to make sure the coordinates are correct
call ES_Timer_SetTimer, Timer 1, 600 ms
call ES_Timer_StartTimer, Timer 1
set flag equal to 1
else
call updateFAC()
set flag equal to 0
else if Event.EventType equals UpdateFinish //after the wait and update, get our coordinates and request align from event checkers
set OurBot equal to queryShip(15)
if SweepCount is not 0
call requestAlign() // turn IR receiver interrupt on
else if Event.EventType equals ES_TIMEOUT and Event.EventParam equals 5
if servo equals 55
call requestAlign() // turn IR receiver interrupt on
sweep servo back and forth
if updown equals 1
call Servo_SetAngle(AimServo, ++servo)
if (servo >= 125)
set updown equal to 0
else // go back the other direction
call Servo_SetAngle(AimServo, --servo);
if servo is less than or equal to 55)
set updown = 1;
add 1 to SweepCount to keep track of when to rotate actual bot if we can't find the beacon in the current sector
if SweepCount equals 2
call RotateCW(45, 1)
turn interrupt off while rotating bot
if SweepCount equals 4
call RotateCCW(90, 1)
turn interrupt off while rotating bot
if SweepCount equals 6
set SweepCount equal to 0
call ES_Timer_SetTimer, Timer 5, 20 ms
call ES_Timer_StartTimer, Timer 5
else if Event.EventType equals Aligned
if updown equals 1
add 5 to servo
call Servo_SetAngle(AimServo, servo)
else
add 5 to servo
call Servo_SetAngle(AimServo, servo);
return(ReturnEvent);
DuringWaitForServo(Event)
Set ReturnEvent equal to Event
int WaitTime
if EventType is ES_ENTRY
call ServoSetAngle(AimServo, angleForPower)
call ES_Timer_SetTimer, Timer 5, 1000 ms
call ES_Timer_StartTimer, Timer 5
else if (EventType is ES_EXIT)
// do nothing
return(ReturnEvent)
DuringWaitForLoader(Event)
Set ReturnEvent equal to Event
int WaitTime
if EventType is ES_ENTRY
call loadShooter
call ES_Timer_SetTimer, Timer 2, 3000 ms // long timer for increased ramp up time for longer distance
call ES_Timer_StartTimer, Timer 2
else if (EventType is ES_EXIT)
// do nothing
return(ReturnEvent)
DuringShooting(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 setShooterSpeed_PowerSM( void)
double Current_BatVoltage
double ShootingDistance
double PowerStationDistance
int ShooterAngle
int botangle
if getColor is 2
set PowerStation.x equal to 0
else if getColor is 1
set PowerStation.y equal to 255
if getColor is 1
set botangle equal to 270-OurBot.r*45/32
set ShooterAngle equal to servo-90
set PowerStation.y equal to OurBot.y - (OurBot.x - PowerStation.x)*tan((ShooterAngle + botangle)*3.14/180)
else if getColor is 2
set botangle equal to 90- OurBot.r*45/32
set ShooterAngle equal toservo-90
set PowerStation.y equal to OurBot.y - (OurBot.x - PowerStation.x)*tan((ShooterAngle + botangle)*3.14/180)
if(PowerStation.y > 255)
set PowerStation.y equal to 255
else if (PowerStation.y <0)
set PowerStation.y equal to 0
set PowerStationDistance equal to (double)sqrt((pow((OurBot.y-PowerStation.y),2) + pow((OurBot.x-PowerStation.x),2)))
set Current_BatVoltage equal to (double)ADS12_ReadADPin(4)
set ShooterMotorPWM equal to (CAL_BAT_VOLTAGE/Current_BatVoltage)*(PowerStationDistance*SHOOTER_DISTANCE_SCALE_M + SHOOTER_DISTANCE_SCALE_B)
call SetDuty(ShooterMotorPWM,1)
Has x, y, r, active, sector (all ints)
Module Variables -
static PowerState_t CurrentState
static coord TargetCoord
static coord OurBot
static int angleForPower
static int servo
static coord PowerStation
static int SweepCount
RunShootPowerSM
initialize and set unsigned char MakeTransitition to False
initialize and set ShootingState_t NextState equal to CurrentState
set ES_Event EntryEventKind equal to { ES_ENTRY, 0 }// default to normal entry to new state
ES_Event ReturnEvent equal to CurrentEvent // assume we are not consuming event
ES_Event currEvent
int angle
if CurrentEvent.EventType is GameEnd
Make NextState Ended
Set MakeTransition to True, Make EntryEventKind.EventType ES_ENTRY
switch CurrentState
case SpinForAlign
Call DuringSpinForAlign, set CurrentEvent equal to returned event
if CurrentEvent is Aligned
call stop() // stop turning
set NextState to WaitForLoader
set MakeTransition to True, Make EntryEventKind.EventType ES_ENTRY
case WaitForServo
Call DuringWaitForServo, set CurrentEvent equal to returned event
if CurrentEvent is ES_TIMEOUT and EventParam is 5
set NextState to WaitForLoader
set MakeTransition to True, Make EntryEventKind.EventType ES_ENTRY
case WaitForLoader
Call DuringWaitForLoader , set CurrentEvent equal to returned event
if CurrentEvent is ES_TIMEOUT and EventParam is 2
set NextState to Shooting
set MakeTransition to True, Make EntryEventKind.EventType ES_ENTRY
case Shooting
Call DuringShooting , set CurrentEvent equal to returned event
if CurrentEvent is ES_TIMEOUT and EventParam is 2
if CurrentEvent is not ES_NO_EVENT
call resetLoad()
call changeAmmot(-1)
set currEvent.EventType equal to BallFired
PostMasterSM(currEvent)
set NextState to EndState
set MakeTransition to True, Make EntryEventKind.EventType ES_ENTRY
case EndState
// do nothing
StartShootPowerSM(CurrentEvent)
Set LocalEvent equal to CurrentEvent
if ES_ENTRY_HISTORY is not equal to CurrentEvent.EventType
if angleForPower is equal to 0
set CurrentState to SpinForAlign
else
set CurrentState to SpinForAlign
RunShootTargetSM(CurrentEvent)
QueryShootTargetSM
return CurrentState
// Private Functions:
DuringSpinForAlign
ES_Event ReturnEvent = Event; // assmes no re-mapping or comsumption
initialize and set static int updown equal to 1
inititalize and set static int flag equal to 0
if Event.EventType equals ES_ENTRY
call UncloakBot()
set servo equal to 55
call Servo_SetAngle(AimServo, servo)
call ES_Timer_SetTimer, Timer 5, 800 ms
call ES_Timer_StartTimer, Timer 5
call updateFAC()
else if Event.EventType equals ES_EXIT
set OurBot equal to queryShip(15)
call setShooterSpeed_PowerSM
set SweepCount equal to 0
else if Event.EventType equals ES_TIMEOUT and Event.EventParam equals 1 // finished rotating
if flag equals 0 //Wait for 600 ms before updating the FAC, to make sure the coordinates are correct
call ES_Timer_SetTimer, Timer 1, 600 ms
call ES_Timer_StartTimer, Timer 1
set flag equal to 1
else
call updateFAC()
set flag equal to 0
else if Event.EventType equals UpdateFinish //after the wait and update, get our coordinates and request align from event checkers
set OurBot equal to queryShip(15)
if SweepCount is not 0
call requestAlign() // turn IR receiver interrupt on
else if Event.EventType equals ES_TIMEOUT and Event.EventParam equals 5
if servo equals 55
call requestAlign() // turn IR receiver interrupt on
sweep servo back and forth
if updown equals 1
call Servo_SetAngle(AimServo, ++servo)
if (servo >= 125)
set updown equal to 0
else // go back the other direction
call Servo_SetAngle(AimServo, --servo);
if servo is less than or equal to 55)
set updown = 1;
add 1 to SweepCount to keep track of when to rotate actual bot if we can't find the beacon in the current sector
if SweepCount equals 2
call RotateCW(45, 1)
turn interrupt off while rotating bot
if SweepCount equals 4
call RotateCCW(90, 1)
turn interrupt off while rotating bot
if SweepCount equals 6
set SweepCount equal to 0
call ES_Timer_SetTimer, Timer 5, 20 ms
call ES_Timer_StartTimer, Timer 5
else if Event.EventType equals Aligned
if updown equals 1
add 5 to servo
call Servo_SetAngle(AimServo, servo)
else
add 5 to servo
call Servo_SetAngle(AimServo, servo);
return(ReturnEvent);
DuringWaitForServo(Event)
Set ReturnEvent equal to Event
int WaitTime
if EventType is ES_ENTRY
call ServoSetAngle(AimServo, angleForPower)
call ES_Timer_SetTimer, Timer 5, 1000 ms
call ES_Timer_StartTimer, Timer 5
else if (EventType is ES_EXIT)
// do nothing
return(ReturnEvent)
DuringWaitForLoader(Event)
Set ReturnEvent equal to Event
int WaitTime
if EventType is ES_ENTRY
call loadShooter
call ES_Timer_SetTimer, Timer 2, 3000 ms // long timer for increased ramp up time for longer distance
call ES_Timer_StartTimer, Timer 2
else if (EventType is ES_EXIT)
// do nothing
return(ReturnEvent)
DuringShooting(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 setShooterSpeed_PowerSM( void)
double Current_BatVoltage
double ShootingDistance
double PowerStationDistance
int ShooterAngle
int botangle
if getColor is 2
set PowerStation.x equal to 0
else if getColor is 1
set PowerStation.y equal to 255
if getColor is 1
set botangle equal to 270-OurBot.r*45/32
set ShooterAngle equal to servo-90
set PowerStation.y equal to OurBot.y - (OurBot.x - PowerStation.x)*tan((ShooterAngle + botangle)*3.14/180)
else if getColor is 2
set botangle equal to 90- OurBot.r*45/32
set ShooterAngle equal toservo-90
set PowerStation.y equal to OurBot.y - (OurBot.x - PowerStation.x)*tan((ShooterAngle + botangle)*3.14/180)
if(PowerStation.y > 255)
set PowerStation.y equal to 255
else if (PowerStation.y <0)
set PowerStation.y equal to 0
set PowerStationDistance equal to (double)sqrt((pow((OurBot.y-PowerStation.y),2) + pow((OurBot.x-PowerStation.x),2)))
set Current_BatVoltage equal to (double)ADS12_ReadADPin(4)
set ShooterMotorPWM equal to (CAL_BAT_VOLTAGE/Current_BatVoltage)*(PowerStationDistance*SHOOTER_DISTANCE_SCALE_M + SHOOTER_DISTANCE_SCALE_B)
call SetDuty(ShooterMotorPWM,1)