Driving State Machine Pseudo-Code /**************************************************************************** Function RunDrivingSM Notes uses nested switch/case to implement the machine. ****************************************************************************/ { Declare local variable ReturnEvent assume no errors switch based on current state { If current state is initial Psedudo State only respond to EF_Init { now put the machine into the actual initial state Start with robot stationary Initialize 3 feet to right Initialize horizontally aligned with center Query the starting orientation from gameplay state machine } Break on case InitDrivingPState If current state is Waiting Base response on event type { If event is Drive forward { Query target x,y positions from driver state machine Calculate orinetation necessary to point towards target if targetOrientation < 0 B/c atan2 returns a value between -180 and 180 { Add 360 to get value between 0 and 360 } Reset lastDistanceIncrement to really high number UpdateState to ReorientingToDriveForward } Break on Drive Forward If event is drive reverse { Query target x,y positions from driver state machine Calculate orientation necessary to point away from target if targetOrientation < 0 B/c atan2 returns a value between -180 and 180 { Add 360 to get value between 0 and 360 } Reset lastDistanceIncrement to really high number UpdateState to ReorientingToDriveReverse } Break from drive Reverse If event is drive clockwise arc { Calculate angle of robot position along radial circle in arena Query target radius from gameplaySM calculateOrientationForCWArc Set startingArcOrientation to targetOrientation Set endingArcOrientation = targetOrientation - CircleThreshold Make sure value is between 0 and 360, adjust if necessary UpdateState to ReorientingToDriveCWArc } Break from drive clockwise arc If event is drive CounterClockwiseArc { Calculate angle of robot position along radial circle in arena Query target arc radius from gameplaySM calculateOrientationForCCWArc Set startingArcOrientation to targetOrientation Set endingArcOrientation = targetOrientation - CircleThreshold Make sure value is between 0 and 360, adjust if necessary UpdateState to ReorientingToDriveCCWArc } Break from drive counterclockwise arc If event is reorient robot { Get target orientation from driver UpdateState to reorienting } Break from reorient robot If event is change radius { Query x,y positions, target radius from gameplaySM Calculate current radius Scale target x,y positions by target radius over current radius Calculate target orientation to point towards target x,y if targetOrientation < 0 B/c atan2 returns a value between -180 and 180 { Add 360 to get value between 0 and 360 } Reset last distance increment to really high number UpdateState ReorientingToDriveForward } Break from change radius If event is spin CW indefinitely { Spin about left wheel to bump both back sensors Set left desired RPM to zero Set right desired RPM to maxRPM Stay in waiting state } break; If event is spin CCW indefinitely { Spin about right wheel to bump both back sensors Set right desired RPM to zero Set left desired RPM to maxRPM Stay in waiting state } break; If event is align to beacon { Read in the target beacon from the event parameter Query desired quadrant from game play state machine and calculate corresponding orientation to that beacon calculateOrientationError If Negative orientation error then too far counter-clockwise, turn cw { CurrentState = AligningToBeaconCW; //UpdateState } else Positive orientation error = too far clockwise, turn ccw { CurrentState = AligningToBeaconCCW; //Update State } } break; If event is reverse to beacon { Read in the target beacon from the event parameter UpdateState to ReversingToBeacon } break; If event is stop robot { stopTheRobot(), Stay in waiting state } break; } End switch on event type Break on case waitingState If current state is reorienting to drive forward Base response on event type { case DrivingUpdate : { On timed interval, adjust desired motor RPMs to turn in correct directions calculateOrientationError(); setReorientationRPMs(); if close enough to target orientation { stopTheRobot(); UpdateState to moving forward } } break; If any external event posted to drivingSM { Stop the robot Send back to waiting state to process new event Repost event so it will get processed in waiting state } Break from default }End switch on event type Break on case ReorientingToDriveForward If current state is reorientingToDriveReverse Base response on event type { case DrivingUpdate : { On timed interval, adjust desired motor RPMs to turn in correct directions calculateOrientationError(); setReorientationRPMs(); if close enough to target orientation { stopTheRobot(); UpdateState to moving reverse } } break; If any external event posted to drivingSM { Stop the robot Send back to waiting state to process new event Repost event so it will get processed in waiting state } Break from default }End switch on event type Break on case ReorientingToDriveReverse If current state is reorienting Base response on event type { case DrivingUpdate : { On timed interval, adjust desired motor RPMs to turn in correct directions calculateOrientationError(); setReorientationRPMs(); if close enough to target orientation { Post event targetPositionReached to GamePlayHSM stopTheRobot() UpdateState to DriveWaitingState } } break; If any external event posted to drivingSM { Stop the robot Send back to waiting state to process new event Repost event so it will get processed in waiting state } Break from default }End switch on event type Break on case Reorienting If current state is reorienting to drive cw arc Base response on event type { case DrivingUpdate : { On timed interval, adjust desired motor RPMs to turn in correct directions calculateOrientationError(); setReorientationRPMs(); if close enough to target orientation { stopTheRobot(); UpdateState to moving CW arc } } break; If any external event posted to drivingSM { Stop the robot Send back to waiting state to process new event Repost event so it will get processed in waiting state } Break from default }End switch on event type Break on case ReorientingToDriveCWArc If current state is reorienting to drive ccw arc Base response on event type { case DrivingUpdate : { On timed interval, adjust desired motor RPMs to turn in correct directions calculateOrientationError(); setReorientationRPMs(); if close enough to target orientation { stopTheRobot(); UpdateState to moving CCW arc } } break; If any external event posted to drivingSM { Stop the robot Send back to waiting state to process new event Repost event so it will get processed in waiting state } Break from default }End switch on event type Break on case ReorientingToDriveCCWArc If current state is movingForward Base response on event type { case DrivingUpdate : { //On timed interval, adjust desired motor RPMs to turn in correct directions calculateOrientationError(); calculateDistanceToTarget(); setDrivingForwardRPMs(); If the robot is close enough to the target { Post TargetPositionReached event to GamePlayHSM stopTheRobot(); UpdateState to DriveWaitingState } Update lastDistanceIncrement to check that robot is still moving towards target If we are now moving away from target { stopTheRobot(); Repost driveForward event to drivingSM Update state to waiting } } break; If any external event posted to drivingSM { Stop the robot Send back to waiting state to process new event Repost event so it will get processed in waiting state } Break from default }End switch on event type Break on case MovingForward If current state is moving reverse Base response on event type { case DrivingUpdate : { //On timed interval, adjust desired motor RPMs to turn in correct directions calculateOrientationError(); calculateDistanceToTarget(); setDrivingReverseRPMs(); Stop the robot if the robot is close enough to target { Post TargetPositionReached event to GamePlayHSM stopTheRobot(); UpdateState to waiting } Update lastDistanceIncrement to check that robot is still moving towards target If we are now moving away from target { stopTheRobot(); Repost driveReverse event to drivingSM Update state to waiting } } break; If any external event posted to drivingSM { Stop the robot Send back to waiting state to process new event Repost event so it will get processed in waiting state } Break from default }End switch on event type Break on case MovingReverse If current state is moving CWArc Base response on event type { case DrivingUpdate : { //On timed interval, adjust desired motor RPMs to turn in correct directions calculateOrientationForCWArc(); calculateOrientationError(); setDrivingForwardRPMs(); if we’ve passed the ending orientation of the arc { Post event EV_360_DEGREE_DRIVEN to GamePlayHSM stopTheRobot(); CurrentState = DriveWaitingState; //UpdateState } } break; If any external event posted to drivingSM { Stop the robot Send back to waiting state to process new event Repost event so it will get processed in waiting state } Break from default }End switch on event type Break on case MovingCWArc If current state is moving CCWArc Base response on event type { case DrivingUpdate : { //On timed interval, adjust desired motor RPMs to turn in correct directions calculateOrientationForCCWArc(); calculateOrientationError(); setDrivingForwardRPMs(); if we’ve passed the ending orientation of the arc { Post event EV_360_DEGREE_DRIVEN to GamePlayHSM stopTheRobot(); CurrentState = DriveWaitingState; //UpdateState } } break; If any external event posted to drivingSM { Stop the robot Send back to waiting state to process new event Repost event so it will get processed in waiting state } Break from default }End switch on event type break; //Break on case MovingCCWArc If current state is AligningToBeaconCW Base response on event type { case DrivingUpdate : { Query Beacon sensed state (left, right, both, neither) from beacon service Query Beacon number sensed from beacon service If the beacon we are sensing is the target beacon { If at target orientation, both beacons sensed { Post TargetPositionReached event to GamePlaySM stopTheRobot(); UpdateState to waiting state } else if BeaconsSensed == RightOnly, we’re getting close to target orientation { Keep turning at slightly slower speed Keep track of last beacon sensed in case we lose both } else if BeaconsSensed == LeftOnly, then Robot has turned too far { Turn back at slightly slower speed Keep track of last beacon sensed in case we lose both Update state to AligningToBeaconCCW } } else { Turn CW at max speed } } Break from driving update If any external event posted to drivingSM { Stop the robot Send back to waiting state to process new event Repost event so it will get processed in waiting state } Break from default }End switch on event type Break on case AligningToBeaconCW If current state is AligningToBeaconCCW Base response on event type { case DrivingUpdate : { Query Beacon sensed state (left, right, both, neither) from beacon service Query Beacon number sensed from beacon service If the beacon we are sensing is the target beacon { If at target orientation, both beacons sensed { Post TargetPositionReached event to GamePlaySM stopTheRobot(); UpdateState to waiting state } else if BeaconsSensed == LeftOnly, getting close to target orientation { Keep turning at slightly slower speed Keep track of last beacon sensed in case we lose both } else if BeaconsSensed == RightOnly, then Robot has turned too far { Turn back at slightly slower speed Keep track of last beacon sensed in case we lose both Update state to AligningToBeaconCW } } else { Turn CCW at max speed } } Break from driving update If any external event posted to drivingSM { Stop the robot Send back to waiting state to process new event Repost event so it will get processed in waiting state } Break from default }End switch on event type Break on case AligningToBeaconCCW If current state is ReversingToBeacon Base response on event type { case DrivingUpdate : { Query Beacon sensed state (left, right, both, neither) from beacon service Query Beacon number sensed from beacon service If the beacon we are sensing is the target beacon { if BeaconsSensed == BothSensed, we’re still at target orientation { Keep reversing at full speed } else if BeaconsSensed == LeftOnly, slightly off target orientation cw { Reduce magnitude of right wheel reverse speed to turn ccw Keep track of last beacon sensed in case we lose both } else if BeaconsSensed == RightOnly, slightly off target orientation ccw { Reduce magnitude of left wheel reverse to turn cw Keep track of last beacon sensed in case we lose both } } else { if LastBeaconSensed == LeftBeacon, we probably are turned too far ccw { Reduce magnitude of left wheel reverse to turn cw } else // LastBeaconSensed == RightBeacon, we probably are turned too far ccw { Reduce magnitude of right wheel reverse to turn ccw } } } Break from driving update If any external event posted to drivingSM { Stop the robot Send back to waiting state to process new event Repost event so it will get processed in waiting state } Break from default }End switch on event type Break on case ReversingToBeacon } end switch on Current State return ReturnEvent; } /**************************************************************************** Function calculateOrientationError Description calculates orientation error between -180 and 180 degrees ****************************************************************************/ void calculateOrientationError(void) { Get orientation from navigationSM Calculate orientation error by subtraction //Positive orientation error = too far clockwise //Negative orientation error = too far counter-clockwise Error could be between -355 to 355, want between -180 to 180, if not in range then { Get value between -180 and 180 } } /**************************************************************************** Function calculateDistanceToTarget Description Query position from navigationSM and calculate distance to target ****************************************************************************/ void calculateDistanceToTarget(void) { Query x,y positions from navigationSM and calculate distance to target Do math to calculate distance to target } /**************************************************************************** Function calculateOrientationForArcs no parameters/returns calculate the desired orientaion for the robot to drive arc around center of arena ****************************************************************************/ void calculateOrientationForCWArc(void) { Use atan2 to calculate angle perpendicular to center as target orientation Query x,y positions from navigationSM Calculate radiusError from targetArcRadius Turn cw into circle if radius too big, ccw out of circle if too small if targetOrientation < 0 /B/c atan2 returns a value between -180 and 180 { get value between 0 and 360 } Subtract 90 from target to orient CW along tangent to circle if target below zero now { get value between 0 and 360 } } void calculateOrientationForCCWArc(void) { Use atan2 to calculate angle perpendicular to center as target orientation Query x,y positions from navigationSM Calculate radiusError from targetArcRadius Turn cw into circle if radius too big, ccw out of circle if too small if targetOrientation < 0 /B/c atan2 returns a value between -180 and 180 { get value between 0 and 360 } Add 90 from target to orient CCW along tangent to circle if target above 360 now { get value between 0 and 360 } } /**************************************************************************** Function setReorientationRPMs ****************************************************************************/ void setReorientationRPMs(void) { //Positive orientation error = too far clockwise, turn ccw //Negative orientation error = too far counter-clockwise, turn cw Set motors in opposite directions at maxRPM, or scale maxRPM if within threshold to slow reorienting Limit magnitude of desired RPMs to max RPM } /**************************************************************************** Function setDrivingForwardRPMs ****************************************************************************/ void setDrivingForwardRPMs(void) { //Positive orientation error = too far clockwise, turn ccw //Negative orientation error = too far counter-clockwise, turn cw Set motors in opposite directions at maxRPM, or scale maxRPM if within not oriented straight towards target Limit magnitude of desired RPMs to max RPM } /**************************************************************************** Function setDrivingReverseRPMs ****************************************************************************/ void setDrivingReverseRPMs(void) { //Positive orientation error = too far clockwise, slow right wheel //Negative orientation error = too far counter-clockwise, slow left wheel Set motors in reverse at maxRPM, slow one wheel if some reorientation needed Limit magnitude of desired RPMs to max RPM }