Embedded Files
Overview
Overview
This entire project was coded in C following the Events and Service framework.
The code is available on BitBucket - Leader PIC / Follower PIC
Leader (PIC 1)
Leader (PIC 1)
Pseudo Code for MainHSM
Pseudo Code for MainHSM
// Module Defines
DEFINE BUTTON_LED as LATAbits.LATA3
DEFINE ONE_MIN as 60000
DEFINE EIGHTEEN_SEC as 18000
// Module Variables
MainState_t CurrentState
uint8_t MyPriority
uint8_t GlobalRollOver
bool LastOnOffButton
// Module Functions
InitMainSM(Priority)
PostMainSM(ThisEvent)
RunMainSM(CurrentEvent)
StartMainSM(CurrentEvent)
QueryMainSM()
GetLastOnOffButton()
SendLastOnOffButton(State)
DuringGameStart(Event)
DuringMidGame(Event)
DuringEndGame(Event)
GlobalTimer()
// InitMainSM function
MyPriority = Priority
ThisEvent = ES_ENTRY
Initialize RA2 as input(1)
Initialize RA3 as output (0)
StartMainSM(ThisEvent)
return true
// PostMainSM function
bool PostMainSM(ThisEvent):
return ES_PostToService(MyPriority, ThisEvent)
// RunMainSM function
ES_Event_t RunMainSM(CurrentEvent):
MakeTransition = false
NextState = CurrentState
EntryEventKind = {ES_ENTRY, 0}
ReturnEvent = {ES_NO_EVENT, 0}
switch CurrentState:
case MAIN_INIT:
switch CurrentEvent.EventType:
case ES_BUTTON_PRESSED:
if LastOnOffButton == 1:
SET BUTTON_LED to 1
Initialize GLOBAL_TIMER for ONE_MIN)
else:
SET BUTTON_LED to 0
NextState = GAME_START
MakeTransition = true
EntryEventKind = {ES_ENTRY_HISTORY, 0}
case ES_TIMEOUT:
if GLOBAL_TIMER is CurrentEvent.EventParam:
Run GlobalTimer()
MakeTransition = false
case ES_GLOBAL_TIMEOUT:
NextState = STOP_ALL
MakeTransition = true
EntryEventKind = {ES_ENTRY, 0}
case GAME_START:
// Execute During function for GAME_START
CurrentEvent = DuringGameStart(CurrentEvent);
// Process any events
switch CurrentEvent.EventType
case ES_WALL_NEAR:
NextState = END_GAME;
MakeTransition = true;
EntryEventKind = ES_ENTRY;
case ES_TIMEOUT:
if GLOBAL_TIMER is CurrentEvent.EventParam:
Run GlobalTimer()
MakeTransition = false
case ES_GLOBAL_TIMEOUT:
NextState = STOP_ALL
MakeTransition = true
EntryEventKind = {ES_ENTRY, 0}
case MID_GAME:
// Execute During function for MID_GAME
CurrentEvent = DuringMidGame(CurrentEvent);
// Process any events
switch CurrentEvent.EventType
case ES_WALL_NEAR:
NextState = END_GAME;
MakeTransition = true;
EntryEventKind = ES_ENTRY_HISTORY;
break;
case ES_TIMEOUT:
if GLOBAL_TIMER is CurrentEvent.EventParam:
Run GlobalTimer()
MakeTransition = false
case ES_GLOBAL_TIMEOUT:
NextState = STOP_ALL
MakeTransition = true
EntryEventKind = {ES_ENTRY, 0}
case END_GAME:
// Execute During function for END_GAME
CurrentEvent = DuringEndGame(CurrentEvent);
// Process any events
switchCurrentEvent.EventType
case ES_TIMEOUT:
if GLOBAL_TIMER is CurrentEvent.EventParam:
Run GlobalTimer()
MakeTransition = false
case ES_GLOBAL_TIMEOUT:
NextState = STOP_ALL
MakeTransition = true
EntryEventKind = {ES_ENTRY, 0}
case STOP_ALL:
// Execute During function for STOP_ALL
ES_Event_t StopEvent
SET StopEvent.EventType as ES_DRIVE_MOTOR
SET StopEvent.EventParam as CMD_STOP
Post StopEvent to PostCL_MotorService
SET BUTTON_LED to 0
// Process any events
switch(CurrentEvent.EventType
case ES_BUTTON_PRESSED:
if LastOnOffButton is 1:
SET BUTTON_LED to 1
Initialize GLOBAL_TIMER for ONE_MIN
else
SET BUTTON_LED to 0
NextState = GAME_START;
MakeTransition = true;
EntryEventKind = ES_ENTRY_HISTORY;
// StartMainSM function
void StartMainSM(CurrentEvent):
CurrentState = MAIN_INIT
BUTTON_LED = 0
RunMainSM(CurrentEvent)
// QueryMainSM function
MainState_t QueryMainSM():
return CurrentState
// GetLastOnOffButton function
bool GetLastOnOffButton():
return LastOnOffButton
// SendLastOnOffButton function
void SendLastOnOffButton(State):
Set LastOnOffButton = State
// DuringGameStart function
ES_Event_t DuringGameStart(Event):
if Event is ES_ENTRY or Event is ES_ENTRY_HISTORY:
Run StartGameStartSM()
Set BUTTON_LED = 1
else if Event is ES_EXIT:
// Exit function
else:
RunGameStartSM()
// DuringMidGame function
ES_Event_t DuringMidGame(Event):
if Event is ES_ENTRY or Event is ES_ENTRY_HISTORY:
Run StartMidgameSM()
else if Event is ES_EXIT:
// Exit function
else:
RunMidGameSM()
// DuringEndGame function
ES_Event_t DuringEndGame(Event):
if Event is ES_ENTRY or Event is ES_ENTRY_HISTORY:
Run StartEndGameSM()
else if Event is ES_EXIT:
// Exit function
else:
RunEndGameSM()
// GlobalTimer function
void GlobalTimer():
Increment GlobalRollOver by 1
if GlobalRollOver is 1:
Initialize GLOBAL_TIMER for ONE_MIN
else if GlobalRollOver is 2:
Initialize GLOBAL_TIMER for EIGHTEEN_SEC
else if GlobalRollOver is 3:
Set Event's event type to ES_GLOBAL_TIMEOUT
Post Event to PostMainSM()
GameStartSM
GameStartSM
Governs the starting sequence of the robot. Once pucks are collected, the tree wall is detected, and that's when the state changes to end game.
Pseudo Code for GameStartSM
Pseudo Code for GameStartSM
// RunGameSM function
Switch on CurrentState:
case ALIGN_WALL:
Execute DuringAlignWall(CurrentEvent)
Update CurrentEvent with the returned value
If CurrentEvent is not ES_NO_EVENT:
Switch on CurrentEvent.EventType:
case ES_ALIGNED_WITH_WALL:
Update NextState to ROTATE_180
Set MakeTransition to true
Set EntryEventKind.EventType to ES_ENTRY_HISTORY
Update ReturnEvent.EventType to ES_NO_EVENT
case ROTATE_180:
Execute DuringRotate180(CurrentEvent)
Update CurrentEvent with the returned value
If CurrentEvent is not ES_NO_EVENT:
Switch on CurrentEvent.EventType:
case ES_MOVE_COMPLETE:
Update NextState to ARM_OUT1
Set MakeTransition to true
Set EntryEventKind.EventType to ES_ENTRY_HISTORY
Update ReturnEvent.EventType to ES_NO_EVENT
case MOVE_TO_CENTER:
Execute DuringMoveToCenter(CurrentEvent)
Update CurrentEvent with the returned value
If CurrentEvent is not ES_NO_EVENT:
Switch on CurrentEvent.EventType:
case ES_SIDE_TAPE_DETECTED:
Update NextState to ARM_IN1
Set MakeTransition to true
Set EntryEventKind.EventType to ES_ENTRY_HISTORY
Update ReturnEvent.EventType to ES_NO_EVENT
case ROTATE_90:
Execute DuringRotate90(CurrentEvent)
Update CurrentEvent with the returned value
If CurrentEvent is not ES_NO_EVENT:
Switch on CurrentEvent.EventType:
case ES_MOVE_COMPLETE:
Update NextState to MOVE_TO_TREE
Set MakeTransition to true
Set EntryEventKind.EventType to ES_ENTRY_HISTORY
Update ReturnEvent.EventType to ES_NO_EVENT
case MOVE_TO_FENCE:
Execute DuringMoveToFence(CurrentEvent)
Update CurrentEvent with the returned value
If CurrentEvent is not ES_NO_EVENT:
Switch on CurrentEvent.EventType:
case ES_MOVE_COMPLETE:
Update NextState to ARM_OUT2
Set MakeTransition to true
Set EntryEventKind.EventType to ES_ENTRY_HISTORY
Update ReturnEvent.EventType to ES_NO_EVENT
case MOVE_TO_TREE:
Execute DuringMoveToTree(CurrentEvent)
Set MakeTransition to false
case ARM_OUT1:
Execute DuringArmOut(CurrentEvent)
Update CurrentEvent with the returned value
If CurrentEvent is not ES_NO_EVENT:
Switch on CurrentEvent.EventType:
case ES_TIMEOUT:
If ARM_TIMER equals CurrentEvent.EventParam:
Update NextState to MOVE_TO_CENTER
Set MakeTransition to true
Set EntryEventKind.EventType to ES_ENTRY_HISTORY
Update ReturnEvent.EventType to ES_NO_EVENT
case ARM_OUT2:
Execute DuringArmOut(CurrentEvent)
Update CurrentEvent with the returned value
If CurrentEvent is not ES_NO_EVENT:
Switch on CurrentEvent.EventType:
case ES_TIMEOUT:
If ARM_TIMER equals CurrentEvent.EventParam:
Update NextState to TRIGGER
Set MakeTransition to true
Set EntryEventKind.EventType to ES_ENTRY_HISTORY
Update ReturnEvent.EventType to ES_NO_EVENT
case ARM_IN1:
Execute DuringArmIn(CurrentEvent)
Update CurrentEvent with the returned value
If CurrentEvent is not ES_NO_EVENT:
Switch on CurrentEvent.EventType:
case ES_TIMEOUT:
If ARM_TIMER equals CurrentEvent.EventParam:
Update NextState to MOVE_TO_FENCE
Set MakeTransition to true
Set EntryEventKind.EventType to ES_ENTRY_HISTORY
Update ReturnEvent.EventType to ES_NO_EVENT
case TRIGGER:
Execute DuringTrigger(CurrentEvent)
Update CurrentEvent with the returned value
If CurrentEvent is not ES_NO_EVENT:
Switch on CurrentEvent.EventType:
case ES_TIMEOUT:
If DEMO_TIMER equals CurrentEvent.EventParam:
Update NextState to ARM_IN2
Set MakeTransition to true
Set EntryEventKind.EventType to ES_ENTRY_HISTORY
Update ReturnEvent.EventType to ES_NO_EVENT
case ARM_IN2:
Execute DuringArmIn(CurrentEvent)
Update CurrentEvent with the returned value
If CurrentEvent is not ES_NO_EVENT:
Switch on CurrentEvent.EventType:
case ES_TIMEOUT:
If ARM_TIMER equals CurrentEvent.EventParam:
Update NextState to ROTATE_1802
Set MakeTransition to true
Set EntryEventKind.EventType to ES_ENTRY_HISTORY
Update ReturnEvent.EventType to ES_NO_EVENT
case ROTATE_1802:
Execute DuringRotate180(CurrentEvent)
Update CurrentEvent with the returned value
If CurrentEvent is not ES_NO_EVENT:
Switch on CurrentEvent.EventType:
case ES_MOVE_COMPLETE:
Update NextState to MOVE_TO_CENTER2
Set MakeTransition to true
Set EntryEventKind.EventType to ES_ENTRY_HISTORY
Update ReturnEvent.EventType to ES_NO_EVENT
case MOVE_TO_CENTER2:
Execute DuringMoveToCenter(CurrentEvent)
Update CurrentEvent with the returned value
If CurrentEvent is not ES_NO_EVENT:
Switch on CurrentEvent.EventType:
case ES_SIDE_TAPE_DETECTED:
Update NextState to ROTATE_902
Set MakeTransition to true
Set EntryEventKind.EventType to ES_ENTRY_HISTORY
Update ReturnEvent.EventType to ES_NO_EVENT
case ROTATE_902:
Execute DriveEvent = ES_DRIVE_MOTOR with EventParam CMD_TURN_LEFT
PostCL_MotorService(DriveEvent)
Update CurrentEvent with the returned value
If CurrentEvent is not ES_NO_EVENT:
Switch on CurrentEvent.EventType:
case ES_MOVE_COMPLETE:
Update NextState to MOVE_TO_TREE
Set MakeTransition to true
Set EntryEventKind.EventType to ES_ENTRY_HISTORY
Update ReturnEvent.EventType to ES_NO_EVENT
static ES_Event_t DuringAlignWall( ES_Event_t Event)
{
ES_Event_t ReturnEvent = Event; // Assume no re-mapping or consumption
if ( (Event.EventType == ES_ENTRY) || (Event.EventType == ES_ENTRY_HISTORY) ) {
// Entry function for this state
Switch On US Sensors
Start Align Request
Post CMD_SPIN to MotorService;
} else if ( Event.EventType == ES_EXIT ) {
// Exit function for this state
} else {
// During function for this state
}
return(ReturnEvent);
}
static ES_Event_t DuringRotate180( ES_Event_t Event)
{
ES_Event_t ReturnEvent = Event; // Assume no re-mapping or consumption
if ( (Event.EventType == ES_ENTRY) || (Event.EventType == ES_ENTRY_HISTORY) ) {
// Entry function for this state
Post CMD_TURN_180 to MotorService;
} else if ( Event.EventType == ES_EXIT ) {
// Exit function for this state
// Stop the motors
} else {
// During function for this state
}
return(ReturnEvent);
}
static ES_Event_t DuringMoveToCenter( ES_Event_t Event)
{
ES_Event_t ReturnEvent = Event; // Assume no re-mapping or consumption
if ( (Event.EventType == ES_ENTRY) || (Event.EventType == ES_ENTRY_HISTORY) ) {
// Entry function for this state
TapeSensors = QueryTapeSensors();
if (TapeSensors == FRONT_LEFT) {
Post CMD_SLIGHT_RIGHT to MotorService;
} else if (TapeSensors == FRONT_RIGHT) {
Post CMD_SLIGHT_LEFT to MotorService;
} else {
Post CMD_FORWARD to MotorService;
}
} else if ( Event.EventType == ES_EXIT ) {
// Exit function for this state
} else {
// During function for this state
TapeSensors = QueryTapeSensors();
if (TapeSensors == SIDE_TAPE) {
Post ES_SIDE_TAPE_DETECTED to MainHSM;
Post CMD_READ_BEACON to SPILService;
}
if (TapeSensors == FRONT_LEFT) {
Post CMD_SLIGHT_RIGHT to MotorService;
} else if (TapeSensors == FRONT_RIGHT) {
Post CMD_SLIGHT_LEFT to MotorService;
} else {
Post CMD_FORWARD to MotorService;
}
}
return(ReturnEvent);
}
static ES_Event_t DuringRotate90( ES_Event_t Event)
{
ES_Event_t ReturnEvent = Event; // Assume no re-mapping or consumption
if ( (Event.EventType == ES_ENTRY) || (Event.EventType == ES_ENTRY_HISTORY) ) {
// Entry function for this state
Post CMD_TURN_RIGHT to MotorService;
} else if ( Event.EventType == ES_EXIT ) {
// Exit function for this state
} else {
// During function for this state
}
return(ReturnEvent);
}
static ES_Event_t DuringMoveToTree( ES_Event_t Event)
{
ES_Event_t ReturnEvent = Event; // Assume no re-mapping or consumption
if ( (Event.EventType == ES_ENTRY) || (Event.EventType == ES_ENTRY_HISTORY) ) {
// Entry function for this state
StartDistReq(true, 2000);
TapeSensors = QueryTapeSensors();
ES_Event_t DriveEvent;
DriveEvent.EventType = ES_DRIVE_MOTOR;
if (TapeSensors == FRONT_LEFT) {
Post CMD_SLIGHT_RIGHT to MotorService;
} else if (TapeSensors == FRONT_RIGHT) {
Post CMD_SLIGHT_LEFT to MotorService;
} else {
Post CMD_FORWARD to MotorService;
}
} else if ( Event.EventType == ES_EXIT ) {
// Exit function for this state
} else {
// During function for this state
TapeSensors = QueryTapeSensors();
if (TapeSensors == FRONT_LEFT) {
Post CMD_SLIGHT_RIGHT to MotorService;
} else if (TapeSensors == FRONT_RIGHT) {
Post CMD_SLIGHT_LEFT to MotorService;
} else if (TapeSensors == FRONT_BOTH || TapeSensors == 0) {
Post CMD_FORWARD to MotorService;
}
}
return(ReturnEvent);
}
static ES_Event_t DuringArmOut( ES_Event_t Event)
{
ES_Event_t ReturnEvent = Event; // Assume no re-mapping or consumption
if ( (Event.EventType == ES_ENTRY) || (Event.EventType == ES_ENTRY_HISTORY) ) {
// Entry function for this state
Post CMD_ARM_OUT to SPILService;
ES_Timer_InitTimer(ARM_TIMER, 2000);
} else if ( Event.EventType == ES_EXIT ) {
// Exit function for this state
} else {
// During function for this state
}
return(ReturnEvent);
}
static ES_Event_t DuringArmIn( ES_Event_t Event)
{
ES_Event_t ReturnEvent = Event; // Assume no re-mapping or consumption
if ( (Event.EventType == ES_ENTRY) || (Event.EventType == ES_ENTRY_HISTORY) ) {
// Entry function for this state
Post CMD_ARM_IN to SPILService;
ES_Timer_InitTimer(ARM_TIMER, 2);
} else if ( Event.EventType == ES_EXIT ) {
// Exit function for this state
} else {
// During function for this state
}
return(ReturnEvent);
}
static ES_Event_t DuringTrigger( ES Event_t Event)
{
ES_Event_t ReturnEvent = Event; // Assume no re-mapping or consumption
if ( (Event.EventType == ES_ENTRY) || (Event.EventType == ES_ENTRY_HISTORY) ) {
// Entry function for this state
Post CMD_TRIGGER_START to SPILService
ES_Timer_InitTimer(DEMO_TIMER, 3000); // 3s demo timer for trigger start to trigger end
} else if ( Event.EventType == ES_EXIT ) {
// Exit function for this state
Post CMD_TRIGGER_END to SPILService
} else {
// During function for this state
}
return(ReturnEvent);
}
EndGameSM
EndGameSM
Governs the sweeping and firing sequence of the robot. After the firing sequence ends, the robot doesn't do anything until the game timer ends.
Pseudo Code for EndGameSM
Pseudo Code for EndGameSM
Function: RunEndGameSM (End Game State Machine)
Pseudo code:
1. Initialize local variables:
- MakeTransition to false
- NextState to CurrentState
- EntryEventKind as ES_ENTRY event with parameter 0
- ReturnEvent as CurrentEvent
2. Switch based on CurrentState:
a. Case REVERSE1:
i. Execute DuringReverseTree function for state REVERSE1 and update CurrentEvent
ii. Check if CurrentEvent is not ES_NO_EVENT:
- If true, switch based on CurrentEvent.EventType
* Case ES_MOVE_COMPLETE:
- Set NextState to TURN_RIGHT1
- Set MakeTransition to true
- Set EntryEventKind.EventType to ES_ENTRY_HISTORY
- Set ReturnEvent.EventType to ES_NO_EVENT
iii. Break from the switch case
b. Case TURN_RIGHT1:
i. Execute DuringTurnRight function for state TURN_RIGHT1 and update CurrentEvent
ii. Check if CurrentEvent is not ES_NO_EVENT:
- If true, switch based on CurrentEvent.EventType
* Case ES_MOVE_COMPLETE:
- Set NextState to FORWARD1
- Set MakeTransition to true
- Set EntryEventKind.EventType to ES_ENTRY_HISTORY
- Set ReturnEvent.EventType to ES_NO_EVENT
iii. Break from the switch case
c. Case FORWARD1:
i. Execute DuringForward30 function for state FORWARD1 and update CurrentEvent
ii. Check if CurrentEvent is not ES_NO_EVENT:
- If true, switch based on CurrentEvent.EventType
* Case ES_MOVE_COMPLETE:
- Set NextState to REVERSE2
- Set MakeTransition to true
- Set EntryEventKind.EventType to ES_ENTRY_HISTORY
- Set ReturnEvent.EventType to ES_NO_EVENT
iii. Break from the switch case
d. Case REVERSE2:
i. Execute DuringReverseSlow function for state REVERSE2 and update CurrentEvent
ii. Check if CurrentEvent is not ES_NO_EVENT:
- If true, switch based on CurrentEvent.EventType
* Case ES_MOVE_COMPLETE:
- Set NextState to TURN_RIGHT2
- Set MakeTransition to true
- Set EntryEventKind.EventType to ES_ENTRY_HISTORY
- Set ReturnEvent.EventType to ES_NO_EVENT
iii. Break from the switch case
e. Case TURN_RIGHT2:
i. Execute DuringTurnRight function for state TURN_RIGHT2 and update CurrentEvent
ii. Check if CurrentEvent is not ES_NO_EVENT:
- If true, switch based on CurrentEvent.EventType
* Case ES_MOVE_COMPLETE:
- Set NextState to FORWARD2
- Set MakeTransition to true
- Set EntryEventKind.EventType to ES_ENTRY_HISTORY
- Set ReturnEvent.EventType to ES_NO_EVENT
iii. Break from the switch case
f. Repeat this pattern for the remaining states (FORWARD2, REVERSE3, TURN_RIGHT3, FORWARD3, TURN_LEFT1, FORWARD5, REVERSE5, TURN_LEFT2, FORWARD6, REVERSE6, TURN_LEFT3, and FORWARD7).
3. Check if MakeTransition is true:
- If true, execute the exit function for the current state by setting CurrentEvent.EventType to ES_EXIT and recursively call RunEndGameSM
- Update CurrentState to NextState
- Execute the entry function for the new state by calling RunEndGameSM with EntryEventKind
4. Return ReturnEvent
During Functions:
DuringReverseSlow:
If Event.EventType is ES_ENTRY or ES_ENTRY_HISTORY:
Start the reverse sweep motor command.
Start the alignment request.
If Event.EventType is ES_EXIT:
Stop the alignment request.
Otherwise:
Determine the appropriate motor command based on the event type (ES_ALIGNED_WITH_WALL, ES_NOT_ALIGNED_LEFT, ES_NOT_ALIGNED_RIGHT).
DuringReverseTree:
Similar to DuringReverseSlow, but with a different motor command (CMD_REVERSE_TREE).
DuringReverseFast:
If Event.EventType is ES_ENTRY or ES_ENTRY_HISTORY:
Start the reverse sweep motor command.
Start the fast reverse motor command.
No action on ES_EXIT.
No action for other event types.
DuringTurnRight:
If Event.EventType is ES_ENTRY or ES_ENTRY_HISTORY:
Start the turn right motor command.
No action on ES_EXIT.
No action for other event types.
DuringTurnLeft:
If Event.EventType is ES_ENTRY or ES_ENTRY_HISTORY:
Start the turn left motor command.
No action on ES_EXIT.
No action for other event types.
DuringForward4:
If Event.EventType is ES_ENTRY or ES_ENTRY_HISTORY:
Start the forward 148 motor command.
Determine the appropriate motor command for line following based on tape sensor readings.
No action on ES_EXIT.
Determine the appropriate motor command for line following based on tape sensor readings.
DuringForward30:
If Event.EventType is ES_ENTRY or ES_ENTRY_HISTORY:
Start the forward 30 motor command.
Start the fast forward motor command.
No action on ES_EXIT.
No action for other event types.
DuringForwardToCenter:
If Event.EventType is ES_ENTRY or ES_ENTRY_HISTORY:
Start the fast forward motor command.
No action on ES_EXIT.
If Event.EventType is ES_TAPE_DETECTED, check for side tape detection and post ES_SIDE_TAPE_DETECTED to the MainHSM.
DuringForward148:
If Event.EventType is ES_ENTRY or ES_ENTRY_HISTORY:
Start the forward 148 motor command.
Start the fast forward motor command.
No action on ES_EXIT.
No action for other event types.
SPILService
SPILService
Governs SPI communication from Leader to Follower. 0x00 (CMD_STATUS) is sent every 100ms to keep the communication always on. When the receive buffer is non-zero, ES_RECEIVE_FROM_FOLLOWER is posted to the service. If the service receives a transmit request, it fills the buffer with the event parameter.
Pseudo Code for SPILService
Pseudo Code for SPILService
Function: InitSPILService
Pseudo code:
1. Set MyPriority to Priority
2. Set up SPI1 configuration:
a. Set SPI1 as leader with middle sample point
b. Map SS to pin RA0
c. Map SDO to pin RA1
d. Set clock idle state to high
e. Set active edge to second edge
f. Disable enhanced buffer
g. Set transfer width to 8 bits
h. Set bit time to 100kHz
i. Enable SDI
j. Configure RB5 as input and map SDI1 to RB5
k. Enable SPI1
3. Disable interrupts
4. Enable multi-vector interrupts
5. Clear SPI1 receive interrupt flag
6. Enable SPI1 receive interrupt
7. Set SPI1 interrupt priority to 5
8. Enable interrupts
9. Post the initial transition event to the service
10. If posting was successful, return true; otherwise, return false
Function: RunSPILService
Pseudo code:
1. Initialize ReturnEvent with ES_NO_EVENT
2. Switch based on ThisEvent.EventType:
a. If ES_INIT:
i. Initialize SPI timer with a timeout value of 100
b. If ES_TIMEOUT:
i. Create a new event FirstEvent with EventType ES_TRANSMIT_TO_FOLLOWER and EventParam CMD_STATUS
ii. Post FirstEvent to SPILService
iii. Reinitialize SPI timer with a timeout value of 100
c. If ES_TRANSMIT_TO_FOLLOWER:
i. Switch based on ThisEvent.EventParam:
- If CMD_READ_BEACON: Transmit CMD_READ_BEACON over SPI1
- If CMD_TRIGGER_START: Transmit CMD_TRIGGER_START over SPI1
- If CMD_TRIGGER_END: Transmit CMD_TRIGGER_END over SPI1
- If CMD_ARM_IN: Transmit CMD_ARM_IN over SPI1
- If CMD_ARM_OUT: Transmit CMD_ARM_OUT over SPI1
- If CMD_STATUS: Transmit CMD_STATUS over SPI1
d. If ES_RECEIVE_FROM_FOLLOWER:
i. Switch based on ThisEvent.EventParam:
- For each received command, perform corresponding actions (e.g., handle beacon signals, limit switch states)
ii. If the received command is related to tape sensors (CMD_TAPE_SENSOR), extract tape sensor information from EventParam, create a new TapeEvent with EventType ES_TAPE_DETECTED, set TapeEvent.EventParam accordingly, and post TapeEvent to MainHSM
3. Return ReturnEvent
Function: SPIBuffer (Interrupt Service Routine)
Pseudo code:
1. Clear the SPI1 Receive Interrupt Flag (IFS1CLR)
2. Check if the receive buffer is full (SPI1STATbits.SPIRBF == 1)
a. If the buffer is full:
i. Initialize a new event ThisEvent with EventType ES_RECEIVE_FROM_FOLLOWER
ii. Read the received command from SPI1BUF and store it in a local variable command
iii. Set ThisEvent.EventParam to the value of command
iv. Post ThisEvent to the SPILService
Pseudo Code for USSensorService
Pseudo Code for USSensorService
Declare global variables:
- RolloverCount: uint16_t
- TRIG: bool
- LeftDis: volatile uint16_t
- RightDis: volatile uint16_t
- DistReqVal: uint16_t
- TimeCount_t: union containing a uint32_t Full and a struct ByBytes with uint16_t slice and RollOver
- NextEdge_t: enum with RISE and FALL
- LeftRiseTick: static TimeCount_t
- LeftFallTick: static TimeCount_t
- RightRiseTick: static TimeCount_t
- RightFallTick: static TimeCount_t
- StartUSRequest: static bool
- DistReq: static bool
- AlignReq: static bool
- TMR2Period: constant uint16_t with value 0xFFFF
- PBClkFreq: constant uint32_t with value 20000000
Function: InitUSSensorService(uint8_t Priority)
Pseudo code:
1. Set TRISBbits.TRISB12 to 0 // Configure Trig pin as output
2. Set TRISBbits.TRISB2 to 1 // Configure Echo 1 pin as input
3. Set ANSELBbits.ANSB2 to 0 // Set analog input mode off for Echo 1 pin
4. Set TRISBbits.TRISB3 to 1 // Configure Echo 2 pin as input
5. Set ANSELBbits.ANSB3 to 0 // Set analog input mode off for Echo 2 pin
6. Configure Timer 2 for input capture mode:
a. Turn timer off (T2CONbits.ON = 0)
b. Use internal peripheral clock (T2CONbits.TCS = 0)
c. Disable external gating (T2CONbits.TGATE = 0)
d. Set prescale to 4 (T2CONbits.TCKPS = 2)
e. Clear timer (TMR2 = 0)
f. Load period value (PR2 = TMR2Period)
g. Set priority (IPC2bits.T2IP = 6)
h. Clear interrupt flag (IFS0CLR = _IFS0_T2IF_MASK)
i. Enable the timer interrupt (IEC0SET = _IEC0_T2IE_MASK)
7. Configure input capture 1 for Echo1 (RB2):
a. Switch off IC1 (IC1CONbits.ON = 0)
b. Configure capture rising edge first (IC1CONbits.FEDGE = 1)
c. Configure 16-bit timer resource capture (IC1CONbits.C32 = 0)
d. Select Timer2 as timing source (IC1CONbits.ICTMR = 1)
e. Interrupt on every capture event (IC1CONbits.ICI = 0)
f. Simple capture event mode - every edge (IC1CONbits.ICM = 1)
g. Configure IC1 pin (IC1R = 0b0100)
h. Set priority (IPC1bits.IC1IP = 7)
i. Clear IC1 interrupt flag (IFS0CLR = _IFS0_IC1IF_MASK)
j. Enable IC1 interrupt (IEC0SET = _IEC0_IC1IE_MASK)
8. Configure input capture 4 for Echo2 (RB3):
a. Switch off IC4 (IC4CONbits.ON = 0)
b. Configure capture rising edge first (IC4CONbits.FEDGE = 1)
c. Configure 16-bit timer resource capture (IC4CONbits.C32 = 0)
d. Select Timer2 as timing source (IC4CONbits.ICTMR = 1)
e. Interrupt on every capture event (IC4CONbits.ICI = 0)
f. Simple capture event mode - every edge (IC4CONbits.ICM = 1)
g. Configure IC4 pin (IC4R = 0b0001)
h. Set priority (IPC4bits.IC4IP = 7)
i. Clear IC4 interrupt flag (IFS0CLR = _IFS0_IC4IF_MASK)
j. Enable IC4 interrupt (IEC0SET = _IEC0_IC4IE_MASK)
9. Set MyPriority to Priority
10. Post the initial transition event:
a. Set ThisEvent.EventType to ES_INIT
b. If ES_PostToService(MyPriority, ThisEvent) returns true:
- Return true
c. Else:
- Return false
Function: RunUSSensorService(ES_Event_t ThisEvent)
Pseudo code:
1. Initialize ReturnEvent.EventType to ES_NO_EVENT
2. Switch on ThisEvent.EventType:
a. Case ES_INIT:
- // Initialize ultrasonic sensor service
- // DB_printf("Ultrasonic INIT\n");
- // ES_Timer_InitTimer(TRIG_TIMER, 10);
- // StartUSRequest = true;
b. Case ES_ALIGN_WALL_REQUEST:
- Initialize TRIG_TIMER
- Enable Timer 2 (T2CONbits.ON = 1)
- Enable input capture 1 (IC1CONbits.ON = 1)
- Enable input capture 4 (IC4CONbits.ON = 1)
- Set StartUSRequest to true
- // DB_printf("Align wall request received in USSensorService\n");
c. Case ES_TIMEOUT:
- If TRIG_TIMER is equal to ThisEvent.EventParam and StartUSRequest is true:
- // Send a 1ms pulse every 160ms to trigger the ultrasonic
- If TRIG is true:
- Initialize TRIG_TIMER to 1
- Set LATBbits.LATB12 to 1
- Set TRIG to false
- Else:
- Initialize TRIG_TIMER to 160
- Set LATBbits.LATB12 to 0
- Set TRIG to true
- If LeftDis is less than 5000:
- // DB_printf("LeftDis = %d RightDis = %d\n", LeftDis, RightDis);
- If AlignReq is true:
- If LeftDis is greater than RightDis and LeftDis is less than 3000:
- If LeftDis - RightDis is less than or equal to 150:
- Create an AlignEvent of type ES_ALIGNED_WITH_WALL
- Post AlignEvent to MainSM
- Set AlignReq to false
- Else:
- Create an AlignEvent of type ES_NOT_ALIGNED_LEFT
- Post AlignEvent to MainSM
- Else if RightDis is greater than LeftDis and LeftDis is less than 3000:
- If RightDis - LeftDis is less than or equal to 150:
- Create an AlignEvent of type ES_ALIGNED_WITH_WALL
- Post AlignEvent to MainSM
- Set AlignReq to false
- Else:
- Create an AlignEvent of type ES_NOT_ALIGNED_RIGHT
- Post AlignEvent to MainSM
- If RightDis < DistReqVal and LeftDis < DistReqVal and DistReq is true:
- Create a DistEvent of type ES_WALL_NEAR
- Post DistEvent to MainSM
- Set DistReq to false
3. Return ReturnEvent
Function: Timer2ISR
Pseudo code:
1. Disable interrupts
2. If Timer 2 interrupt flag is set:
a. Increment RolloverCount
b. Clear Timer 2 interrupt flag
3. Enable interrupts
Function: IC1ISR
Pseudo code:
1. Initialize NextEdge as RISE
2. Loop:
a. If NextEdge is RISE:
i. Set NextEdge to FALL
ii. Capture the rising edge time and update LeftRiseTick
iii. Check if Timer 2 interrupt flag is set and the captured value is less than 0x8000:
- Increment RolloverCount
- Clear Timer 2 interrupt flag
iv. Calculate EchoTime and update LeftDis
b. If NextEdge is FALL:
i. Set NextEdge to RISE
ii. Capture the falling edge time and update LeftFallTick
iii. Check if Timer 2 interrupt flag is set and the captured value is less than 0x8000:
- Increment RolloverCount
- Clear Timer 2 interrupt flag
iv. Calculate EchoTime and update LeftDis
3. Clear Input Capture 1 interrupt flag
Function: IC4ISR
Pseudo code:
1. Initialize NextEdge as RISE
2. Loop:
a. If NextEdge is RISE:
i. Set NextEdge to FALL
ii. Capture the rising edge time and update RightRiseTick
iii. Check if Timer 2 interrupt flag is set and the captured value is less than 0x8000:
- Increment RolloverCount
- Clear Timer 2 interrupt flag
iv. Calculate EchoTime and update RightDis
b. If NextEdge is FALL:
i. Set NextEdge to RISE
ii. Capture the falling edge time and update RightFallTick
iii. Check if Timer 2 interrupt flag is set and the captured value is less than 0x8000:
- Increment RolloverCount
- Clear Timer 2 interrupt flag
iv. Calculate EchoTime and update RightDis
3. Clear Input Capture 4 interrupt flag
Function: SwitchOnUSSensors
Pseudo code:
1. Initialize Timer 2 and input capture modules
2. Set StartUSRequest to true
Function: SwitchOffUSSensors
Pseudo code:
1. Set StartUSRequest to false
Function: StartDistReq
Pseudo code:
1. Set DistReq to Req
2. Set DistReqVal to Val
Function: StartAlignReq
Pseudo code:
1. Set AlignReq to Req
CL_MotorService (Leader PIC)
CL_MotorService (Leader PIC)
Receives commands from other SMs, calculates the closed loop controls, and generates the PWM commands to the motor
Pseudo Code for CL_MotorService
Pseudo Code for CL_MotorService
Module: CL_Motor Service
bool InitCL_MotorService(uint8_t Priority)
{
initialize timer for 100ms
setup motor pwm pins
setup Encoder read timer 3
setup IC 3 for motor 1
setup IC 2 for motor 2
turn timer 3, IC3, IC2 back on for encoder operation
set up timer 4 for precisely timed closed loop control operation
master enable interrupt
}
ES_Event_t RunCL_MotorService(ES_Event_t ThisEvent)
{
ES_Event_t ReturnEvent;
ReturnEvent.EventType = ES_NO_EVENT; // assume no errors
if event is time out
initialize timer for 100ms
if (M1_TimedOut)
M1_RPM_Meas = 0;
if (M2_TimedOut)
M2_RPM_Meas = 0;
if (!M1_TimedOut)
M1_TimedOut = true;
if (!M2_TimedOut)
M2_TimedOut = true;
if event is ES_DRIVE_MOTOR:
if command is CMD_FORWARD
GoForward(comfort_spd);
if command is CMD_STOP
StopMotors();
if command is CMD_REVERSE
GoBackward(comfort_spd);
if command is CMD_TURN_LEFT
Left90(comfort_spd);
if command is CMD_TURN_RIGHT
Right90(comfort_spd);
if command is CMD_SLIGHT_RIGHT
SlightRight(comfort_spd);
if command is CMD_SLIGHT_LEFT:
SlightLeft(comfort_spd);
if command is CMD_SPIN:
SpinInPlace(precision_spd);
if command is CMD_TURN_180:
Turn180(comfort_spd);
if command is CMD_REVERSE_SWEEP:
CountEncThenStop(Backup_Dist_Sweep);
if command is CMD_REVERSE_TREE:
CountEncThenStop(Backup_Dist);
if command is CMD_FORWARD_30:
CountEncThenStop(Forward30);
if command is CMD_FORWARD_148:
CountEncThenStop(Forward148);
if command is CMD_REVERSE_30:
CountEncThenStop(Forward30);
if command is CMD_FORWARD_FAST:
GoForward(gas_spd);
if command is CMD_REVERSE_FAST:
GoBackward(gas_spd);
if command is CMD_TOFENCE:
CountEncThenStop(To_Fence);
}
void StopMotors(void)
{
M1_TargetRPM = M2_TargetRPM = 0; // stop the motor
set 0 duty cycle for all pwm pins
}
void GoForward(uint16_t req_spd)
{
M1_Dir = false;
M2_Dir = false;
M1_TargetRPM = M2_TargetRPM = req_spd;
}
void GoBackward(uint16_t req_spd)
{
M1_Dir = true;
M2_Dir = true;
M1_TargetRPM = M2_TargetRPM = req_spd;
}
void Right90(uint16_t req_spd)
{
// encoder control
running_enc_count = 0;
last_enc_count = 0;
enc_goal = turn_90;
enc_ctrl = true;
// motor commands
M1_Dir = false;
M2_Dir = true;
M1_TargetRPM = M2_TargetRPM = req_spd;
}
void Left90(uint16_t req_spd)
{
// encoder control
running_enc_count = 0;
last_enc_count = 0;
enc_goal = turn_90;
enc_ctrl = true;
// motor commands
M1_Dir = true;
M2_Dir = false;
M1_TargetRPM = M2_TargetRPM = req_spd;
}
void SlightRight(uint16_t req_spd)
{
M1_Dir = false;
M2_Dir = false;
M1_TargetRPM = req_spd - slight_turn_spd;
M2_TargetRPM = req_spd + slight_turn_spd;
}
void SlightLeft(uint16_t req_spd)
{
M1_Dir = false;
M2_Dir = false;
M1_TargetRPM = req_spd + slight_turn_spd;
M2_TargetRPM = req_spd - slight_turn_spd;
}
void SpinInPlace(uint16_t req_spd)
{
M1_Dir = true;
M2_Dir = false;
M1_TargetRPM = M2_TargetRPM = req_spd;
}
void Turn180(uint16_t req_spd)
{
// encoder control
running_enc_count = 0;
last_enc_count = 0;
enc_goal = 2*turn_90;
enc_ctrl = true;
// motor commands
M1_Dir = true;
M2_Dir = false;
M1_TargetRPM = M2_TargetRPM = req_spd;
}
void ForwardDist(uint16_t EncTiks, uint16_t req_spd) // drive the robot forward according to specified encoder ticks
{
// encoder control
running_enc_count = 0;
last_enc_count = 0;
enc_goal = EncTiks;
enc_ctrl = true;
// motor commands
M1_Dir = false;
M2_Dir = false;
M1_TargetRPM = M2_TargetRPM = req_spd;
}
void CountEncThenStop(uint16_t EncTiks) // activates encoder counter according to specified encoder ticks
{
// encoder control
running_enc_count = 0;
last_enc_count = 0;
enc_goal = EncTiks;
enc_ctrl = true;
}
bool TurnChecker(void)
{
static ES_Event_t PostEvent;
static uint32_t Last;
static bool return_val;
return_val = false;
__builtin_disable_interrupts();
if (running_enc_count >= enc_goal && last_enc_count < enc_goal && enc_ctrl)
{
enc_ctrl = false; // self reset after triggering
PostEvent.EventType = ES_DRIVE_MOTOR; // Posts CMD_STOP to MotorService
PostEvent.EventParam = CMD_STOP;
PostCL_MotorService(PostEvent);
PostEvent.EventType = ES_MOVE_COMPLETE; // Posts rotate complete to HSM
PostEvent.EventParam = 0;
last_enc_count = Last = 0;
running_enc_count = 0;
PostMainSM(PostEvent);
//DB_printf("TurnChecked\n");
return_val = true;
}
last_enc_count = Last = running_enc_count;
__builtin_enable_interrupts();
return return_val;
}
// timer 3 over flow ISR
void __ISR(_TIMER_3_VECTOR, IPL6SOFT) Timer3ISR(void)
{
__builtin_disable_interrupts();
if (IFS0bits.T3IF == 1)
{
T3RolloverCount++;
IFS0CLR = _IFS0_T3IF_MASK;
}
__builtin_enable_interrupts();
}
// Motor 1 encoder ISR, updates RPM
void __ISR(_INPUT_CAPTURE_3_VECTOR, IPL7SOFT) Encoder1ISR(void)
{
static uint16_t capture;
__builtin_disable_interrupts();
M1_TimedOut = false;
do{
capture = (uint16_t)IC3BUF;
if(IFS0bits.T3IF == 1 && (capture < 0x8000))
{
T3RolloverCount++;
IFS0CLR = _IFS0_T3IF_MASK; //clear rollover flag
}
M1_ThisTime.ByBytes.rollover = T3RolloverCount;
M1_ThisTime.ByBytes.capture = capture;
M1_EncInt = M1_ThisTime.full - M1_LastTime.full;
M1_LastTime.full = M1_ThisTime.full;
// if(MotorActiveStatus.LDuty >= 0){
// leftTicks++;
// }
// else{
// leftTicks--;
// }
}while(IC3CONbits.ICBNE != 0);
IFS0CLR = _IFS0_IC3IF_MASK; //clear capture interrupt
// prevent divide by 0
if (M1_EncInt == 0)
{
M1_EncInt = 1;
}
M1_RPM_Meas = helper_int / M1_EncInt;
if (enc_ctrl) // index the turn counter iff encoder control is active
{
running_enc_count++;
}
__builtin_enable_interrupts();
//print_LEFT = true;
//edgeUpdate_LEFT = true; //new edge found
}
// Motor 2 encoder ISR, updates RPM
void __ISR(_INPUT_CAPTURE_2_VECTOR, IPL7SOFT) Encoder2ISR(void)
{
static uint16_t capture;
__builtin_disable_interrupts();
//DEBUG_PIN = 1;
M2_TimedOut = false;
do{
capture = (uint16_t)IC2BUF;
if(IFS0bits.T3IF == 1 && (capture < 0x8000))
{
T3RolloverCount++;
IFS0CLR = _IFS0_T3IF_MASK; //clear rollover flag
}
M2_ThisTime.ByBytes.rollover = T3RolloverCount;
M2_ThisTime.ByBytes.capture = capture;
M2_EncInt = M2_ThisTime.full - M2_LastTime.full;
M2_LastTime.full = M2_ThisTime.full;
// if(MotorActiveStatus.LDuty >= 0){
// leftTicks++;
// }
// else{
// leftTicks--;
// }
} while(IC2CONbits.ICBNE != 0);
IFS0CLR = _IFS0_IC2IF_MASK; //clear capture interrupt
// prevent divide by 0
if (M2_EncInt == 0)
{
M2_EncInt = 1;
}
M2_RPM_Meas = helper_int / M2_EncInt;
__builtin_enable_interrupts();
//print_LEFT = true;
//edgeUpdate_LEFT = true; //new edge found
//DEBUG_PIN = 0;
}
void __ISR(_TIMER_4_VECTOR, IPL6SOFT) Timer4ISR(void)
{
static float M1_SumError = 0.0;
static float M2_SumError = 0.0;
static float M2_RPMError;
static float M1_RPMError;
static int16_t debug_var;
IFS0CLR = _IFS0_T4IF_MASK; // clear the interrupt
// cap the rpm measured
if (M1_RPM_Meas > 2000)
{
M1_RPM_Meas = 2000;
}
if (M2_RPM_Meas > 2000)
{
M2_RPM_Meas = 2000;
}
M1_RPMError = M1_TargetRPM - M1_RPM_Meas;
M2_RPMError = M2_TargetRPM - M2_RPM_Meas;
M1_SumError += M1_RPMError;
M2_SumError += M2_RPMError;
// calculate the control effort
M1_RequestedDuty = (int16_t) (pGain * ((M1_RPMError) + (iGain * M1_SumError)));
M2_RequestedDuty = (int16_t) (pGain * ((M2_RPMError) + (iGain * M2_SumError)));
// anti-windup
if (M1_RequestedDuty > 100)
{
M1_RequestedDuty = 100;
M1_SumError -= M1_RPMError;
}
else if (M1_RequestedDuty < 0)
{
M1_RequestedDuty = 0;
M1_SumError -= M1_RPMError;
}
if (M2_RequestedDuty > 100)
{
M2_RequestedDuty = 100;
M2_SumError -= M2_RPMError;
}
else if (M2_RequestedDuty < 0)
{
M2_RequestedDuty = 0;
M2_SumError -= M2_RPMError;
}
//Set the PWM width according to control effort
if (M1_Dir)
{
PWMOperate_SetDutyOnChannel(0, 2); // stop the other channel
PWMOperate_SetDutyOnChannel(M1_RequestedDuty, 1);
}
else
{
PWMOperate_SetDutyOnChannel(0, 1); // stop the other channel
PWMOperate_SetDutyOnChannel(M1_RequestedDuty, 2);
}
if (M2_Dir)
{
PWMOperate_SetDutyOnChannel(0, 3); // stop the other channel
PWMOperate_SetDutyOnChannel(M2_RequestedDuty, 4);
}
else
{
PWMOperate_SetDutyOnChannel(0, 4); // stop the other channel
PWMOperate_SetDutyOnChannel(M2_RequestedDuty, 3);
}
}
Follower (PIC 2)
Follower (PIC 2)
Pseudo Code for ServoService
Pseudo Code for ServoService
Module: ServoService
Constants:
NUM_PINS 3
PWM_PinMap_t[NUM_PINS]: PWM_RPB15 PWM_RPB11 PWM_RPB9
PIN_1 1
PIN_2 2
PIN_3 3
Variables:
uint8_t MyPriority
PinStates[3]: uint16_t
Functions:
InitServoService(Priority): boolean
MyPriority = Priority
Post ES_INIT
PostServoService(ThisEvent): boolean
Return ES_PostToService(MyPriority, ThisEvent)
UpdatePins(void): void
Set pulse width on PIN_1 to PinStates[0]
Set pulse width on PIN_2 to PinStates[1]
Set pulse width on PIN_3 to PinStates[2]
RunServoService(ThisEvent): ES_Event_t
ReturnEvent: ES_Event_t
ReturnEvent.EventType = ES_NO_EVENT
Switch ThisEvent.EventType
Case ES_INIT:
Ports setup
Set PWM frequency to 50 Hz
Set duty cycles on all pins to 0
Case ES_SERVO_YARD:
Set PinStates[0] to ThisEvent.EventParam
Print "ES_SERVO_YARD"
UpdatePins()
break
Case ES_SERVO_ARM:
Set PinStates[1] to ThisEvent.EventParam
Print "ES_SERVO_ARM"
UpdatePins()
break
Case ES_SERVO_ROD:
Set PinStates[2] to ThisEvent.EventParam
Print "ES_SERVO_ROD"
UpdatePins()
break
End Switch
Return ReturnEvent
Pseudo Code for ReflectanceService
Pseudo Code for ReflectanceService
Module: ServoService
Constants:
FRONT_LEFT 0
FRONT_RIGHT 1
RIGHT 2
REFLECTANCE_THRESHOLD 700
Variables:
uint8_t MyPriority
AnalogValue[3]: uint32_t
OldTape[3]: bool 0 0 0
NewTape[3]: bool 0 0 0
Output: uint8_t 0xA0
Functions:
InitReflectanceService(Priority): boolean
Configure pins
MyPriority = Priority
Start 10ms timer
Post ES_INIT
PostReflectanceService(ThisEvent): boolean
Return ES_PostToService(MyPriority, ThisEvent)
RunReflectanceService(ThisEvent): ES_Event_t
ReturnEvent: ES_Event_t
ReturnEvent.EventType = ES_NO_EVENT
Switch ThisEvent.EventType
Case ES_INIT:
Case ES_TIMEOUT:
If ThisEvent.EventParam = REFLECTANCE_TIMER
Read reflectance pins
Start 100ms timer
Update NewTape using reflectance threshold and reflectance pin values
If NewTape != OldTape
OldTape = NewTape
Post ES_TRANSMIT_TO_LEADER to SPIFService with Output as param
Output = 0xA0
break
End Switch
Return ReturnEvent
Pseudo Code for TriggerService
Pseudo Code for TriggerService
Module: ServoService
Constants:
TRIGGER_PIN LATBbits.LATB10
LIMIT_PIN PORTAbits.RA2
Variables:
uint8_t MyPriority
ReadLimitSwitch: bool true
Functions:
InitTriggerService(Priority): boolean
MyPriority = Priority
Configure
Post ES_INIT
PostTriggerService(ThisEvent): boolean
Return ES_PostToService(MyPriority, ThisEvent)
RunTriggerService(ThisEvent): ES_Event_t
ReturnEvent: ES_Event_t
ReturnEvent.EventType = ES_NO_EVENT
Switch ThisEvent.EventType
Case ES_INIT:
Ports setup
Set PWM frequency to 50 Hz
Set duty cycles on all pins to 0
Case ES_TRIGGER_START:
Set TRIGGER_PIN high
break
Case ES_TIMEOUT:
If ThisEvent.EventParam = DEBOUNCING_TIMER
ReadLimitSwitch = true
break
Case ES_LIMIT_SWITCH_ON:
Set TRIGGER_PIN low
break
End Switch
Return ReturnEvent
CheckLimitSwitch(void): bool
If new limit switch state and time to read limit switch
Set last state to current state
Set return value to true
Start DEBOUNCING_TIMER at 100ms
Post ES_ServoRod
If limit switch is high
Post ES_LIMIT_SWITCH_ON to self
Post ES_TRANSMIT_TO_LEADER to SPIFService with CMD_LIMIT_SWITCH_ON as param
Pseudo Code for SPIFService
Pseudo Code for SPIFService
Module: ServoService
Constants:
TAPE_MASK 0xF0
Variables:
uint8_t MyPriority
Functions:
InitSPIFService(Priority): boolean
MyPriority = Priority
Configure pins
Configure Interrupts
Configure SPI
Post ES_INIT
PostSPIFService(ThisEvent): boolean
Return ES_PostToService(MyPriority, ThisEvent)
RunSPIFService(ThisEvent): ES_Event_t
ReturnEvent: ES_Event_t
ReturnEvent.EventType = ES_NO_EVENT
Switch ThisEvent.EventType
Case ES_INIT:
Post ES_SERVO_ARM to ServoService with ARM_IN_PWM param
Case ES_TRANSMIT_TO_LEADER:
If ThisEvent.EventParam = CMD_BEACON_E_FOUND
Send CMD_BEACON_E_FOUND response to leader PIC
else If ThisEvent.EventParam = CMD_BEACON_D_FOUND
Send CMD_BEACON_D_FOUND response to leader PIC
else If ThisEvent.EventParam = CMD_BEACON_K_FOUND
Send CMD_BEACON_K_FOUND response to leader PIC
else If ThisEvent.EventParam = CMD_BEACON_A_FOUND
Send CMD_BEACON_A_FOUND response to leader PIC
else If ThisEvent.EventParam = CMD_TAPE_SENSOR and TAPE_MASK
Send TAPE_MASK response to leader PIC
else If ThisEvent.EventParam = CMD_LIMIT_SWTICH_OFF
Send CMD_LIMIT_SWTICH_OFF response to leader PIC
else If ThisEvent.EventParam = CMD_LIMIT_SWITCH_ON
Send CMD_lIMIT_SWITCH_ON response to leader PIC
Case ES_RECEIVE_FROM_LEADER:
Switch ThisEvent.EventParam
Case ThisEvent.EventParam = CMD_READ_BEACON
Post ES_BEACON_START to BeaconService
Case ThisEvent.EventParam = CMD_ARM_IN
Post ES_SERVO_ARM to ServoService with ARM_IN_PWM param
Case ThisEvent.EventParam = CMD_ARM_OUT
Post ES_SERVO_ARM to ServoService with ARM_OUT_PWM param
Case ThisEvent.EventParam = CMD_TRIGGER_START
Post ES_TRIGGER_START to TriggerService
Case ThisEvent.EventParam = CMD_TRIGGER_END
Do nothing
End Switch
Return ReturnEvent
SDOToLeader(void): void
If the transfer buffer is empty, send response value
__ISR SPIBuffer(void): void
If receive buffer is full
Post ES_RECEIVE_FROM_BUFFER with param SPI1BUF to self
Clear mask
Pseudo Code for BeaconService
Pseudo Code for BeaconService
Module: ServoService
Constants:
Variables:
uint8_t MyPriority
uint32_t PreviousTime 0
uint16_t IC_Period 0
uint16_t Freq
uint16_t CountE 0
uint16_t CountD 0
uint16_t CountK 0
uint16_t COuntA 0
union: time
volatile uint32_t Time
Volatile uint16_t RollOver[2]
time TotalTime
Functions:
InitBeaconService(Priority): boolean
MyPriority = Priority
Configure pins
Configure Interrupts
Post ES_INIT
PostBeaconService(ThisEvent): boolean
Return ES_PostToService(MyPriority, ThisEvent)
RunBeaconService(ThisEvent): ES_Event_t
ReturnEvent: ES_Event_t
ReturnEvent.EventType = ES_NO_EVENT
Switch ThisEvent.EventType
Case ES_INIT:
Case ES_BEACON_START:
Enable corresponding interrupt capture
Case ES_BEACON_E_DETECTED:
Post TRANSMIT_TO_LEADER event with param CMD_BEACON_E_FOUND to SPIFService
Post ES_SERVO_YARD event to ServoService with YARD_ED param
Case ES_BEACON_D_DETECTED:
Post TRANSMIT_TO_LEADER event with param CMD_BEACON_D_FOUND to SPIFService
Post ES_SERVO_YARD event to ServoService with YARD_ED param
Case ES_BEACON_K_DETECTED:
Post TRANSMIT_TO_LEADER event with param CMD_BEACON_K_FOUND to SPIFService
Post ES_SERVO_YARD event to ServoService with YARD_KARL param
Case ES_BEACON_A_DETECTED:
Post TRANSMIT_TO_LEADER event with param CMD_BEACON_A_FOUND to SPIFService
Post ES_SERVO_YARD event to ServoService with YARD_KARL param
End Switch
Return ReturnEvent
__ISR InputCapture1_ISR(void): void
Read IC1BUF into TotalTime.RollOver[0] until IC1BUF is empty
Clear IC1IF mask
If timer has rolled over
Increment rollover counter TotalTime.RollOver[1]
Clear rollover interrupt
Use timer and rollover counter to create uint_32 TotalTime
Calculate IR freuqency
If IR frequency matches Beacon D
Beacon D counter ++
If beacon D counter = 10, post ES_BEACON_D_DETECTED to self
If IR frequency matches Beacon K
Beacon K counter ++
If beacon K counter = 10, post ES_BEACON_A_DETECTED to self
If IR frequency matches Beacon A
Beacon A counter ++
If beacon A counter = 10, post ES_BEACON_D_DETECTED to self
If IR frequency matches Beacon E
Beacon E counter ++
If beacon E counter = 10, post ES_BEACON_D_DETECTED to self
__ISR Timer3_ISR(void): void
Disable global interrupts
If rollover interrupt
Increment Rollover counter
Clear rollover interrupt
Enable global interrupts
Page updated
Report abuse