LEADER PIC PSEUDOCODE

TopHSM.c
FUNCTION InitTopHSM ( uint8_t Priority )
InitDCMotor(true)
InitEventOverSPI(true)
InitTimer5()
InitAdjustButtons()

PortSetup_ConfigureDigitalInputs(_Port_B, _Pin_15)
PortSetup_ConfigureDigitalInputs(_Port_B, _Pin_12)

ThisEvent.EventType = ES_ENTRY
StartTopHSM( ThisEvent )

return true

FUNCTION PostTopHSM( ES_Event_t ThisEvent )

return ES_PostToService( MyPriority, ThisEvent)

FUNCTION RunTopHSM( ES_Event_t CurrentEvent )

MakeTransition = false
NextState = CurrentState
EntryEventKind = { ES_ENTRY, 0 }
ReturnEvent = CurrentEvent

SWITCH(CurrentState)
CASE IDLE:
CurrentEvent = DuringIdle(CurrentEvent)

IF (CurrentEvent.EventType != ES_NO_EVENT)
SWITCH(CurrentEvent.EventType)
CASE ES_TIMEOUT:
IF (CurrentEvent.EventParam == QUERY_FOLLOWER_TIMER)
ES_Event_t NewEvent
NewEvent.EventType = ES_NO_EVENT
PostEventOverSPI(NewEvent)
ES_Timer_InitTimer(QUERY_FOLLOWER_TIMER, follower_query_timer)
ENDIF
 END CASE

CASE ES_BUTTON_PRESS:
ES_Timer_StopTimer(QUERY_FOLLOWER_TIMER)
MakeTransition = true
NextState = CALIBRATION
END CASE

CASE_GAME_OVER
END SWITCH
ENDIF
END CASE

CASE CALIBRATION:
CurrentEvent = DuringCalibration(CurrentEvent)
IF (CurrentEvent.EventType != ES_NO_EVENT)
SWITCH(CurrentEvent.EventType)
CASE ES_FOUND_BEACON_B:
PostEventOverSPI(CurrentEvent)

END CASE
CASE ES_FOUND_BEACON_C:
PostEventOverSPI(CurrentEvent)
END CASE
CASE ES_FINISH:
EntryEventKind.EventType = ES_ENTRY
MakeTransition = true
NextState = GO_TO_BRANCH_ORIGIN
END CASE
CASE_GAME_OVER
END SWITCH
ENDIF
END CASE

CASE GO_TO_BRANCH_ORIGIN:
CurrentEvent = DuringGo2BranchOrigin(CurrentEvent)
IF (CurrentEvent.EventType != ES_NO_EVENT)
SWITCH(CurrentEvent.EventType)
CASE ES_FINISH:
setDesiredSpeed(LEFT_MOTOR, FORWARD, 0)
setDesiredSpeed(RIGHT_MOTOR, FORWARD, 0)
EntryEventKind.EventType = ES_ENTRY
MakeTransition = true
NextState = PUSH_COMMIT
END CASE
CASE_GAME_OVER
END SWITCH
ENDIF
END CASE

CASE PUSH_COMMIT:
 CurrentEvent = DuringPushCommit(CurrentEvent)
IF (CurrentEvent.EventType != ES_NO_EVENT)
SWITCH(CurrentEvent.EventType)
// change this to start btn click event
CASE ES_NEW_KEY:
IF (CurrentEvent.EventParam == 'z')
EntryEventKind.EventType = ES_ENTRY
MakeTransition = true
NextState = COME_BACK
ENDIF
END CASE

CASE ES_FINISH:
EntryEventKind.EventType = ES_ENTRY
MakeTransition = true
NextState = COME_BACK
END CASE
CASE_GAME_OVER
END SWITCH
ENDIF
END CASE

CASE COME_BACK:
CurrentEvent = DuringComeBack(CurrentEvent)
IF (CurrentEvent.EventType != ES_NO_EVENT)
SWITCH(CurrentEvent.EventType)
// change this to start btn click event
CASE ES_NEW_KEY:
IF (CurrentEvent.EventParam == 'x')
EntryEventKind.EventType = ES_ENTRY_HISTORY
MakeTransition = true
NextState = GO_TO_BRANCH_ORIGIN
ENDIF
END CASE

CASE ES_FINISH:
EntryEventKind.EventType = ES_ENTRY_HISTORY
MakeTransition = true
NextState = GO_TO_BRANCH_ORIGIN
END CASE
CASE_GAME_OVER
END SWITCH
ENDIF
END CASE
END SWITCH

IF (MakeTransition == true)
CurrentEvent.EventType = ES_EXIT
RunTopHSM(CurrentEvent)

CurrentState = NextState
 RunTopHSM(EntryEventKind)
ENDIF
return(ReturnEvent)

FUNCTION StartTopHSM (ES_Event_t CurrentEvent)

CurrentState = IDLE
RunTopHSM(CurrentEvent)

FUNCTION QueryTopHSM ( void )

return CurrentState

FUCNTION DuringIdle( ES_Event_t Event )

ReturnEvent = Event
IF ((Event.EventType == ES_ENTRY) ||
(Event.EventType == ES_ENTRY_HISTORY))
ES_Timer_InitTimer(QUERY_FOLLOWER_TIMER,follower_query_timer)
ENDIF
ELSE IF ( Event.EventType == ES_EXIT )

ES_Timer_StopTimer(QUERY_FOLLOWER_TIMER)
ENDIF

FUCNTION DuringCalibration( ES_Event_t Event)

ReturnEvent = Event // assme no re-mapping or comsumption

// process ES_ENTRY, ES_ENTRY_HISTORY & ES_EXIT events

IF ( (Event.EventType == ES_ENTRY) ||
(Event.EventType == ES_ENTRY_HISTORY) )
startGameTimer()
StartCalibrationSM(Event)
ENDIF
ELSE IF ( Event.EventType == ES_EXIT )
RunCalibrationSM(Event)

ENDIF
ELSE
RunCalibrationSM(Event)
ENDIF
return ReturnEvent

FUCNTION DuringGo2BranchOrigin( ES_Event_t Event)

ReturnEvent = Event // assme no re-mapping or comsumption

// process ES_ENTRY, ES_ENTRY_HISTORY & ES_EXIT events

IF ( (Event.EventType == ES_ENTRY) ||
 (Event.EventType == ES_ENTRY_HISTORY) )
StartGoToBranchOriginSM(Event)
ENDIF
ELSE IF ( Event.EventType == ES_EXIT )
RunGoToBranchOriginSM(Event)
ENDIF
ELSE
RunGoToBranchOriginSM(Event)
ENDIF
return ReturnEvent

static ES_Event_t DuringPushCommit(ES_Event_t Event)

ReturnEvent = Event

IF ( (Event.EventType == ES_ENTRY) ||
(Event.EventType == ES_ENTRY_HISTORY) )
StartPushCommitSM(Event)
ENDIF
ELSE IF (Event.EventType == ES_EXIT)
RunPushCommitSM(Event)
END IF

ELSE
RunPushCommitSM(Event)
END IF

return ReturnEvent

FUNCTION DuringComeBack(ES_Event_t Event)

ReturnEvent = Event

// process ES_ENTRY, ES_ENTRY_HISTORY & ES_EXIT events

IF ( (Event.EventType == ES_ENTRY) ||
(Event.EventType == ES_ENTRY_HISTORY) )
StartComeBackSM(Event)
END IF

ELSE IF ( Event.EventType == ES_EXIT )

RunComeBackSM(Event)
END IF
ELSE
RunComeBackSM(Event)
END IF
return ReturnEvent
CalibrationSM.c
RunCalibrationSM(CurrentEvent)
MakeTransition = false
NextState = CurrentState
EntryEventKind = { ES_ENTRY, 0 }
ReturnEvent = CurrentEvent

SWITCH CurrentState
CASE ROTATE_TO_ALIGN:
CurrentEvent = DuringRotateToAlign(CurrentEvent)
IF CurrentEvent.EventType != ES_NO_EVENT THEN
SWITCH CurrentEvent.EventType
CASE ES_NEW_KEY:
IF CurrentEvent.EventParam == 'f' THEN
NewEvent.EventType = ES_FOUND_BEACON
NewEvent.EventParam = BeaconB
PostTopHSM(NewEvent)
ENDIF
END CASE

CASE ES_FOUND_BEACON:
BeaconName = CurrentEvent.EventParam
IF CurrentEvent.EventParam == BeaconB OR CurrentEvent.EventParam ==
BeaconC THEN
NextState = BACK_UP
MakeTransition = true
SWITCH BeaconName
CASE BeaconB:
puts("Beacon B Detected\r\n")
BeaconEvent.EventType = ES_FOUND_BEACON_B
PostTopHSM(BeaconEvent)
END CASE

CASE BeaconC:
puts("Beacon C Detected\r\n")
BeaconEvent.EventType = ES_FOUND_BEACON_C
PostTopHSM(BeaconEvent)
END CASE
ENDIF
ENDIF
END CASE

ENDSWITCH

ENDIF
END CASE

CASE BACK_UP:
 CurrentEvent = DuringBackUp(CurrentEvent)
IF CurrentEvent.EventType != ES_NO_EVENT THEN
SWITCH CurrentEvent.EventType
CASE ES_DONE_BACK_UP:
NextState = STOP
MakeTransition = true
END CASE
ENDSWITCH
ENDIF
END CASE

CASE STOP:
IF ES_TIMEOUT == CurrentEvent.EventType AND CurrentEvent.EventParam ==
STOP_TIMER THEN
NextState = FORWARD_UNTIL_BEACON
MakeTransition = true
ENDIF
END CASE

CASE FORWARD_UNTIL_BEACON_FOUND:
CurrentEvent = DuringForwardUntilBeacon(CurrentEvent)
IF (CurrentEvent.EventType != ES_NO_EVENT) THEN
SWITCH (CurrentEvent.EventType)
CASE ES_NEW_KEY:
IF (CurrentEvent.EventParam == 'g') THEN
NewEvent.EventType = ES_FOUND_BEACON
NewEvent.EventParam = BeaconB
PostTopHSM(NewEvent)
ENDIF
END CASE
CASE ES_TRANSLATED:
NewEvent.EventType = ES_FINISH
PostTopHSM(NewEvent)
END CASE
END SWITCH
ENDIF
END CASE
END SWITCH
IF (MakeTransition == true)
CurrentEvent.EventType = ES_EXIT
RunCalibrationSM(CurrentEvent)

CurrentState = NextState

RunCalibrationSM(EntryEventKind)

ENDIF
return ReturnEvent
FUNCTION StartCalibrationSM( ES_Event_t CurrentEvent )
IF (ES_ENTRY_HISTORY != CurrentEvent.EventType)
CurrentState = ROTATE_TO_ALIGN
ENDIF
RunCalibrationSM(CurrentEvent)

FUNCTION QueryCalibrationSM ( void )

return CurrentState

FUNCTION DuringRotateToAlign(ES_Event_t Event)

ReturnEvent = Event
IF ( (Event.EventType == ES_ENTRY) ||
(Event.EventType == ES_ENTRY_HISTORY))
setDesiredSpeed(LEFT_MOTOR, FORWARD, rotate_speed)
setDesiredSpeed(RIGHT_MOTOR, BACKWARD, rotate_speed)
ENDIF
ELSE IF (Event.EventType == ES_EXIT)
setDesiredSpeed(LEFT_MOTOR, FORWARD, 0)
setDesiredSpeed(RIGHT_MOTOR, FORWARD, 0)
ENDIF

return ReturnEvent

FUNCTION DuringBackUp(ES_Event_t Event)

ReturnEvent = Event
IF ( (Event.EventType == ES_ENTRY) ||
(Event.EventType == ES_ENTRY_HISTORY))
setDesiredSpeed(LEFT_MOTOR, BACKWARD, backup_speed)
setDesiredSpeed(RIGHT_MOTOR, BACKWARD, backup_speed)
ENDIF
ELSE IF (Event.EventType == ES_EXIT)
setDesiredSpeed(LEFT_MOTOR, FORWARD, 0)
setDesiredSpeed(RIGHT_MOTOR, FORWARD, 0)
ES_Timer_InitTimer(STOP_TIMER, stop_ms)
ENDIF

return ReturnEvent

FUNCTION DuringForwardUntilBeacon(ES_Event_t Event)

ReturnEvent = Event
IF ( (Event.EventType == ES_ENTRY) ||
(Event.EventType == ES_ENTRY_HISTORY))
drive(FORWARD, a_bit_cm, forward_speed)
ENDIF

return ReturnEvent
GoToBranchOriginSM.c
FUNCTION RunGoToBranchOriginSM(CurrentEvent):
MakeTransition = false
NextState = CurrentState
EntryEventKind = { ES_ENTRY, 0 }
ReturnEvent = CurrentEvent
QueryEvent.EventType = ES_NO_EVENT

SWITCH(CurrentState)
CASE BRANCH_ONE:
CurrentEvent = DuringBranchOne(CurrentEvent)
IF(CurrentEvent.EventType != ES_NO_EVENT)
SWITCH(CurrentEvent.EventType)
CASE ES_TIMEOUT:
IF(CurrentEvent.EventParam == QUERY_TIMER)
PostEventOverSPI(QueryEvent)
ES_Timer_InitTimer(QUERY_TIMER, QueryTime)
ENDIF
END CASE

CASE ES_BUTTON_PRESS:
IF(CurrentEvent.EventParam == Branch1)
ES_Timer_StopTimer(QUERY_TIMER)
puts("\tGo to branch 1 from branch 1\r\n")
PrevState = BRANCH_ONE
NewEvent.EventType = ES_FINISH
PostTopHSM(NewEvent)
ELSE IF(CurrentEvent.EventParam == Branch2)
ES_Timer_StopTimer(QUERY_TIMER)
puts("\tGo to branch 2 from branch 1\r\n")
PrevState = BRANCH_ONE
MakeTransition = true
NextState = BRANCH_TWO
ELSE IF(CurrentEvent.EventParam == Branch3)
ES_Timer_StopTimer(QUERY_TIMER)
puts("\tGo to branch 3 from branch 1\r\n")
PrevState = BRANCH_ONE
MakeTransition = true
NextState = BRANCH_THREE
ENDIF
END CASE

CASE ES_REACHED_ONE:
NewEvent.EventType = ES_FINISH
PostTopHSM(NewEvent)
END CASE
END SWITCH
ENDIF
END CASE
CASE BRANCH_TWO:
CurrentEvent = DuringBranchTwo(CurrentEvent)
IF(CurrentEvent.EventType != ES_NO_EVENT)
SWITCH(CurrentEvent.EventType)
CASE ES_TIMEOUT:
IF(CurrentEvent.EventParam == QUERY_TIMER)
PostEventOverSPI(QueryEvent)
ES_Timer_InitTimer(QUERY_TIMER, QueryTime)
ENDIF
END CASE

CASE ES_BUTTON_PRESS:
IF(CurrentEvent.EventParam == Branch1)
ES_Timer_StopTimer(QUERY_TIMER)
puts("\tGo to branch 1 from branch 2\r\n")
PrevState = BRANCH_TWO
MakeTransition = true
NextState = BRANCH_ONE
ELSE IF(CurrentEvent.EventParam == Branch2)
ES_Timer_StopTimer(QUERY_TIMER)
puts("\tGo to branch 2 from branch 2\r\n")
PrevState = BRANCH_TWO
NewEvent.EventType = ES_FINISH
PostTopHSM(NewEvent)
ELSE IF(CurrentEvent.EventParam == Branch3)
ES_Timer_StopTimer(QUERY_TIMER)
puts("\tGo to branch 3 from branch 2\r\n")
PrevState = BRANCH_TWO
MakeTransition = true
NextState = BRANCH_THREE
ENDIF
END CASE

CASE ES_TRANSLATED:
NewEvent.EventType = ES_FINISH
PostTopHSM(NewEvent)
END CASE
END SWITCH
ENDIF
END CASE

CASE BRANCH_THREE:
CurrentEvent = DuringBranchThree(CurrentEvent)
IF(CurrentEvent.EventType != ES_NO_EVENT)
SWITCH(CurrentEvent.EventType)
CASE ES_TIMEOUT:
IF(CurrentEvent.EventParam == QUERY_TIMER)
PostEventOverSPI(QueryEvent)
ES_Timer_InitTimer(QUERY_TIMER, QueryTime)
ENDIF
 END CASE

CASE ES_BUTTON_PRESS:
IF(CurrentEvent.EventParam == Branch1)
ES_Timer_StopTimer(QUERY_TIMER)
puts("\tGo to branch 1 from branch 3\r\n")
PrevState = BRANCH_THREE
MakeTransition = true
NextState = BRANCH_ONE
ELSE IF(CurrentEvent.EventParam == Branch2)
ES_Timer_StopTimer(QUERY_TIMER)
puts("\tGo to branch 2 from branch 3\r\n")
PrevState = BRANCH_THREE
MakeTransition = true
NextState = BRANCH_TWO
ELSE IF(CurrentEvent.EventParam == Branch3)
ES_Timer_StopTimer(QUERY_TIMER)
puts("\tGo to branch 3 from branch 3\r\n")
PrevState = BRANCH_THREE
NewEvent.EventType = ES_FINISH
PostTopHSM(NewEvent)
ENDIF
END CASE

CASE ES_REACHED_THIRD:
NewEvent.EventType = ES_FINISH
PostTopHSM(NewEvent)
END CASE
END SWITCH
ENDIF
END CASE
END SWITCH
IF (MakeTransition == true)
CurrentEvent.EventType = ES_EXIT
RunGoToBranchOriginSM(CurrentEvent)

CurrentState = NextState

RunGoToBranchOriginSM(EntryEventKind)

ENDIF
return ReturnEvent

FUNCTION StartGoToBranchOriginSM( ES_Event_t CurrentEvent )

IF (ES_ENTRY_HISTORY != CurrentEvent.EventType)
CurrentState = BRANCH_ONE
PrevState = BRANCH_ONE
ENDIF
 ES_Timer_InitTimer(QUERY_TIMER, QueryTime)
RunGoToBranchOriginSM(CurrentEvent)

FUNCTION QueryGoToBranchOriginSM ( void )

return CurrentState

FUNCTION QueryGoToBranchOriginPrevSM (void)

returnPrevState

FUNCTION SetGoToBranchOriginPrevSM(Branch)
PrevState = Branch

FUNCTION DuringBranchOne(ES_Event_t Event)

ReturnEvent = Event
IF ( (Event.EventType == ES_ENTRY) ||
(Event.EventType == ES_ENTRY_HISTORY))
IF(CurrentState > PrevState){
setDesiredSpeed(LEFT_MOTOR, FORWARD, translation_speed)
setDesiredSpeed(RIGHT_MOTOR, FORWARD, translation_speed)
}
IF(CurrentState < PrevState)
{
setDesiredSpeed(LEFT_MOTOR, BACKWARD, translation_speed)
setDesiredSpeed(RIGHT_MOTOR, BACKWARD, translation_speed)
}
ENDIF
ELSE IF (Event.EventType == ES_EXIT)

ES_Timer_StopTimer(QUERY_TIMER)
ENDIF

return ReturnEvent

/* Drive a certain distance to middle branch. */

FUNCTION DuringBranchTwo(ES_Event_t Event)
ReturnEvent = Event
IF ( (Event.EventType == ES_ENTRY) ||
(Event.EventType == ES_ENTRY_HISTORY))
IF(CurrentState > PrevState){
drive(FORWARD, dist_from_one_to_two, translation_speed)
}
IF(CurrentState < PrevState)
{
drive(BACKWARD, dist_from_three_to_two, translation_speed)
}
ENDIF
ELSE IF (Event.EventType == ES_EXIT)
ES_Timer_StopTimer(QUERY_TIMER)
ENDIF
 return ReturnEvent

FUNCTION DuringBranchThree(ES_Event_t Event)

ReturnEvent = Event
IF ( (Event.EventType == ES_ENTRY) ||
(Event.EventType == ES_ENTRY_HISTORY))
IF(CurrentState > PrevState)
setDesiredSpeed(LEFT_MOTOR, FORWARD, translation_speed)
setDesiredSpeed(RIGHT_MOTOR, FORWARD, translation_speed)
ENDIF
IF(CurrentState < PrevState)
setDesiredSpeed(LEFT_MOTOR, BACKWARD, translation_speed)
setDesiredSpeed(RIGHT_MOTOR, BACKWARD, translation_speed)
ENDIF
ENDIF
ELSE IF (Event.EventType == ES_EXIT)
ES_Timer_StopTimer(QUERY_TIMER)
ENDIF
return ReturnEvent
PushCommit.c
FUNCTION RunPushCommitSM(ES_Event_t CurrentEvent)
MakeTransition = false
NextState = CurrentState
EntryEventKind = { ES_ENTRY, 0 }
ReturnEvent = CurrentEvent
QueryEvent.EventType = ES_NO_EVENT

SWITCH(CurrentState)
CASE BACK_UP_A_BIT:
CurrentEvent = DuringBackUpABit(CurrentEvent)
IF(CurrentEvent.EventType != ES_NO_EVENT)
SWITCH(CurrentEvent.EventType)
CASE ES_TRANSLATED:
NextState = ROTATE_TO_FACE_BRANCH
MakeTransition = true
END CASE
END SWITCH
ENDIF
END CASE

CASE FORWARD_A_BIT:
CurrentEvent = DuringForwardABit(CurrentEvent)
IF(CurrentEvent.EventType != ES_NO_EVENT)
SWITCH(CurrentEvent.EventType)
CASE ES_TRANSLATED:
NextState = ROTATE_TO_FACE_BRANCH
MakeTransition = true
END CASE
END SWITCH
ENDIF
END CASE

CASE ROTATE_TO_FACE_BRANCH:
CurrentEvent = DuringRotateToFaceBranch(CurrentEvent)
IF(CurrentEvent.EventType != ES_NO_EVENT)
SWITCH(CurrentEvent.EventType)
CASE ES_ROTATED:
DB_printf("PushCommitSM: Finished rotating to face branch\r\n")
NextState = FORWARD1
MakeTransition = true
END CASE
END SWITCH
ENDIF
END CASE

CASE FORWARD1:
CurrentEvent = DuringForward1(CurrentEvent)
 IF(CurrentEvent.EventType != ES_NO_EVENT)
SWITCH(CurrentEvent.EventType)
CASE ES_TRANSLATED:
NextState = FORWARD2
MakeTransition = true
END CASE
END SWITCH
ENDIF
END CASE

CASE FORWARD2:
CurrentEvent = DuringForward2(CurrentEvent)
IF(CurrentEvent.EventType != ES_NO_EVENT)
SWITCH(CurrentEvent.EventType)
CASE ES_TRANSLATED:
NextState = FORWARDLAST
MakeTransition = true
END CASE
END SWITCH
ENDIF
END CASE

CASE FORWARDLAST:
CurrentEvent = DuringForwardLast(CurrentEvent)
IF(CurrentEvent.EventType != ES_NO_EVENT)
SWITCH(CurrentEvent.EventType)
CASE ES_TRANSLATED:
ES_Event_t NewEvent
NewEvent.EventType = ES_FINISH
PostTopHSM(NewEvent)
END CASE
END SWITCH
ENDIF
END CASE
END SWITCH

IF (MakeTransition == true)
CurrentEvent.EventType = ES_EXIT
RunPushCommitSM(CurrentEvent)

CurrentState = NextState

RunPushCommitSM(EntryEventKind)

ENDIF
return ReturnEvent

FUNCTION StartPushCommitSM( ES_Event_t CurrentEvent )

{
IF (ES_ENTRY_HISTORY != CurrentEvent.EventType)
 CurrentBranch = QueryGoToBranchOriginSM()
PrevBranch = QueryGoToBranchOriginPrevSM()
IF (CurrentBranch == PrevBranch)
CurrentState = ROTATE_TO_FACE_BRANCH
ENDIF

ELSE IF(CurrentBranch == BRANCH_THREE)

CurrentState = BACK_UP_A_BIT
ENDIF
ELSE IF(CurrentBranch == BRANCH_TWO)
CurrentState = ROTATE_TO_FACE_BRANCH
ENDIF
ELSE IF(CurrentBranch == BRANCH_ONE)
CurrentState = FORWARD_A_BIT
ENDIF

ENDIF
RunPushCommitSM(CurrentEvent)
}

FUNCTION QueryPushCommitSM ( void )

return CurrentState

FUNCTION DuringBackUpABit(ES_Event_t Event)

ReturnEvent = Event
IF ( (Event.EventType == ES_ENTRY) ||
(Event.EventType == ES_ENTRY_HISTORY))
drive(BACKWARD, a_bit_cm, a_bit_speed)
ENDIF

return ReturnEvent

FUNCTION DuringRotateToFaceBranch(ES_Event_t Event)

ReturnEvent = Event
IF ( (Event.EventType == ES_ENTRY) ||
(Event.EventType == ES_ENTRY_HISTORY))
rotate90(CCW, rotate_speed)
ENDIF

return ReturnEvent

FUNCTION DuringForwardABit(ES_Event_t Event)

ReturnEvent = Event
IF ( (Event.EventType == ES_ENTRY) ||
(Event.EventType == ES_ENTRY_HISTORY))
drive(FORWARD, a_bit_cm, a_bit_speed)
ENDIF
 return ReturnEvent

FUNCTION DuringForward1(ES_Event_t Event)

ES_Event_t ReturnEvent = Event
IF ( (Event.EventType == ES_ENTRY) ||
(Event.EventType == ES_ENTRY_HISTORY))
drive(FORWARD, forward1_cm, forward1_speed)
ENDIF

return ReturnEvent

FUNCTION DuringForward2(ES_Event_t Event)

ReturnEvent = Event
IF ( (Event.EventType == ES_ENTRY) ||
(Event.EventType == ES_ENTRY_HISTORY))
drive(FORWARD, forward2_cm, forward2_speed)

return ReturnEvent

FUNCTION DuringForwardLast(ES_Event_t Event)

ES_Event_t ReturnEvent = Event
IF ( (Event.EventType == ES_ENTRY) ||
(Event.EventType == ES_ENTRY_HISTORY))
drive(FORWARD, getDesiredBranchDistance() - forward1_cm - forward2_cm,
forwardlast_speed)
ENDIF

return ReturnEvent
ComeBackSM.c

FUNCTION RunComeBackSM( ES_Event_t CurrentEvent )

MakeTransition = false
NextState = CurrentState
EntryEventKind = { ES_ENTRY, 0 }
ReturnEvent = CurrentEvent
QueryEvent.EventType = ES_NO_EVENT

SWITCH ( CurrentState )
CASE STOP_COME_BACK:
IF (ES_TIMEOUT == CurrentEvent.EventType && CurrentEvent.EventParam ==
COME_BACK_STOP_TIMER)
NextState = REVERSE1_TO_WALL
MakeTransition = true
ENDIF
END CASE

CASE REVERSE1_TO_WALL :
CurrentEvent = DuringReverse1ToWall(CurrentEvent)
//process any events
IF ( CurrentEvent.EventType != ES_NO_EVENT ) //IF an event is active
SWITCH (CurrentEvent.EventType)
CASE ES_TRANSLATED : //IF event is event one
NextState = REVERSE2_TO_WALL
MakeTransition = true //mark that we are taking a transition
END CASE
END SWITCH
ENDIF
END CASE

CASE REVERSE2_TO_WALL :
CurrentEvent = DuringReverse2ToWall(CurrentEvent)
//process any events
IF ( CurrentEvent.EventType != ES_NO_EVENT ) //IF an event is active
SWITCH (CurrentEvent.EventType)
CASE ES_TRANSLATED : //IF event is event one
NextState = REVERSELAST_TO_WALL
MakeTransition = true //mark that we are taking a transition
END CASE
END SWITCH
ENDIF
END CASE

CASE REVERSELAST_TO_WALL :
CurrentEvent = DuringReverseLastToWall(CurrentEvent)
//process any events
 IF ( CurrentEvent.EventType != ES_NO_EVENT ) //IF an event is active
SWITCH (CurrentEvent.EventType)
CASE ES_REACHED_WALL : //IF event is event one
NextState = MOVE_FORWARD_A_BIT
MakeTransition = true //mark that we are taking a transition
END CASE
END SWITCH
ENDIF
END CASE

CASE MOVE_FORWARD_A_BIT:
CurrentEvent = DuringMoveForwardABit(CurrentEvent)
IF ( CurrentEvent.EventType != ES_NO_EVENT )
SWITCH (CurrentEvent.EventType)
CASE ES_TRANSLATED:
NextState = ROTATE_IN_REPO
MakeTransition = true
END CASE
END SWITCH
ENDIF
END CASE

CASE ROTATE_IN_REPO:
CurrentEvent = DuringRotateInRepo(CurrentEvent)

IF (CurrentEvent.EventType != ES_NO_EVENT)
SWITCH (CurrentEvent.EventType){
CASE ES_ROTATED:
DB_printf("ComeBackSM: got ES_ROTATED\r\n")

CurrentBranch = QueryGoToBranchOriginSM()
SetGoToBranchOriginPrevSM(CurrentBranch)
ES_Event_t NewEvent
NewEvent.EventType = ES_FINISH
PostTopHSM(NewEvent)
END CASE
END SWITCH
ENDIF
END CASE
END SWITCH

IF (MakeTransition == true)
CurrentEvent.EventType = ES_EXIT
RunComeBackSM(CurrentEvent)

CurrentState = NextState

RunComeBackSM(EntryEventKind)

ENDIF
return ReturnEvent
/* Stop on entry into ComeBack SM. */
FUNCTION StartComeBackSM ( ES_Event_t 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
IF ( ES_ENTRY_HISTORY != CurrentEvent.EventType )
ES_Timer_InitTimer(COME_BACK_STOP_TIMER, stop_ms)
CurrentState = STOP_COME_BACK
setDesiredSpeed(LEFT_MOTOR, FORWARD, 0)
setDesiredSpeed(RIGHT_MOTOR, FORWARD, 0)
ENDIF
// call the entry function (IF any) for the ENTRY_STATE
RunComeBackSM(CurrentEvent)

FUNCTION QueryComeBackSM ( void )

return CurrentState

FUNCTION DuringReverse1ToWall( ES_Event_t Event)

ReturnEvent = Event // assume no re-mapping or consumption

// process ES_ENTRY, ES_ENTRY_HISTORY & ES_EXIT events

IF ( (Event.EventType == ES_ENTRY) ||
(Event.EventType == ES_ENTRY_HISTORY) )
drive(BACKWARD, reverse1_cm, reverse1_speed)
ENDIF

FUNCTION DuringReverse2ToWall( ES_Event_t Event)

ReturnEvent = Event // assume no re-mapping or consumption

// process ES_ENTRY, ES_ENTRY_HISTORY & ES_EXIT events

IF ( (Event.EventType == ES_ENTRY) ||
(Event.EventType == ES_ENTRY_HISTORY) )
drive(BACKWARD, reverse2_cm, reverse2_speed)
ENDIF

return ReturnEvent

FUNCTION DuringReverseLastToWall( ES_Event_t Event)

ReturnEvent = Event // assume no re-mapping or consumption

// process ES_ENTRY, ES_ENTRY_HISTORY & ES_EXIT events

IF ( (Event.EventType == ES_ENTRY) ||
(Event.EventType == ES_ENTRY_HISTORY) )
setDesiredSpeed(RIGHT_MOTOR, BACKWARD, reverselast_speed)
setDesiredSpeed(LEFT_MOTOR, BACKWARD, reverselast_speed)
 ENDIF
ELSE IF ( Event.EventType == ES_EXIT )
setDesiredSpeed(RIGHT_MOTOR, BACKWARD, 0)
setDesiredSpeed(LEFT_MOTOR, BACKWARD, 0)
ENDIF

return ReturnEvent

FUNCTION DuringMoveForwardABit( ES_Event_t Event)

ReturnEvent = Event
IF ( (Event.EventType == ES_ENTRY) ||
(Event.EventType == ES_ENTRY_HISTORY) )
drive(FORWARD, a_bit_cm, a_bit_speed)
ENDIF

return ReturnEvent

FUNCTION DuringRotateInRepo( ES_Event_t Event)

ReturnEvent = Event // assume no re-mapping or consumption

// process ES_ENTRY, ES_ENTRY_HISTORY & ES_EXIT events

IF ( (Event.EventType == ES_ENTRY) ||
(Event.EventType == ES_ENTRY_HISTORY) )
rotate90(CW, rotate_speed)
ENDIF
return ReturnEvent
DCMotor
InitDCMotor(enablePI):
R2 = 0
L2 = 0
setPWM()
OC3RS = 0
OC2RS = 0
initEncoderISRs()
initPIController()
if enablePI then enablePIControl()
else disablePIControl()
DB_printf("Initialized DC Motor, compiled at %s on %s\n", __TIME__, __DATE__)

initEncoderISRs():
disable interrupts
Map A2 to IC1 for right encoder
Set up IC1CON to capture every rising edge
Set interrupt flag and priority for IC1
Map B5 to IC3 for left encoder
Set up IC3CON to capture every rising edge
Set interrupt flag and priority for IC3
Use multi-vectored interrupts
Turn on IC1CON

initPIController():
Ldirection_desired = 0
Rdirection_desired = 0
Lspeed_desired = 0
Rspeed_desired = 0
low_speed_threshold = 500
fastLkP = 0.2
fastLkI = 0.04
fastRkP = 0.15
fastRkI = 0.04
slowLkP = 0.1
slowLkI = 0.005
slowRkP = 0.1
slowRkI = 0.005
Lcurr_sum_e = 0
Llast_sum_e = 0
Rcurr_sum_e = 0
Rlast_sum_e = 0

setPWM():
Set Timer 2 to use System Clock, Prescale of 1:4, and Period of PWM_PERIOD
Set OC3 to PWM mode with Timer 2 as clock source
Set OC2 to PWM mode with Timer 2 as clock source
Enable OC3
Enable OC2
 enablePIControl():
Turn on the control loop

disablePIControl():
Turn off the control loop

adjust():
Calculate the number of pulses needed to travel the given distance
Disable interrupts to make sure motor counts are updated atomically
Update the desired pulse count for the specified motor
Re-enable interrupts
Set the desired speed for the motor
Set the event to be triggered when the motor reaches the desired pulse count

rotate90():
Calculate the distance needed to rotate 90 degrees assuming a wheel circumference of 76cm
Call the drive function with that distance and the given speed

isNoise():
Define the minimum valid period between interrupts
Check if the current interrupt time is less than the previous interrupt time
Check if the period between interrupts is less than the minimum valid period
Return true if either condition is met

isStopped():
Define the maximum valid period between encoder ticks
Check if the time since the last encoder tick is greater than the maximum valid period

reachedDesiredLPulses():
reachedDesiredRPulses():
If desired pulses mode is not enabled, return false
Disable interrupts to make sure motor counts are updated atomically
Get the current and desired pulse counts for the left motor
Re-enable interrupts
Check if the current pulse count is greater than the desired pulse count
If so, set stop this motor’s duty cycle.
Set a reached desired pulses boolean bit.

reachedBothDesiredPulses():
If both desired pulses have been met, return true and set both to false
Otherwise return false

ISR_RightEncoder():
ISR_LeftEncoder():
While the IC buffer is not empty, read the buffer. If not classified as noise, update the
current encoder period.
If counting up to desired pulses, increment the count.
After emptying the IC buffer, clear the interrupt flag.
PIControllerISR():
Update the error sums.
Update the duty cycle using kP and kI gains with anti-windup
Clear the interrupt flag

CountRollOver():
Update the rollover counter with updateGlobalTime()
If either motor’s last encoder pulse time is a while ago, set their motor period to 0, which is
assumed to mean the motor has stopped turning.
EventOverSPI
InitEventOverSPI(bool isDriveMaster):
set isLeader to isDriveMaster
if isLeader is true, print "EventOverSPI init'd to LEADER mode."
else, print "EventOverSPI init'd to FOLLOWER mode."
setup SPI module 1 with the following configurations:
Module = SPI_SPI1
Phase = SPI_SMP_MID
SPI_ClkPeriodIn_ns = 25000
SSPin = SPI_RPA0
SDIPin = SPI_RPB8
SDOPin = SPI_RPA1
WhichState = SPI_CLK_HI
WhichEdge = SPI_SECOND_EDGE
DataWidth = SPI_16BIT
if isLeader is true, call SPISetup_SetLeader with Module and Phase as parameters
else, call SPISetup_SetFollower with Module as a parameter
call SPISetup_SetBitTime with Module and SPI_ClkPeriodIn_ns as parameters
if isLeader is true, call SPISetup_MapSSOutput with Module and SSPin as parameters
else, call SPISetup_MapSSInput with Module and SSPin as parameters
call SPISetup_MapSDInput with Module and SDIPin as parameters
call SPISetup_MapSDOutput with Module and SDOPin as parameters
call SPISetup_SetClockIdleState with Module and WhichState as parameters
call SPISetup_SetActiveEdge with Module and WhichEdge as parameters
call SPISetup_SetXferWidth with Module and DataWidth as parameters
call SPISetEnhancedBuffer with Module and false as parameters
call SPISetup_EnableSPI with Module as a parameter
set SPIBUF to NO_LEFTOVER_WORD if isLeader is false
disable interrupts
clear IFS1_SPI1RXIF_MASK
set IEC1_SPI1RXIE_MASK
set INTCONbits.MVEC to 1
set IPC7bits.SPI1IP to 6
enable interrupts
configure digital outputs on Port_A for pins 0 and 1
configure digital inputs on Port_B for pin 8

PostEventOverSPI(ES_Event_t e):
if e.EventParam cannot fit in 8 bits or e.EventType cannot fit in 8 bits, print an error
message and return false
create a new SPI_Event_t se with e.EventType and e.EventParam as the two 8-bit
fields of the struct
if isLeader is true:
call SPIOperate_SPI1_Send16Wait with se.w as a parameter
call SPIOperate_SPI1_Send16Wait with QUERY_WORD as a parameter
else:
add se.w to the end of followerQueue
increment followerQueueSize
print "PostEventOverSPI, follower: queued [se.w]"
 print "PostEventOverSPI, follower: queue size now [followerQueueSize]"
return true

FitsIn8Bits(uint16_t data):
return whether data is compressible.