Embedded Files
This page details the design choices and implementation of all the software used in the completion of this project.
Given that we are using SPI communications between two PIC32s we decided to create two copies of the Framework4PIC32. One for the "Drive and Navigation" leader PIC32 called "MPFrameworkLead" and another for the "Operation and Manipulation" follower PIC32 called "MPFrameworkFollow". This helped with organization of the events and services that each PIC32 is concerned about. Throughout the following sections the system that the section is referring to will be stated explicitly. These sections include the state diagrams and pseudo-code for the Driver or Operation sub-systems and any shared libraries. All the detailed code can be found in the GitHub repository found in this link.
State Diagrams:
State Diagrams:
TopHSMPathPlanner (Drive and Navigation PIC32)
TopHSMPathPlanner (Drive and Navigation PIC32)
This is the top Hierarchical State Machine (HSM) running in the lead PIC32.
It guides the driving by responding to SPI messages. During standby does nothing, during running it is executing the plan that correspond to the index on the global sequence by running the DriverFromTo SM.
DriverFromTo HSM (Drive and Navigation PIC32)
DriverFromTo HSM (Drive and Navigation PIC32)
This is a Hierarchical State Machine (HSM) running in the lead PIC32. Could also be as FSM.
It responds to the ES_START_PLAN event from the TopHSM and runs through the steps of the desired sequence. During running it posts the desired event (from the Post Event/ Action section) upon new events that match the stopping condition, and it sends the primitive command to the DCMotorService. It also uses and communicates with (posts events to) the BeaconService and DriveCorrectionService when necessary.
Operator FSM (Operation and Manipulation PIC32)
Operator FSM (Operation and Manipulation PIC32)
This is the Finite State Machine (FSM) running in the follower PIC32.
It guides the manipulation of cargo by controlling the intake and drop off subsystems. It communicates with the DropoffLoweringArmFSM, and FeederService.
Arm FSM (Operation and Manipulation PIC32)
Arm FSM (Operation and Manipulation PIC32)
This is a Finite State Machine (FSM) running in the follower PIC32.
It responds to the either ES_RAISE_ARM or ES_LOWER_ARM from the Operator FSM and it steps through a corresponding sequence of joint angles in the two servos every given time. It exits once the sequence is completed with the ES_ARM_MOVED event which either comes from a limit switch or the end of a sequence of angles.
Pseudo-code:
Pseudo-code:
SPI Communication:
SPI Communication:
For our state machines we need limited communication between the two PIC32s. In total we have about 15 defined commands that trigger transitions using information from the other PIC32. For that reason on the leader side we are using a simple timer to trigger a send message (SPIOperate_SPI1_Send8(message2send);) and an Event Checker on the follower for (SPIOperate_HasSS1_Risen()) to read a new message like (uint8_t newMessage = (uint8_t)SPIOperate_ReadData(SPI_SPI1);) when the SPI chip select line has risen indicating a tranmission was completed. Then, in order to organize and send the commands we have ES events for commands to be sent (ES_NEW_SPI_CMD_SEND) or indicate that we received a command (ES_NEW_SPI_CMD_RECEIVED) for which we encode the 8 bit message in the event parameter. The command values for the event parameters are defined in the SPI header so that any other service can see them and use them with ease.
SPIService.h
SPIService.h
// Defining SPI commands shorthand in header for easy access across services
/* LAB 8 Keyboard COMMANDS */
#define LAB8_FWD_FULL 0x09 // 'w'
#define LAB8_REV_FULL 0x11 // 's'
#define LAB8_ROT_CCW_90 0x04 // 'a'
#define LAB8_ROT_CW_90 0x02 // 'd'
#define LAB8_STOP 0x00 // 'x'
#define LAB8_CW_BEACON 0x20 // 'b'
/* LEAD2FOLLOW SPI CMDS*/
#define CMD_SPI_LEAD_INITIAL 0xAA
#define CMD_SPI_INTAKE_ON 0xA1
#define CMD_SPI_DROPOFF_REACHED 0xA2
#define CMD_SPI_BLUE_TEAM 0xA3
#define CMD_SPI_GREEN_TEAM 0xA4
#define CMD_SPI_RELEASE_DROPOFF 0xA5
/* FOLLOW2LEAD SPI CMDS*/
#define CMD_SPI_FOLLOW_INITIAL 0xFF
#define CMD_SPI_START 0xB1
#define CMD_SPI_INTAKE_INCOMPLETE 0xB2
#define CMD_SPI_LOADED 0xB3
#define CMD_SPI_UNLOADED 0xB4
#define CMD_SPI_RELEASED 0xB8
#define CMD_SPI_UNLOAD_INCOMPLETE 0xB5
#define CMD_SPI_END 0xB6
#define CMD_SPI_START_DEATHMATCH 0xB7
SPIService.c (Lead, Drive and Navigation)
SPIService.c (Lead, Drive and Navigation)
Module-level variables: MyPriority, message2send
/*------------------------------ Module Code ------------------------------*/
Function InitSPILeadService(Priority)
set MyPriority to Priority
Initialize SPI using PIC32_SPI_HAL calls: lead, clock idle high, transmit on active to idle
Initialize message2send to CMD_SPI_LEAD_INITIAL as default message
// Initialize timers and variables
ES_Timer_InitTimer(SPI_TIMER, SPI_TIMER_MS);
message2send = CMD_SPI_LEAD_INITIAL;
Post ES_INIT event to this queue
Return true if the event posted successfully, false otherwise
Function PostSPILeadService(ThisEvent)
Post event to this queue
Function RunSPILeadService(ThisEvent)
Initialize ReturnEvent as ES_NO_EVENT
switch (ThisEvent.EventType)
case ES_INIT:
// Do nothing, announce
case ES_TIMEOUT: // SPI timer
if (ThisEvent.EventParam is SPI_TIMER)
set static local variable LastMessage to CMD_SPI_FOLLOW_INITIAL
if (SPIOperate_HasSS1_Risen()) // Transmission completed
set local variable newMessage to value from SPI buffer (SPI_Operate_ReadData(SPI_SPI1))
// check if message different and not bad command
if ((newMessage is not LastMessage) and (newMessage is not CMD_SPI_FOLLOW_INITIAL))
Create New ES_NEW_SPI_CMD_RECEIVED event
Set newMessage as NewEvent param
Post NewEvent to all
set LastMessage to newMessage for next call
// write the next message to be sent into the buffer
SPIOperate_SPI1_Send8(message2send);
break;
case ES_NEW_SPI_CMD_SEND: // Posted from anywhere that wants to send a message
set message2send to this event’s event param
break;
return ReturnEvent
SPIService.c (Follow, Operation and Manipulation)
SPIService.c (Follow, Operation and Manipulation)
Module-level variables: MyPriority, message2send
/*------------------------------ Module Code ------------------------------*/
Function InitSPIFollowService(Priority)
set MyPriority to Priority
Initialize SPI using PIC32_SPI_HAL calls: follow, clock idle high, transmit on active to idle
Initialize message2send to CMD_SPI_FOLLOW_INITIAL as as default message
Post ES_INIT event to this queue
Return true if the event posted successfully, false otherwise
Function PostSPIFollowService(ThisEvent)
Post event to this queue
Function RunSPIFollowService(ThisEvent)
Initialize ReturnEvent as ES_NO_EVENT
switch (ThisEvent.EventType)
case ES_INIT:
// Initialize buffer with message2send
case ES_SPI_COMPLETE: // Posted from EventChecker
set static local variable LastMessage to CMD_SPI_LEAD_INITIAL
set local variable newMessage to ThisEvent.EventParam
// check if message different and not bad command
if ((newMessage is not LastMessage) and (newMessage is not CMD_SPI_LEAD_INITIAL))
Create New ES_NEW_SPI_CMD_RECEIVED event
Set newMessage as NewEvent param
Post NewEvent to all
set LastMessage to newMessage for next call
// write the next message to be sent into the buffer
SPIOperate_SPI1_Send8(message2send);
break;
case ES_NEW_SPI_CMD_SEND: // Posted from anywhere that wants to send a message
set message2send to this event’s event param
break;
return ReturnEvent
Drive and Navigation:
Drive and Navigation:
Consists of two main HSMs. One that does top level plan execution and one that does lower level step execution. Some of these transitions are done upon receiving SPI messages as described in the SPI communications section. The steps in the Drive And Navigation PIC involve looking for the beacons, using simple commands to drive, and follow the line or use encoders to drive. For all these we used the following: SPILeadService, TOPHsmPathPlanner, DriverFromToHSm, BeaconService, DCMotorService, and DriveCorrectionService.
Event Checkers (Drive and Navigation)
Event Checkers (Drive and Navigation)
Event checkers for line detectors and limit switch on the arm. The line detection event checkers also indicate when the bot is centered (both left and right are on the tape).
/*** EVENT CHECKER: Keyboard Input ***/
/* Description:
Reads Keyboard inputs
Posts an ES_NEW_KEY event to test events, inputs or SPI messages.
*/
Function Check4Keystroke
if (IsNewKeyReady()) // new key waiting?
Create ThisEvent
Set EventType to ES_NEW_KEY
Set EventParam to value from GetNewKey()
Post ThisEvent to all services
return true
end if
return false
/*** EVENT CHECKER: Tape Sensors at Pivot Input ***/
/* Description:
Reads LINE_PIVOT_L_PIN and LINE_PIVOT_R_PIN inputs
Posts an ES_LINE_PIVOT_L, ES_LINE_PIVOT_R or ES_CENTERED_PIVOT event indicate which sensor(s) are on the tape
*/
Function Check4TapePivot
set ReturnValue to false
set static variable Initialized to false
// Initialize static variables to keep track of which ones are one
set static variable centered to false
set static variable leftOnLine to false
set static variable rightOnLine to false
set static variable LastPinLState to 0
set static variable LastPinRState to 0
Declare CurrentPinLState
Declare CurrentPinRState
if (is Not Initialized)
// Disable analog, set input, set low both pivot sensors
LINE_PIVOT_L_ANSEL = 0; // digital mode
LINE_PIVOT_L_TRIS = 1; // input
LINE_PIVOT_L_LAT = 0; // set low initially
LINE_PIVOT_R_ANSEL = 0; // digital mode
LINE_PIVOT_R_TRIS = 1; // input
LINE_PIVOT_R_LAT = 0; // set low initially
Set Initialized to true
read CurrentPinLState from LINE_PIVOT_L_PIN
read CurrentPinRState from LINE_PIVOT_R_PIN
// check for pin event AND different from last time
if (CurrentPinLState is NOT LastPinLState or CurrentPinRState is NOT LastPinRState) {
if (CurrentPinLState is equal to LINE_PIVOT_L_EVENT) // event detected
Set leftOnLine to true
Create ThisEvent
Set EventType to ES_LINE_PIVOT_L
Post ThisEvent to all services
Set ReturnValue to true
else
Set leftOnLine to false
Set centered to false
// Same but for right side now
if (CurrentPinRState is NOT LastPinRState) {
if (CurrentPinRState is equal to LINE_PIVOT_R_EVENT) // event detected
Set rightOnLine to true
Create ThisEvent
Set EventType to ES_LINE_PIVOT_R
Post ThisEvent to all services
Set ReturnValue to true
else
Set rightOnLine to false
Set centered to false
// if there’s been a change and now both are on the line
if ((ReturnVal) and (leftOnLine and rightOnLine) are true){
Create ThisEvent
Set EventType to ES_CENTERED_PIVOT
Post ThisEvent to all services
Set ReturnValue to true
LastPinLState = CurrentPinLState; // update the state for next time
LastPinRState = CurrentPinRState; // update the state for next time
return ReturnVal
/*** EVENT CHECKER: Tape Sensors in Front Input ***/
/* Description: Basically same as previous event checker but for the other 2 sensors
Reads LINE_FRONT_L_PIN and LINE_FRONT_R_PIN inputs
Posts an ES_LINE_FRONT_L, ES_LINE_FRONT_R or ES_CENTERED event indicate which sensor(s) are on the tape
*/
Function Check4TapeFront
set ReturnValue to false
set static variable Initialized to false
// Initialize static variables to keep track of which ones are one
set static variable centered to false
set static variable leftOnLine to false
set static variable rightOnLine to false
set static variable LastPinLState to 0
set static variable LastPinRState to 0
Declare CurrentPinLState
Declare CurrentPinRState
if (is Not Initialized)
// Disable analog, set input, set low both pivot sensors
LINE_FRONT_L_ANSEL = 0; // digital mode
LINE_FRONT_L_TRIS = 1; // input
LINE_FRONT_L_LAT = 0; // set low initially
LINE_FRONT_R_ANSEL = 0; // digital mode
LINE_FRONT_R_TRIS = 1; // input
LINE_FRONT_R_LAT = 0; // set low initially
Set Initialized to true
read CurrentPinLState from LINE_FRONT_L_PIN
read CurrentPinRState from LINE_FRONT_R_PIN
// check for pin event AND different from last time
if (CurrentPinLState is NOT LastPinLState or CurrentPinRState is NOT LastPinRState) {
if (CurrentPinLState is equal to LINE_FRONT_L_EVENT) // event detected
Set leftOnLine to true
Create ThisEvent
Set EventType to ES_LINE_FRONT_L
Post ThisEvent to all services
Set ReturnValue to true
else
Set leftOnLine to false
Set centered to false
// Same but for right side now
if (CurrentPinRState is NOT LastPinRState) {
if (CurrentPinRState is equal to LINE_FRONT_R_EVENT) // event detected
Set rightOnLine to true
Create ThisEvent
Set EventType to ES_LINE_FRONT_R
Post ThisEvent to all services
Set ReturnValue to true
else
Set rightOnLine to false
Set centered to false
// if there’s been a change and now both are on the line
if ((ReturnVal) and (leftOnLine and rightOnLine) are true){
Create ThisEvent
Set EventType to ES_CENTERED
Post ThisEvent to all services
Set ReturnValue to true
LastPinLState = CurrentPinLState; // update the state for next time
LastPinRState = CurrentPinRState; // update the state for next time
return ReturnVal
/****************************************************************************
Function
Check4Limit
Checks for when the limit switch for changes. Posts ES_LIMIT
events to all when a change is detected.
****************************************************************************/
Function Check4Limit
set ReturnValue to false
set static variable Initialized to false
// Initialize static variables
set static variable LastLimitState to true
Declare CurrentLimitState
if (is Not Initialized)
// Disable analog, set input, low initially
LIMIT_TRIS = 1; // input
LIMIT_LAT = 1; // set high initially
Set Initialized to true
read CurrentLimitState from LIMIT_IN_PIN
//compare to static LastButtonState
if ((CurrentLimitState is not equal to LastLimitState) and (CurrentLimitState is 0) )
set ReturnValue to true
Create ThisEvent
Set EventType to ES_LIMIT_SWITCH
Post ThisEvent to all services
end if
update LastLimitState to CurrentLimitState
return ReturnValue
TopHSMPathPlanner (Drive and Navigation)
TopHSMPathPlanner (Drive and Navigation)
It guides the driving by responding to SPI messages. During standby does nothing, during running it is executing the plan that correspond to the index on the global sequence by running the DriverFromTo SM.
Module-level variables: CurrentState, MyPriority, PlanIndex = 0, DEATHMATCH_MODE = 0
/*------------------------------ Module Code ------------------------------*/
Function InitMasterSM(Priority)
Declare ThisEvent
set ThisEvent’s type to ES_ENTRY
set MyPriority to Priority
// Start the Master State machine
StartMasterSM( ThisEvent )
// Start Lower Level SM
StartDriverFromToSM( ThisEvent )
return true
Function PostMasterSM(ThisEvent)
Post event to this queue
Function RunMasterSM(ThisEvent)
Set MakeTransition to false
Set NextState to CurrentState
Set EntryEventKind to { ES_ENTRY, 0 };// default to normal entry to new state
Set ReturnEvent to { ES_NO_EVENT, 0 }; // assume no error
switch ( CurrentState )
case STANDBY_PLANNER :
if ( CurrentEvent.EventType is NOT ES_NO_EVENT ) //If an event is active
switch (CurrentEvent.EventType)
case ES_NEW_SPI_CMD_RECEIVED:
switch(CurrentEvent.EventParam)
case CMD_SPI_START:
Set PlanIndex to 0
Make Transition to RUNNING_PLAN
Set EntryEventKind.EventType to ES_ENTRY
break;
case CMD_SPI_START_DEATHMATCH:
Set DEATHMATCH_MODE to true
Set PlanIndex to 0
Make Transition to RUNNING_PLAN
Set EntryEventKind.EventType to ES_ENTRY
break;
case CMD_SPI_LOADED:
case CMD_SPI_UNLOADED:
//If there are still plans, increment planIndex and transition to RUNNING_PLAN
break;
default:
break;
End switch on EventParam
End switch on EventType
End if on ES_NO_EVENT
break; //break on STANBY_PLANNER STATE
case RUNNING_PLAN:
Run DuringRunningPlan(CurrentEvent) and set CurrentEvent to the return value
//process any events
if ( CurrentEvent.EventType is NOT ES_NO_EVENT ) //If an event is active
switch (CurrentEvent.EventType)
case ES_PLAN_DONE:
Make Transition to STANDBY_PLANNER
Set EntryEventKind.EventType to ES_ENTRY
break;
End switch on EventType
End if on ES_NO_EVENT
// If we are making a state transition
if (MakeTransition == true)
// NOTHING HAPPENS ON EXIT
// Execute entry function for new state
RunMasterSM(EntryEventKind);
return(ReturnEvent)
Function StartMasterSM ( CurrentEvent )
Set CurrentState to STANDBY_PLANNER at start.
// now we need to let the Run function init the lower level state machines
RunMasterSM(CurrentEvent);
return
Function DuringStandbyPlanner( Event)
On ENTRY Do:
// Turn motors off
create ThisEvent
set event type to ES_MOTORS_OFF
Post ThisEvent to DCMotorService to ensure that motors are off during Standby
On EXIT nothing to do:
On During, also nothing to do during Standby.
return(ReturnEvent)
Function DuringRunningPlan( Event)
On ENTRY Do:
// implement any entry actions required for this state machine
Create StartPlanEvent
If DEATHMATCH_MODE, set Event Type to ES_START_DM_PLAN, or ES_START_PLAN otherwise
Set EventParam to current PlanIndex value
// after that start any lower level machines that run in this state
RunDriverFromToSM(StartPlanEvent);
On EXIT nothing to do:
On During Do:
Set ReturnEvent to output from lower machine: RunDriverFromToSM(Event)
return(ReturnEvent)
DriverFromTo HSM (Drive and Navigation)
DriverFromTo HSM (Drive and Navigation)
It responds to the ES_START_PLAN event from the TopHSM and runs through the steps of the desired sequence. During running it posts the desired event (from the Post Event/ Action section) upon new events that match the stopping condition, and it sends the primitive command to the DCMotorService. It also uses and communicates with the BeaconService and DriveCorrectionService when
Module-level variables: CurrentState, MyPriority, *ActivePlan = 0, StepCounter = 0, CurrentPlanIndex, CurrentDMPlanIndex
/*------------------------------ Module Code ------------------------------*/
Function RunDriverFromToSM(ThisEvent)
Set MakeTransition to false
Set NextState to CurrentState
Set EntryEventKind to { ES_ENTRY, 0 };// default to normal entry to new state
Set ReturnEvent to { ES_NO_EVENT, 0 }; // assume no error
switch ( CurrentState )
case STANDBY_STEP :
// Run during function for this state
ReturnEvent = CurrentEvent = DuringStandbyStep(CurrentEvent);
if ( CurrentEvent.EventType is NOT ES_NO_EVENT ) //If an event is active
switch (CurrentEvent.EventType)
case ES_START_PLAN:
Set CurrentPlanIndex to the value from the CurrentEvent EventParam
Set the ActivePlan pointer to the Plans defined at the CurrentPlanIndex
Make Transition to RUNNING_STEP
Set Step Counter to 0
Set EntryEventKind.EventType to ES_ENTRY
Keep ReturnEvent.EventType to ES_NO_EVENT
break;
case ES_START_DM_PLAN:
Set CurrentDMPlanIndex to the value from the CurrentEvent EventParam
Set the ActivePlan pointer to the DM Plans defined at the CurrentDMPlanIndex
Make Transition to RUNNING_STEP
Set Step Counter to 0
Set EntryEventKind.EventType to ES_ENTRY
Keep ReturnEvent.EventType to ES_NO_EVENT
break;
End switch on EventType
End if on ES_NO_EVENT
break; //break on STANBY_STEP STATE
case RUNNING_STEP:
// Run during function for this state
ReturnEvent = CurrentEvent = DuringRunningStep(CurrentEvent);
//process any events
if ( CurrentEvent.EventType is NOT ES_NO_EVENT ) //If an event is active
switch (CurrentEvent.EventType)
case ES_PLAN_DONE:
Make Transition to STANDBY_STEP
Set EntryEventKind.EventType to ES_ENTRY
break;
End switch on EventType
End if on ES_NO_EVENT
// If we are making a state transition
if (MakeTransition == true)
// NOTHING HAPPENS ON EXIT
Set CurrentState to NextState; //Modify state variable
// Execute entry function for new state
RunDriverFromToSM(EntryEventKind);
return(ReturnEvent)
Function StartDriverFromToSM( CurrentEvent )
Set CurrentState to STANDBY_PLANNER at start.
// call the entry function (if any) for the ENTRY_STATE
RunDriverFromToSM(CurrentEvent);
StepCounter = 0; // initialize variable
return
Function DuringStandbyStep( Event)
On ENTRY Do:
// Turn motors off
create ThisEvent
set event type to ES_MOTORS_OFF
Post ThisEvent to DCMotorService to ensure that motors are off during Standby
On EXIT nothing to do:
On During, also nothing to do during Standby.
return(ReturnEvent)
Function DuringRunningStep( Event)
On ENTRY Do:
// implement any entry actions required for this state machine
// Post primitive event to DCMotorService, post event from the plan sequence at this index,if the event to post is a TIMEOUT, initialize the timer with correct time
On EXIT nothing to do:
On During Do:
Check that the ActivePlan pointer is not NULL,
Then check if the current event matches the current index stopping condition
If it matches increase the stepCounter and Do the same step action that where done on entry:
// Post primitive event to DCMotorService, post event from the plan sequence at this index,if the event to post is a TIMEOUT, initialize the timer with correct time
Also set ReturnEvent to ES_NO_EVENT
If the stepCounter reaches the number of steps, NumSteps
change the ReturnEvent to ES_PLAN_DONE
return(ReturnEvent)
PlansAndSteps.h and PlansAndSteps.c (Drive and Navigation)
PlansAndSteps.h and PlansAndSteps.c (Drive and Navigation)
Documents to store the differnt plans and steps that are called from the DriverFromTo SM and the TopHSM PathPlaner HSM.
/*
Header:
Declares an enum for the primitive cmds (Forwards, Backwards, Rotate, etc.): PrimitiveCmd_t
Declares a struct for the Steps.
Includes: PrimitiveCmd_t, ES_Event_t StoppingCondition, ES_Event_t PostEvent
Declares an enum for the plans, to keep track of on which plan we are on: PlanIndex_t
Defines a struc to keep plans and the respective number of steps in each plan: Plan_t
Declares extern const Plan_t Plans[NUM_PLANS]; to be used for the plans
Module:
Defines the individual plans with their steps:
i.e. const PlanStep_t StartPos2LoadingDockSeq[] = { ... };
Puts the plans into Plans (declared in the header):
const Plan_t Plans[NUM_PLANS] =
{
[PLAN_STARTPOS2LOADINGDOCK] =
{
.Steps = StartPos2LoadingDockSeq,
.NumSteps = sizeof(StartPos2LoadingDockSeq)/sizeof(PlanStep_t)
},
// ...
// ...
};
*/
BeaconService (Drive and Navigation)
BeaconService (Drive and Navigation)
This service uses Timer 3 with Input Capture 3 on a 1:2 prescaler to determine the presence and frequency of the beacons. It also has flexibility to store and analize a sequence of beacons to determine on which side of the field we are on. It posts ES_BEACON_DISPENSER to indicate that one of the dispenser beacons was seen, ES_SIDE_FOUND with either BLUE or GREEN as parameter, and it can also post the ES_NEW_SPI_CMD_SEND with either CMD_SPI_GREEN_TEAM or CMD_SPI_BLUE_TEAM as parameter to the SPILeadService to send it over to the follower.
Module-level variables: MyPriority, rollover, periodInTicks, lastCapture, Union for time and rollover, GreenSequence[], BlueSequence[], BeaconBuffer[], BufferIndex = 0, BufferFilled = 0
/*------------------------------ Module Code ------------------------------*/
Function InitBeaconService(Priority)
set MyPriority to Priority
Initialize IC3 using our PIC32_IC_Lib calls:
configure timer, enable channel, map channel to pin
// Initialize timers and variables
// Timer for how often to check the frequency that the Input Capture calculated
ES_Timer_InitTimer(BeaconDetectTimer, BEACON_TIMER_MS);
Post ES_INIT event to this queue
Return true if the event posted successfully, false otherwise
Function PostBeaconService(ThisEvent)
Post event to this queue
Function RunBeaconService(ThisEvent)
Initialize ReturnEvent as ES_NO_EVENT
switch (ThisEvent.EventType)
case ES_INIT:
// Do nothing, announce
case ES_TIMEOUT: // SPI timer
if (ThisEvent.EventParam is BeaconDetectTimer)
set static local variable LastBeaconState to 0
Declare CurrentBeaconState and CurrentBeaconFreq
Set ReturnVal to false
Set the CurrentBeaconFreq to the result from the ticks from periodInTicks from ISR
Compare the CurrentBeaconFreq with all the beacon nominal frequencies (+- some tolerance)
Set the CurrentBeaconState to the beacon found or 0 if none
Run the CheckSide logic and post events accordingly
Set LastBeaconState to CurrentBeaconState
Reinitialize the timer (ES_Timer_InitTimer(BeaconDetectTimer, BEACON_TIMER_MS))
break;
return ReturnEvent
// From class labs basically
Function __ISR(_INPUT_CAPTURE_3_VECTOR, IPL7SOFT) ICHandler(void)
Set capture to the value from the buffer, IC3BUF;
while (IC3CONbits.ICBNE == 1)
IC3BUF; // clear the buffer
IC_ClearInterruptFlag(IC3);
// Handle rollover
if (IFS0bits.T3IF == 1 && capture < (PR3 / 2))
rollover++;
IFS0CLR = _IFS0_T3IF_MASK;
}
TimeUnion currentTime;
currentTime.byBytes[0] = capture;
currentTime.byBytes[1] = rollover;
if ((currentTime.time - lastCapture) > 0)
periodInTicks = (currentTime.time - lastCapture); // One tick is 0.1us = 1/10 MHz
lastCapture = currentTime.time; // update the last capture value
Function __ISR(_TIMER_3_VECTOR, IPL6SOFT) Timer3ISR(void)
if (IFS0bits.T3IF == 1)
rollover++;
IFS0CLR = _IFS0_T3IF_MASK;
return
DCMotorService (Drive and Navigation)
DCMotorService (Drive and Navigation)
This service uses Timer 2 with Output Compares 2 and 3 for the left and right DC motors on the drive train, respectively.
Module-level variables: MyPriority
/*------------------------------ Module Code ------------------------------*/
Function InitDCMotorService(Priority)
set MyPriority to Priority
Initialize OCs 2 and 3 on timer 2 using our PIC32_PWM_Lib calls:
configure timer, set channel, assign channel to timer, map channeltoOutputPin,
Initialize analog, digital, output for all the motor pins
Set the dutyCycle in both motors to 0, to ensure they start off
Post ES_INIT event to this queue
Return true if the event posted successfully, false otherwise
Function PostDCMotorService(ThisEvent)
Post event to this queue
Function RunDCMotorService(ThisEvent)
Initialize ReturnEvent as ES_NO_EVENT
switch (ThisEvent.EventType)
case ES_INIT:
// Do nothing, announce
break;
case ES_MOTORS_OFF:
// Stop the robot, set duty cycle to 0.
break;
case ES_MOTOR_PRIMITIVE:
Execute primitive according to ThisEvent.EventParam
(Function that maps forwards, backwards, rotateCW, etc. with corresponding duty cycles)
For primitives that use line following or encoder counts, post corresponding events to DriveCorrectionService to start that functionality
break;
return ReturnEvent
DriveCorrectionService (Drive and Navigation)
DriveCorrectionService (Drive and Navigation)
It uses the line sensors and motor encoders to adjust the motors' duty cycles to drive straight or for a fixed number of counts. Note that it uses interrupts 3 and 4 to right and left encoder counts respectively, as well as Timer 4 for line following control loop timing.
Module-level variables: MyPriority, CurrentState, Error, Integral, EncCountL, EncCountR, LineError, LineIntegral, MidCount
/*------------------------------ Module Code ------------------------------*/
Function InitDriveCorrectionService(Priority)
set MyPriority to Priority
set CurrentState to DC_Idle
Initialize interrupts for the encoders and line sensors
set as digital, input, INT4R for left encoder, INT3R for right encoder
set flag to 0, enable interrupt, set priority to 5
Set timer 4 to use for line following error accumulation calculations
Initialize timer that does encoder count check
Post ES_INIT event to this queue
Return true if the event posted successfully, false otherwise
Function PostDriveCorrectionService(ThisEvent)
Post event to this queue
Function RunDriveCorrectionService(ThisEvent)
Initialize ReturnEvent as ES_NO_EVENT
// Always respond to events regardless of state
switch (ThisEvent.EventType)
case ES_TIMEOUT: // Control timer
if (ThisEvent.EventParam is DriveCorrectionTimer)
Restart timer
case ES_START_ENC_FWD:
Clear errors, integral terms and counts
Set state to DC_EncFwd to drive forward using encoder counts
break;
case ES_START_ENC_REV:
Clear errors, integral terms and counts
Set state to DC_LineFwdMid to drive backwards using encoder counts
break;
case ES_START_LINE_FWD:
Clear errors, integral terms and counts
Set state to DC_LineFwdMid to drive forward using line following
break;
case ES_START_LINE_REV:
Clear errors, integral terms and counts
Set state to DC_LineFwdMid to drive backwards using line following
break;
// Apply control loop
switch (CurrentState)
case DC_EncFwd:
Set error to difference between Left and Right encoder counts
Get desired duty cycle from based on zero error between counts, increase duty cycle for side with less counts, decrease duty cycle for side with higher counts
Check if the average of the two counts exceeds the desired counts (distance)
Then post ES_COUNT_DONE and stop motors
break;
case DC_EncRev:
Set error to difference between Left and Right encoder counts
Get desired duty cycle from based on zero error between counts, increase duty cycle for side with less counts, decrease duty cycle for side with higher counts
Check if the average of the two counts exceeds the desired counts (distance)
Then post ES_COUNT_DONE and stop motors
break;
case DC_LineFwdMid:
// Line following control loop handled in interrupts
Check if the average of the encoder counts exceeds the desired counts (distance)
Then post ES_COUNT_DONE and stop motors
break;
case DC_LineRevMid:
// Line following control loop handled in interrupts
Check if the average of the encoder counts exceeds the desired counts (distance)
Then post ES_COUNT_DONE and stop motors
break;
return ReturnEvent
Function __ISR(_EXTERNAL_4_VECTOR, IPL5SOFT) _EncoderL_ISR:
Clear interrupt 4 flag
Increase EncCountL
Function __ISR(_EXTERNAL_3_VECTOR, IPL5SOFT) _EncoderR_ISR:
Clear interrupt 3 flag
Increase EncCountR
// Line-following ISR mini PI
Function __ISR(_TIMER_4_VECTOR, IPL4SOFT) _Line_Timer_ISR(void)
Clear Timer 4 interpret flag
// Only do things if on line following states
if(CurrentState is not DC_LineFwdMid and
CurrentState is not DC_LineRevMid)
return;
Read left and right line following sensors
if(Left is not on the line and right is)
Increase LineError by +1
else if(Left is on the line and right is not)
Decrease LineError by -1
else // both are on the line
Don’t change LineError
Calculate desired left and right motor duty cycles
(Calculated with PI control, output from the controller is trimmed to valid duty cycle range and then added to one side and subtracted from the other appropriately)
if(CurrentState is DC_LineFwdMid)
Update duty cycle for forward direction
else
Update duty cycle for backward direction
Operation and Manipulation:
Operation and Manipulation:
Consists of two main FSMs. One that does operates the intake and dispensing of cargo and keeps track of the game state (between standy, drop off, intaking, etc.) and one that does the 2 jount arm lowering or raising motions. Some of these transitions are done upon receiving SPI messages as described in the SPI communications section. The operator FSM makes calls to the following: SPIFollowService, DropOffLoweringArmFSM, FieldSideServoService, and FeederService. Note: the IntakeService (turning intake motors on and off) was discarded and its functionality was absorbed by operator FSM.
Event Checkers (Operation and Manipulation)
Event Checkers (Operation and Manipulation)
Event checkers for the start button, and for the SPI chip select line in our SPI communication protocol.
/*** EVENT CHECKER: Start Button ***/
Function Check4StartButton
set ReturnValue to false
set static variable Initialized to false
if (is Not Initialized)
// Disable analog, set input, set low
BUTTON_TRIS = 1; // input
BUTTON_LAT = 0; // set low initially
Initialized = true;
set static variable LastButtonState to 1(assumed not pressed)
read CurrentButtonState from button input pin
compare to static LastButtonState
if (CurrentButtonState is not equal to LastButtonState and CurrentButtonState is 0)
set ReturnValue to true
create new event
set event type to ES_START_DOWN
set event parameter to CurrentButtonState
post event to PostOperatorFSM
set ReturnValue to true
end if
update LastButtonState to CurrentButtonState
return ReturnValue
/*** EVENT CHECKER: SPI Follow ***/
Function Check4SPI
if (SPIOperate_HasSS1_Risen()) // chip select went high, which means a transmission was completed
Set newMessage to value from SPI1 buffer, SPIOperate_ReadData(SPI_SPI1);
create new event
set event type to ES_SPI_COMPLETE
set event parameter to newMessage
post event to all
return true
end if
return false
Operator FSM (Operation and Manipulation)
Operator FSM (Operation and Manipulation)
It guides the manipulation of coal by controlling the intake and drop off subsystems. It communicates with the DropoffLoweringArmFSM, and FeederService.
Module-level variables: MyPriority, CurrentState, carrying, gameCounter, GameMode, Strategy
/*------------------------------ Module Code ------------------------------*/
Function InitOperatorFSM(Priority)
set MyPriority to Priority
set CurrentState to OperatorInitPState
Post ES_INIT event to this queue
Return true if the event posted successfully, false otherwise
Function PostOperatorFSM(ThisEvent)
Post event to this queue
Function RunOperatorFSM(ThisEvent)
Initialize ReturnEvent as ES_NO_EVENT
switch (ThisEvent.EventType)
case ES_TIMEOUT: // Always handle this overall game timeout timer
// uses a 21800 MS timer counted 10 times to indicate the 2:18s of gametime
if (ThisEvent.EventParam is GAME_TIMER)
Keep track of how many times it has looped
Increase gameCounter by 1
if (gameCounter > 10)
Stop all timers, all motors, send end game message over SPI
Reset variables for next game
Set currentState to Standby
else re-start the timer for next interval
break;
switch (CurrentState)
case OperatorInitPState:
if (ThisEvent.EventParam is ES_INIT)
// Initialize variables and subsystems
Set carrying, gameMode, Strategy and gameCounter to 0
Initialize agitator: set to digital output, starts off
Initialize intake: Use Timer 2 at 10kHz, set channel 4, map it to RB2, set to digital output, set to 0 duty cycle to start
Set up interrupts for the distance sensors that detect cargo in or out (INT3 and INT1), both with priority 5
break;
case Standby:
switch (ThisEvent.EventType)
case ES_START_DOWN:
Set carrying to 0
Set GameMode to input from RB4 (deathmatch or normal game switch)
Set gameCounter to 0 and start GAME_TIMER with 21800ms
Create ES_NEW_SPI_CMD_SEND event
Set param to either CMD_SPI_START or CMD_SPI_START_DEATHMATCH depending on mode
Post event to SPIService, so that it can be sent to Leader PIC
Set CurrentState to WaitingForNavigation
break;
break;
case WaitingForNavigation:
switch (ThisEvent.EventType)
case ES_NEW_SPI_CMD_RECEIVED:
if (ThisEvent.EventParam is CMD_SPI_INTAKE_ON)
Start INTAKE_PACE_TIMER for 5 seconds
Set CurrentState to Intaking
Turn the intake on by setting the PWM to the desired value
else if (ThisEvent.EventParam is CMD_SPI_DROPOFF_REACHED)
Start DROPOFF_PACE_TIMER for 7 seconds
Set CurrentState to LoweringDropoff
Post ES_START_LOWERING_ARM event to the Arm FSM
else if (ThisEvent.EventParam is CMD_SPI_RELEASE_DROPOFF)
Set CurrentState to RaisingDropoff
Post ES_START_RAISING_ARM event to the Arm FSM
Break;
break;
case Intaking:
switch (ThisEvent.EventType)
case ES_CARGO_IN:
Increase carrying count by one
if (carrying is less than the max capacity)
Re-start the timer to allow enough time to continue intaking
else // reached intake capacity
Stop the intake
Stop the INTAKE_PACE_TIMER
Create ES_NEW_SPI_CMD_SEND event
Set param to CMD_SPI_LOADED
Post event to SPIService, so that it can be sent to Leader PIC
Set CurrentState to WaitingForNavigation
break;
case ES_TIMEOUT:
if (ThisEvent.EventParam is INTAKE_PACE_TIMER) // Intake failed
Stop the intake
Create ES_NEW_SPI_CMD_SEND event
Set param to CMD_SPI_INTAKE_INCOMPLETE
Post event to SPIService, so that it can be sent to Leader PIC
Set CurrentState to WaitingForNavigation
break;
break;
case LoweringDropoff:
switch (ThisEvent.EventType)
case ES_ARM_LOWERED:
if (carrying is 0)
Create ES_NEW_SPI_CMD_SEND event
Set param to CMD_SPI_UNLOADED
Post event to SPIService, so that it can be sent to Leader PIC
Set CurrentState to WaitingForNavigation
else // carrying more than 0
Stop the pacing timer and move to DroppingOff state
Create ES_FEEDER_START event
Post event to FeederService
Turn on the agitator
break;
case ES_NEW_SPI_CMD_RECEIVED:
if (ThisEvent.EventParam is CMD_SPI_RELEASE_DROPOFF)
Create ES_START_RAISING event
Post event to DropoffLoweringArmFSM
Set CurrentState to RaisingDropoff
break;
break;
case DroppingOff:
switch (ThisEvent.EventType)
case ES_CARGO_OUT:
if (carrying is more than 0)
Decrease carrying by 1.
if (carrying is 0)
Create ES_NEW_SPI_CMD_SEND event
Set param to CMD_SPI_UNLOADED
Post event to SPIService, so that it can be sent to Leader PIC
Create ES_FEEDER_STOP event
Post event to FeederService
Turn off the agitator
Set CurrentState to WaitingForNavigation
break;
break;
case RaisingDropoff:
switch (ThisEvent.EventType)
case ES_ARM_RELEASED:
Create ES_NEW_SPI_CMD_SEND event
Set param to CMD_SPI_RELEASED
Post event to SPIService, so that it can be sent to Leader PIC
Set CurrentState to WaitingForNavigation
break;
break;
return ReturnEvent
Function __ISR(_EXTERNAL_1_VECTOR, IPL5SOFT) _CargoOut_ISR:
Clear interrupt 1 flag
Post an ES_CARGO_OUT event
Function __ISR(_EXTERNAL_3_VECTOR, IPL5SOFT) _CargoIn_ISR:
Clear interrupt 3 flag
Post an ES_CARGO_IN event
Arm FSM (Operation and Manipulation)
Arm FSM (Operation and Manipulation)
It responds to the either ES_RAISE_ARM or ES_LOWER_ARM from the Operator FSM and it steps through a corresponding sequence of joint angles in the two servos every given time. It uses timer 3 and channels 1 and 2.
Module-level variables: MyPriority, CurrentState, seqIndex, (angle sequences for each servo for lowering and raising)
/*------------------------------ Module Code ------------------------------*/
Function InitDropoffLoweringArmFSM(Priority)
set MyPriority to Priority
Set CurrentState to ArmInitPState
Post ES_INIT event to this queue
Return true if the event posted successfully, false otherwise
Function PostDropoffLoweringArmFSM(ThisEvent)
Post event to this queue
Function RunDropoffLoweringArmFSM(ThisEvent)
Initialize ReturnEvent as ES_NO_EVENT
switch (CurrentState)
case ArmInitPState:
if (ThisEvent.EventType is ES_INIT)
Setup timer 3 with prescaler 64 and 625 ticks for the required 50Hz
Setup channels 1 and 2 for the arm and hand servos
Map pins RA0 and RA1 to the channels for elbow and hand
Set initial pulse width to start angle
Set CurrentState to ArmWaiting
break;
case ArmWaiting:
if (ThisEvent.EventType is ES_START_LOWERING_ARM)
Set seqIndex to 0
Start ARM_MOVE_TIMER to start going over the steps
Set CurrentState to Lowering
if (ThisEvent.EventType is ES_START_RAISING_ARM)
Set seqIndex to 0
Start ARM_MOVE_TIMER to start going over the steps
Set CurrentState to Raising
break;
case Lowering:
if (ThisEvent.EventType is ES_TIMEOUT and EventParam is ARM_MOVE_TIMER)
Set elbow and hand angles to the seqIndex value from their lowering sequences
Increment seqIndex by 1 for next cycle
if (seqIndex exceeds reaches the length of the sequence)
Create ES_ARM_LOWERED event
Post event to Operator FSM
Set CurrentState to ArmWaiting
else Restart the ARM_MOVE_TIMER
break;
case Raising:
if (ThisEvent.EventType is ES_TIMEOUT and EventParam is ARM_MOVE_TIMER)
Set elbow and hand angles to the seqIndex value from their raising sequences
Increment seqIndex by 1 for next cycle
if (seqIndex exceeds reaches the length of the sequence)
Create ES_ARM_RELEASED event
Post event to Operator FSM
Set CurrentState to ArmWaiting
else Restart the ARM_MOVE_TIMER
break;
return ReturnEvent
FeederService (Operation and Manipulation)
FeederService (Operation and Manipulation)
It controls the motor used for the dispensing of cargo. It works on 50% duty cycle from 14V by using a normal timeout
Module-level variables: MyPriority
/*------------------------------ Module Code ------------------------------*/
Function InitFeederService(Priority)
set MyPriority to Priority
Initialize pins as digital outputs
Post ES_INIT event to this queue
Return true if the event posted successfully, false otherwise
Function PostFeederService(ThisEvent)
Post event to this queue
Function RunFeederService(ThisEvent)
Initialize ReturnEvent as ES_NO_EVENT
switch (ThisEvent.EventType)
case ES_INIT:
// Do nothing, announce
Set output pin low
break;
case ES_FEEDER_START:
Set output pin high
Start FEEDER_TIMERfor 2ms
break;
case ES_FEEDER_STOP:
// Stop the robot, set duty cycle to 0.
break;
case ES_TIMEOUT:
if (ThisEvent.EventParam is FEEDER_TIMER)
Toggle output pin
Re-start FEEDER_TIMER
// Stop the robot, set duty cycle to 0.
break;
return ReturnEvent
FieldSideServoService (Operation and Manipulation)
FieldSideServoService (Operation and Manipulation)
Starts in neutral position and moves the indicator to either blue or green when the Drive and Navigation PIC identifies the side. Uses channel 3 with timer 3.
Module-level variables: MyPriority
/*------------------------------ Module Code ------------------------------*/
Function InitFieldSideServoService(Priority)
set MyPriority to Priority
Initialize PWM for servo
Setup timer 3 with prescaler 64 and 6250 ticks for the required 50Hz
Setup channel 3 for this servo
Map RB10 to this output
Set pulse width to neutral position
Post ES_INIT event to this queue
Return true if the event posted successfully, false otherwise
Function PostFieldSideServoService(ThisEvent)
Post event to this queue
Function RunFieldSideServoService(ThisEvent)
Initialize ReturnEvent as ES_NO_EVENT
switch (ThisEvent.EventType)
case ES_NEW_SPI_CMD_RECEIVED:
if (ThisEvent.EventParam is CMD_SPI_BLUE_TEAM)
Set Duty Cycle to corresponding length for going full to blue side
if (ThisEvent.EventParam is CMD_SPI_GREEN_TEAM)
Set Duty Cycle to corresponding length for going full to green side
break;
case ES_RESET:
Set pulse width for neutral position
break;
return ReturnEvent
Libraries Used:
Libraries Used:
PIC32_IC_Lib: Input capture library to set up timers, channels, modes, and mapping pins. Primary use being beacon detection.
PIC32_PWM_Lib: Output compare library to set up timers, channels, modes, and mapping pins. Primary use is servo PWM.
PIC32_SPI_HAL: Serial Peripheral Interface library to set up SPI modules, modes, mapping pins and sending data.
See github for full source code on this link.
Page updated
Report abuse