MovementSM (Pseudo code)
Global Variables –
MovementState_t CurrentState
coord TargetCoord, OurBot
int count, tape, angleInterval, dir
double angle
StartMovementSM (Takes x, y, r, angleint, and direction)
angleint sets tolerances on angle for the bot to be satisfied with the orientation, and direction sets moving forward to the target or reversing.
Set TargetCoord.x, .y, .r to x, y, and r
Set count to 0, and dir to direction, and angleInterval to angleint
Set CurrentState to WaitForUncloakB, and run ES_ENTRY in MovementSM
RunMovementSM (This will process events through the state machine to do movements)
For each state, run its corresponding during function
If GameEnd is passed in,
Set NextState to End, and set changing state variables
State machine –
State WaitForUncloakB
If timer 3 timed out, updateFAC
If UpdateFinish, set NextState to TurnAtTarget and set changing state variables
State TurnAtTarget
If timer 1 timed out, updateFAC
If UpdateFinish,
set OurBot to queryShip(OurShipNumber (15))
find rotation difference by calculating target angle – newangle
if the rotation difference is less than the interval allowed
proceed to movingforward
else
call rotateto with target angle
State MovingForward
If timer 4 timed out,
If pause (which is a flag to keep a pause before updating fac) is 0,
Set timer 4 again to 150ms, and set pause to 1
Else
updateFAC, set pause to 0
If UpdateFinish and count is 1, (count is a flag to keep track of how many times we’ve tried to move, so for each time we call movementSM we do the process twice for precision)
proceed to Aligning
else if count is 0,
set count to 1 and go back to TurnAtTarget
State Aligning
If timer 1 timed out,
If pause is 0, set timer 1 to 100ms and set pause to 1
If pause is 1, updateFAC and set pause to 0
If UpdateFinish,
If OurBot.r is within the TargetCoord.r’s interval
Proceed to End and send MovementFinished to MasterSM
Else
RotateTo TargetCoord.r
DuringWaitForUncloakB
On Entry, set timer 3 to 300ms
DuringTurnAtTarget
On Entry, set OurBot to queryShip(OurShipNumber (15))
Calculate the angle between current bot position and target position
Calculate which angle to turn to based on the relative x coordinate, and change angle to point in the correct direction (due to arctangent return values)
Call RotateTo with the angle
DuringMovingForward
On Entry, calculate the distance by taking the square root of difference of x squared added to the difference of y squared.
If distance is 0, set it to 1 so we don’t pass in a 0 distance value.
If count is 1, which means we’re on the first attempt, multiply distance by 1.3
If dir is 1 (Reverse), Drive reverse with timer 4 set to distance*distanceScale*reverseScale
If dir is 0 (Forward), Drive forward with timer 4 set to distance*distanceScale
DuringAligning
On Entry, call RotateTo with TargetCoord.r
RotateTo
Takes an integer angle as the target angle
Set OurBot to queryShip(OurShipNumber (15)), and calculate the rotation difference between the inputted angle and OurBot.r in degrees
Depending on the degree amount, rotateCW or rotateCCW the correct angle.
RotateToExact
Does the same thing as RotateTo except calls rotateC(C)WExact for different scale
MovementState_t CurrentState
coord TargetCoord, OurBot
int count, tape, angleInterval, dir
double angle
StartMovementSM (Takes x, y, r, angleint, and direction)
angleint sets tolerances on angle for the bot to be satisfied with the orientation, and direction sets moving forward to the target or reversing.
Set TargetCoord.x, .y, .r to x, y, and r
Set count to 0, and dir to direction, and angleInterval to angleint
Set CurrentState to WaitForUncloakB, and run ES_ENTRY in MovementSM
RunMovementSM (This will process events through the state machine to do movements)
For each state, run its corresponding during function
If GameEnd is passed in,
Set NextState to End, and set changing state variables
State machine –
State WaitForUncloakB
If timer 3 timed out, updateFAC
If UpdateFinish, set NextState to TurnAtTarget and set changing state variables
State TurnAtTarget
If timer 1 timed out, updateFAC
If UpdateFinish,
set OurBot to queryShip(OurShipNumber (15))
find rotation difference by calculating target angle – newangle
if the rotation difference is less than the interval allowed
proceed to movingforward
else
call rotateto with target angle
State MovingForward
If timer 4 timed out,
If pause (which is a flag to keep a pause before updating fac) is 0,
Set timer 4 again to 150ms, and set pause to 1
Else
updateFAC, set pause to 0
If UpdateFinish and count is 1, (count is a flag to keep track of how many times we’ve tried to move, so for each time we call movementSM we do the process twice for precision)
proceed to Aligning
else if count is 0,
set count to 1 and go back to TurnAtTarget
State Aligning
If timer 1 timed out,
If pause is 0, set timer 1 to 100ms and set pause to 1
If pause is 1, updateFAC and set pause to 0
If UpdateFinish,
If OurBot.r is within the TargetCoord.r’s interval
Proceed to End and send MovementFinished to MasterSM
Else
RotateTo TargetCoord.r
DuringWaitForUncloakB
On Entry, set timer 3 to 300ms
DuringTurnAtTarget
On Entry, set OurBot to queryShip(OurShipNumber (15))
Calculate the angle between current bot position and target position
Calculate which angle to turn to based on the relative x coordinate, and change angle to point in the correct direction (due to arctangent return values)
Call RotateTo with the angle
DuringMovingForward
On Entry, calculate the distance by taking the square root of difference of x squared added to the difference of y squared.
If distance is 0, set it to 1 so we don’t pass in a 0 distance value.
If count is 1, which means we’re on the first attempt, multiply distance by 1.3
If dir is 1 (Reverse), Drive reverse with timer 4 set to distance*distanceScale*reverseScale
If dir is 0 (Forward), Drive forward with timer 4 set to distance*distanceScale
DuringAligning
On Entry, call RotateTo with TargetCoord.r
RotateTo
Takes an integer angle as the target angle
Set OurBot to queryShip(OurShipNumber (15)), and calculate the rotation difference between the inputted angle and OurBot.r in degrees
Depending on the degree amount, rotateCW or rotateCCW the correct angle.
RotateToExact
Does the same thing as RotateTo except calls rotateC(C)WExact for different scale