static ES_Event DuringStateReversing(ES_Event Event) {
ES_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) ) {
// implement any entry actions required for this state machine
// after that start any lower level machines that run in this state
//StartLowerLevelSM( Event );
// repeat the StartxxxSM() functions for concurrent state machines
// on the lower level
UpdatePWM(30, 30);
ES_Timer_InitTimer(DRIVE_TIMER, REVERSE_TIME);
} else if ( Event.EventType == ES_EXIT ) {
// on exit, give the lower levels a chance to clean up first
//RunLowerLevelSM(Event);
// repeat for any concurrently running state machines
// now do any local exit functionality
} else {// do the 'during' function for this state
// return either Event, if you don't want to allow the lower level machine
// to remap the current event, or ReturnEvent if you do want to allow it.
}
return(ReturnEvent);
}
static ES_Event DuringStateFollowLine( ES_Event Event)
{
ES_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) )
{
// implement any entry actions required for this state machine
#ifdef DEBUG_CAMPAIGN
printf("\rEntered follow line state in campaign\r\n");
#endif
// after that start any lower level machines that run in this state
StartFollowLine( Event );
#ifdef DEBUG_CAMPAIGN
printf("\rFinished initializing lower level SM\r\n");
#endif
// repeat the StartxxxSM() functions for concurrent state machines
// on the lower level
}
else if ( Event.EventType == ES_EXIT )
{
// on exit, give the lower levels a chance to clean up first
//RunLowerLevelSM(Event);
// repeat for any concurrently running state machines
// now do any local exit functionality
//Exit from lower state machine
RunFollowLine(Event);
//Stop motors
UpdatePWM(50, 50);
}else
// do the 'during' function for this state
{
// run any lower level state machine
// ReturnEvent = RunLowerLevelSM(Event);
// repeat for any concurrent lower level machines
//Run loer level SM
ReturnEvent = RunFollowLine(Event);
// do any activity that is repeated as long as we are in this state
}
// return either Event, if you don't want to allow the lower level machine
// to remap the current event, or ReturnEvent if you do want to allow it.
return(ReturnEvent);
}
void servoisr()
{
static int update;
if (update>15){
update = 0;
if (STEP_ENABLE){
SYNC = true;
UpdatePosition();
UpdateTrajectory();
CalculateExtCount();
UpdatePID();
UpdatePWM();
SYNC = false;
}
rx_timer++;
} else update++;
}
void main()
{
InitHardware();
Encoder(true);
ResetServo();
delay_ms(200);
bEnable = !STEP_ENABLE;
// run these asynch tasks
for (;;){
if (bServo){
SYNC = true;
UpdatePosition();
UpdateTrajectory();
CalculateExtCount();
UpdatePID();
UpdatePWM();
HandleErrorTask();
bServo = false;
} else {
//HandleRXData();
if (SYNC){
if (bDirty && !bEnable){
bDirty = false;
SaveEEPROM();
}
SYNC = false;
}
}
}
}
//This function is called in the SPI byte recieved interrupt. After checking for a command it will return the data requested.
unsigned char SPI_State_Machine(unsigned char Input)
{
static unsigned char state = WAITING_FOR_COMMAND_STATE;
static unsigned char ByteNum = 0;
unsigned char ReturnValue;
if(state == WAITING_FOR_COMMAND_STATE)
{
switch(Input)//check the incoming byte for a command
{
case ACCELEROMETER_COMMAND:
ByteNum = 0;
state = ACCELEROMETER_COMMAND_STATE;
break;
case GYRO_COMMAND:
ByteNum = 0;
state = GYRO_COMMAND_STATE;
break;
case COMPASS_COMMAND:
ByteNum = 0;
state = COMPASS_COMMAND_STATE;
break;
case ACCOUSTIC_COMMAND:
ByteNum = 0;
state = ACCOUSTIC_COMMAND_STATE;
break;
case VOLTAGES_COMMAND:
ByteNum = 0;
state = VOLTAGES_COMMAND_STATE;
break;
case TEMPERATURE_COMMAND:
ByteNum = 0;
state = TEMPERATURE_COMMAND_STATE;
break;
case RPM_COMMAND:
ByteNum = 0;
state = RPM_COMMAND_STATE;
break;
case STATUS_COMMAND:
ByteNum = 0;
state = STATUS_COMMAND_STATE;
break;
case PWM_COMMAND:
ByteNum = 0;
state = PWM_COMMAND_STATE;
break;
case GPS_TIME_COMMAND:
ByteNum = 0;
state = GPS_TIME_STATE;
break;
case GPS_LATITUDE_COMMAND:
ByteNum = 0;
state = GPS_LATITUDE_STATE;
break;
case GPS_LONGITUDE_COMMAND:
ByteNum = 0;
state = GPS_LONGITUDE_STATE;
break;
case GPS_SATELLITES_COMMAND:
ByteNum = 0;
state = GPS_SATELLITES_STATE;
break;
case GPS_ALTITUDE_COMMAND:
ByteNum = 0;
state = GPS_ALTITUDE_STATE;
break;
case GPS_HEMISPHERE_COMMAND:
ByteNum = 0;
state = GPS_HEMISPHERE_STATE;
break;
default://Invalid command
state = WAITING_FOR_COMMAND_STATE;
return 0xFF;
}
}
switch(state)
{
case ACCELEROMETER_COMMAND_STATE:
//send the accelerometer byte pointed to by bytenum
ReturnValue = Accelerator[ByteNum];
ByteNum++;
if(ByteNum > 6)//6 bytes in a accelerometer packet
{
state = WAITING_FOR_COMMAND_STATE;
return 0xFF;
}
break;
case GYRO_COMMAND_STATE:
ReturnValue = Gyro[ByteNum];
// ReturnValue = dummydata[ByteNum];
ByteNum++;
if(ByteNum > 6)//6 bytes in a status packet
{
state = WAITING_FOR_COMMAND_STATE;
return 0xFF;
}
break;
case COMPASS_COMMAND_STATE:
//send the compass byte pointed to by bytenum
ReturnValue = Compass[ByteNum];
ByteNum++;
if(ByteNum > 2)//2 bytes in a compass packet
{
state = WAITING_FOR_COMMAND_STATE;
return 0xFF;
}
break;
case ACCOUSTIC_COMMAND_STATE:
//send the accoustic byte pointed to by bytenum
ReturnValue = Accoustic[ByteNum];
ByteNum++;
if(ByteNum > 2)//2 bytes in a accoustic packet
{
state = WAITING_FOR_COMMAND_STATE;
return 0xFF;
}
break;
case VOLTAGES_COMMAND_STATE:
//send the voltage byte pointed to by bytenum
ReturnValue = Voltages[ByteNum];
ByteNum++;
if(ByteNum > 4)//4 bytes in a voltages packet
{
state = WAITING_FOR_COMMAND_STATE;
return 0xFF;
}
break;
case TEMPERATURE_COMMAND_STATE:
//send the temperature byte pointed to by bytenum
ReturnValue = Temperature[ByteNum];
ByteNum++;
if(ByteNum > 2)//2 bytes in a temperature packet
{
state = WAITING_FOR_COMMAND_STATE;
return 0xFF;
}
break;
case RPM_COMMAND_STATE:
//send the RPM byte pointed to by bytenum
ReturnValue = RPM[ByteNum];
ByteNum++;
if(ByteNum > 2)//2 bytes in an RPM packet
{
state = WAITING_FOR_COMMAND_STATE;
return 0xFF;
}
break;
case STATUS_COMMAND_STATE:
//send the status byte pointed to by bytenum
ReturnValue = command[ByteNum];
command[ByteNum] = Input;
ByteNum++;
if(ByteNum > 2)//2 bytes in a status packet
{
state = WAITING_FOR_COMMAND_STATE;
return 0xFF;
}
break;
case PWM_COMMAND_STATE:
ReturnValue = servos[ByteNum];
servos[ByteNum] = Input;
ByteNum++;
if(ByteNum > 4)//4 bytes in a status packet
{
UpdatePWM();
ANTI_COLL_LED ^= 1;
state = WAITING_FOR_COMMAND_STATE;
return 0xFF;
}
break;
case GPS_TIME_STATE:
ReturnValue = GPS_TIME[ByteNum];
ByteNum++;
if(ByteNum > 6)//6 bytes in a status packet
{
state = WAITING_FOR_COMMAND_STATE;
return 0xFF;
}
break;
case GPS_LATITUDE_STATE:
ReturnValue = GPS_LATITUDE[ByteNum];
ByteNum++;
if(ByteNum > 9)//9 bytes in a status packet
{
state = WAITING_FOR_COMMAND_STATE;
return 0xFF;
}
break;
case GPS_LONGITUDE_STATE:
ReturnValue = GPS_LONGITUDE[ByteNum];
ByteNum++;
if(ByteNum > 10)//10 bytes in a status packet
{
state = WAITING_FOR_COMMAND_STATE;
return 0xFF;
}
break;
case GPS_SATELLITES_STATE:
ReturnValue = GPS_SATELLITES[ByteNum];
ByteNum++;
if(ByteNum > 2)//2 bytes in a status packet
{
state = WAITING_FOR_COMMAND_STATE;
return 0xFF;
}
break;
case GPS_ALTITUDE_STATE:
ReturnValue = GPS_ALTITUDE[ByteNum];
ByteNum++;
if(ByteNum > 7)//7 bytes in a status packet
{
state = WAITING_FOR_COMMAND_STATE;
return 0xFF;
}
break;
// case GPS_HEMISPHERE_STATE:
// ReturnValue = GPS_HEMI[ByteNum];
// ByteNum++;
// if(ByteNum > 2)//7 bytes in a status packet
// {
// state = WAITING_FOR_COMMAND_STATE;
// return 0xFF;
// }
// break;
default:
state = WAITING_FOR_COMMAND_STATE;
ReturnValue = 0xA5;
return 0xFF;
break;
}
return ReturnValue;
}
/****************************************************************************
Function
RunMasterSM
Parameters
ES_Event: the event to process
Returns
ES_Event: an event to return
Description
the run function for the top level state machine
Notes
uses nested switch/case to implement the machine.
Author
J. Edward Carryer, 02/06/12, 22:09
****************************************************************************/
ES_Event RunCampaigningSM (ES_Event CurrentEvent )
{
bool MakeTransition = false;/* are we making a state transition? */
CampaignState_t NextState = CurrentState;
ES_Event EntryEventKind = { ES_ENTRY, 0 };// default to normal entry to new state
ES_Event ReturnEvent = { ES_NO_EVENT, 0 }; // assume no error
switch ( CurrentState )
{
case WaitingToStartCampaign: // If current state is state one
// Execute During function for state one. ES_ENTRY & ES_EXIT are
// processed here allow the lowere level state machines to re-map
// or consume the event
CurrentEvent = DuringStateWaiting(CurrentEvent);
//process any events
if ( CurrentEvent.EventType != ES_NO_EVENT ) //If an event is active
{
switch (CurrentEvent.EventType)
{
case START_FOLLOWING : //If event is event one
#ifdef DEBUG_CAMPAIGN
printf("\rI recieved follow line command!\r\n");
#endif
//Execute action function for state one : event one
NextState = FollowLine;//Decide what the next state will be
// for internal transitions, skip changing MakeTransition
MakeTransition = true; //mark that we are taking a transition
// if transitioning to a state with history change kind of entry
EntryEventKind.EventType = ES_ENTRY;
// optionally, consume or re-map this event for the upper
// level state machine
ReturnEvent.EventType = ES_NO_EVENT;
//Post a START_FOLLOWING EVENT
ES_Event ThisEvent;
ThisEvent.EventType = START_FOLLOWING;
PostCampaigningSM(ThisEvent);
break;
// repeat cases as required for relevant events
case CAPTURE_STATION_NOW: //If event is event one
#ifdef DEBUG_CAMPAIGN
printf("\rI recieved capture station command!\r\n");
#endif
// Execute action function for state one : event one
NextState = CaptureStation;//Decide what the next state will be
// for internal transitions, skip changing MakeTransition
MakeTransition = true; //mark that we are taking a transition
// if transitioning to a state with history change kind of entry
EntryEventKind.EventType = ES_ENTRY;
// optionally, consume or re-map this event for the upper
// level state machine
ReturnEvent.EventType = ES_NO_EVENT;
//Post a CAPTURE_STATION_NOW
ThisEvent.EventType = CAPTURE_STATION_NOW;
PostCampaigningSM(ThisEvent);
break;
// repeat cases as required for relevant events
case STOP_NOW_CHANGE_TO_CAPTURE:
//Stop motors now
UpdatePWM(50,50);
#ifdef DEBUG_CAMPAIGN
printf("\rI recieved capture station command STOPPING!\r\n");
#endif
// Execute action function for state one : event one
NextState = CaptureStation;//Decide what the next state will be
// for internal transitions, skip changing MakeTransition
MakeTransition = true; //mark that we are taking a transition
// if transitioning to a state with history change kind of entry
EntryEventKind.EventType = ES_ENTRY;
// optionally, consume or re-map this event for the upper
// level state machine
ReturnEvent.EventType = ES_NO_EVENT;
break;
case ACTIVATE_SHOOTER:
//Stop motors now
UpdatePWM(50,50);
#ifdef DEBUG_CAMPAIGN
printf("\rI recieved activate shooting command in Campaigning SM!\r\n");
#endif
// Execute action function for state one : event one
NextState = ActivateShooter;//Decide what the next state will be
// for internal transitions, skip changing MakeTransition
MakeTransition = true; //mark that we are taking a transition
// if transitioning to a state with history change kind of entry
EntryEventKind.EventType = ES_ENTRY;
// optionally, consume or re-map this event for the upper
// level state machine
ReturnEvent.EventType = ES_NO_EVENT;
//Post a ACTIVATE_SHOOTER to self
ThisEvent.EventType = ACTIVATE_SHOOTER;
PostCampaigningSM(ThisEvent);
break;
}
}
break;
// repeat state pattern as required for other states
case FollowLine: // If current state is state one
// Execute During function for state one. ES_ENTRY & ES_EXIT are
// processed here allow the lowere level state machines to re-map
// or consume the event
CurrentEvent = DuringStateFollowLine(CurrentEvent);
//printf("\rI am in follow line state!\r\n");
//process any events
if ( CurrentEvent.EventType != ES_NO_EVENT ) //If an event is active
{
switch (CurrentEvent.EventType)
{
case CAPTURE_STATION_NOW : //If event is event one
UpdatePWM(50,50);
// Execute action function for state one : event one
NextState = CaptureStation;//Decide what the next state will be
// for internal transitions, skip changing MakeTransition
MakeTransition = true; //mark that we are taking a transition
// if transitioning to a state with history change kind of entry
EntryEventKind.EventType = ES_ENTRY;
// optionally, consume or re-map this event for the upper
// level state machine
ReturnEvent.EventType = ES_NO_EVENT;
//Post a CAPTURE_STATION_NOW
ES_Event ThisEvent;
ThisEvent.EventType = CAPTURE_STATION_NOW;
PostCampaigningSM(ThisEvent);
break;
// repeat cases as required for relevant events
case STOP_NOW_CHANGE_TO_CAPTURE:
UpdatePWM(50,50);
//printf("\rI recieved capture station command STOPPING!\r\n");
// Execute action function for state one : event one
NextState = CaptureStation;//Decide what the next state will be
// for internal transitions, skip changing MakeTransition
MakeTransition = true; //mark that we are taking a transition
// if transitioning to a state with history change kind of entry
EntryEventKind.EventType = ES_ENTRY;
// optionally, consume or re-map this event for the upper
// level state machine
ReturnEvent.EventType = ES_NO_EVENT;
break;
case ACTIVATE_SHOOTER:
//Stop motors now
UpdatePWM(50,50);
#ifdef DEBUG_CAMPAIGN
printf("\rI recieved activate shooting command in Campaigning SM!\r\n");
#endif
// Execute action function for state one : event one
NextState = ActivateShooter;//Decide what the next state will be
// for internal transitions, skip changing MakeTransition
MakeTransition = true; //mark that we are taking a transition
// if transitioning to a state with history change kind of entry
EntryEventKind.EventType = ES_ENTRY;
// optionally, consume or re-map this event for the upper
// level state machine
ReturnEvent.EventType = ES_NO_EVENT;
//Post a ACTIVATE_SHOOTER to self
ThisEvent.EventType = ACTIVATE_SHOOTER;
PostCampaigningSM(ThisEvent);
break;
}
}
break;
case CaptureStation: // If current state is state one
// Execute During function for state one. ES_ENTRY & ES_EXIT are
// processed here allow the lowere level state machines to re-map
// or consume the event
CurrentEvent = DuringStateCaptureStation(CurrentEvent);
//process any events
#ifdef DEBUG_CAMPAIGN
printf("\rI am in capture station state!\r\n");
#endif
if ( CurrentEvent.EventType != ES_NO_EVENT ) //If an event is active
{
switch (CurrentEvent.EventType)
{
case START_FOLLOWING : //If event is event one
// Execute action function for state one : event one
NextState = FollowLine;//Decide what the next state will be
// for internal transitions, skip changing MakeTransition
MakeTransition = true; //mark that we are taking a transition
// if transitioning to a state with history change kind of entry
EntryEventKind.EventType = ES_ENTRY;
// optionally, consume or re-map this event for the upper
// level state machine
ReturnEvent.EventType = ES_NO_EVENT;
//Post an event to motor following again
//Post a START_FOLLOWING
ES_Event ThisEvent;
ThisEvent.EventType = START_FOLLOWING;
PostCampaigningSM(ThisEvent);
break;
// repeat cases as required for relevant events
case ACTIVATE_SHOOTER:
//Stop motors now
UpdatePWM(50,50);
#ifdef DEBUG_CAMPAIGN
printf("\rI recieved activate shooting command in Campaigning SM!\r\n");
#endif
// Execute action function for state one : event one
NextState = ActivateShooter;//Decide what the next state will be
// for internal transitions, skip changing MakeTransition
MakeTransition = true; //mark that we are taking a transition
// if transitioning to a state with history change kind of entry
EntryEventKind.EventType = ES_ENTRY;
// optionally, consume or re-map this event for the upper
// level state machine
ReturnEvent.EventType = ES_NO_EVENT;
//Post a ACTIVATE_SHOOTER to self
ThisEvent.EventType = ACTIVATE_SHOOTER;
PostCampaigningSM(ThisEvent);
break;
}
}
break;
case ActivateShooter: // If current state is state one
// Execute During function for state one. ES_ENTRY & ES_EXIT are
// processed here allow the lowere level state machines to re-map
// or consume the event
CurrentEvent = DuringStateActivateShooter(CurrentEvent);
//process any events
//printf("\rI am in Activte shooter state!\r\n");
if ( CurrentEvent.EventType != ES_NO_EVENT ) //If an event is active
{
switch (CurrentEvent.EventType)
{
case START_FOLLOWING : //If event is event one
// Execute action function for state one : event one
NextState = FollowLine;//Decide what the next state will be
// for internal transitions, skip changing MakeTransition
MakeTransition = true; //mark that we are taking a transition
// if transitioning to a state with history change kind of entry
EntryEventKind.EventType = ES_ENTRY;
// optionally, consume or re-map this event for the upper
// level state machine
ReturnEvent.EventType = ES_NO_EVENT;
//Post an event to motor following again
//Post a START_FOLLOWING
ES_Event ThisEvent;
ThisEvent.EventType = START_FOLLOWING;
PostCampaigningSM(ThisEvent);
break;
// repeat cases as required for relevant events
}
}
break;
default:
break;
}
// If we are making a state transition
if (MakeTransition == true)
{
// Execute exit function for current state
CurrentEvent.EventType = ES_EXIT;
RunCampaigningSM(CurrentEvent);
CurrentState = NextState; //Modify state variable
// Execute entry function for new state
// this defaults to ES_ENTRY
RunCampaigningSM(EntryEventKind);
}
// in the absence of an error the top level state machine should
// always return ES_NO_EVENT, which we initialized at the top of func
return(ReturnEvent);
}
ES_Event RunGetOnLine(ES_Event thisEvent) {
ES_Event ReturnVal;
ReturnVal.EventType = ES_NO_EVENT;
GetOnLineState_t NextState;
NextState = CurrentState;
//If there is a reversal command, change the directional multiplier
if(thisEvent.EventType == ES_REVERSE_DRIVE) { //The corresponding event to rotate camera is in Camera.c
Direction = -Direction;
}
Direction = 1; //HOTWIRE
switch(CurrentState) {
case WaitingToStart:
if(thisEvent.EventType == ES_GET_ON_LINE) {
NextState = AlignWithBeacon;
ES_Timer_InitTimer(DRIVE_TIMER_GET_ON_LINE, 1);
}
break;
case AlignWithBeacon:
//Call AlignWithBeacon, AlignWithBeacon will send an event upon finding it
//Check if the event prompts a switch to the FullSpeedForward state.
if(thisEvent.EventType == ES_BEACON_FOUND) {
NextState = FullSpeedForward;
ES_Timer_InitTimer(DRIVE_TIMER_GET_ON_LINE, 1);
}
break;
case FullSpeedForward:
if(thisEvent.EventType == ES_TIMEOUT && thisEvent.EventParam == DRIVE_TIMER_GET_ON_LINE) {
//See whether line has been found.
bool LineFound = ReturnLineFound();
if(LineFound && LineFoundCount > LINE_FOUND_COUNT_THRESHOLD) {
//Reset LineFoundCount
LineFoundCount = 0;
//Stop the motors
UpdatePWM(NEUTRAL_DUTY, NEUTRAL_DUTY);
//Set NextState to LineUp
NextState = LineUp;
//Generate Timeout to advance state.
ES_Timer_InitTimer(DRIVE_TIMER_GET_ON_LINE, 1);
} else if(LineFound) {
LineFoundCount += 1;
ES_Timer_InitTimer(DRIVE_TIMER_GET_ON_LINE, CONTROL_LOOP_TIMEOUT);
} else {
//Reset LineFoundCount (force it to get LINE_FOUND_COUNT_THRESHOLD LineFounds in a row)
LineFoundCount = 0;
//Drive both motors at full speed.
LeftInput = LEFT_NOMINAL_DUTY*Direction+NEUTRAL_DUTY;
RightInput = RIGHT_NOMINAL_DUTY*Direction+NEUTRAL_DUTY;
//Send PWM commands.
UpdatePWM(LeftInput, RightInput);
//Set a timer for the drive loop timeout.
ES_Timer_InitTimer(DRIVE_TIMER_GET_ON_LINE, CONTROL_LOOP_TIMEOUT);
}
}
break;
case LineUp:
//Rotate until the error is on the interior side of the bot
Error = ReturnLineCenter();
if(Error > 0) {
//Stop the motors
UpdatePWM(NEUTRAL_DUTY, NEUTRAL_DUTY);
//Change the state to FollowLine.
ReturnVal.EventType = ES_LINE_FOUND;
} else {
//Rotate the motors CCW
UpdatePWM(NEUTRAL_DUTY, NEUTRAL_DUTY+RIGHT_NOMINAL_DUTY);
//Set the next state to be FullSpeedForward in case the rotation loses line
NextState = FullSpeedForward;
//Set a timeout
ES_Timer_InitTimer(DRIVE_TIMER_GET_ON_LINE, CONTROL_LOOP_TIMEOUT);
}
break;
}
if(thisEvent.EventType == ES_EXIT) {
//Set NextState to be FollowWaiting.
NextState = WaitingToStart;
//Generate a timeout to advance the state machine.
ES_Timer_InitTimer(DRIVE_TIMER_GET_ON_LINE, 1);
}
CurrentState = NextState;
return ReturnVal;
}
static ES_Event DuringStateFollowing( ES_Event Event)
{
ES_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) )
{
// implement any entry actions required for this state machine
// after that start any lower level machines that run in this state
//StartLowerLevelSM( Event );
// repeat the StartxxxSM() functions for concurrent state machines
// on the lower level
ES_Timer_InitTimer(DRIVE_TIMER, CONTROL_LOOP_TIMEOUT);
}
else if ( Event.EventType == ES_EXIT )
{
// on exit, give the lower levels a chance to clean up first
//RunLowerLevelSM(Event);
// repeat for any concurrently running state machines
// now do any local exit functionality
}else
// do the 'during' function for this state
{
// run any lower level state machine
// ReturnEvent = RunLowerLevelSM(Event);
// repeat for any concurrent lower level machines
//Get the current line position from Camera.
Error = ReturnLineCenter();
//Inversion logic says to make error opposite of last lost line
#ifdef INVERSION_LOGIC
if(!ReturnLineFound) {
Error = -Error;
}
#endif
//Lowpass applies a filter to previous line centers
#ifdef LOWPASS
//Only add the line center to the average if it was found
if(ReturnLineFound()) {
//Set LineLostRecovery to false, since line is found
LineLostRecovery = false;
//Set the past line average to 0, and calculate new running average
PastLineAverage = 0;
LineLostCounter = 0; //Line is found, so reset the line lost counter
for (int i = 0; i < NUM_PAST_CENTER-1; i+= 1) {
PastLine[i+1] = PastLine[i];
}
PastLine[0] = Error;
for (int i = 0; i < NUM_PAST_CENTER; i+= 1) {
PastLineAverage += PastLine[i];
}
PastLineAverage /= NUM_PAST_CENTER;
} else {
LineLostCounter += 1;
}
//Set the error to the previous average
Error = PastLineAverage;
#endif
//Implement anti-windup logic and I-control.
//If the accumulated error is greater than some bound, and error is positive,
//don't add to the accumulated error
if(AccumulatedError > WINDUP_BOUND && Error > 0) {
AppendError = 0;
//If the accumulated error is less than than negative of some bound, and error is negative,
//don't add to the accumulated error
} else if(AccumulatedError < -WINDUP_BOUND && Error < 0) {
AppendError = 0;
//Otherwise, add to the accumulated error
} else {
AppendError = (float) Error;
}
AccumulatedError += ((float) CONTROL_LOOP_TIMEOUT/1000)*AppendError;
//Implement D-control
Derivative = (Error - PreviousError)/((float) CONTROL_LOOP_TIMEOUT/1000);
//Set the previous error to the current error
PreviousError = Error;
//Make the robot follow the line by slowing down one of the motors by a certain amount
//and leaving the other motor at the nominal PWM setting
Command = Kp*Error+Ki*AccumulatedError+Kd*Derivative;
//Implement adaptive speed, which slows the car down based on amount of turning
#ifdef ADAPTIVE_SPEED
SpeedReduction = Ks*Command;
if(SpeedReduction > RIGHT_NOMINAL_DUTY) {
SpeedReduction = RIGHT_NOMINAL_DUTY;
}
#else
SpeedReduction = 0;
#endif
//If the line has been lost for more than LINE_LOST_COUNTER_THRESHOLD cycles,
//make the car go straight
if(LineLostCounter > LINE_LOST_COUNTER_THRESHOLD) {
LineLostRecovery = true;
}
//Based on the calculated command, slow down one of the wheels, saturate command if too large
if(Command >= 0) {
if((RIGHT_NOMINAL_DUTY+NEUTRAL_DUTY - Command) < RIGHT_UPPER_DEADBAND) {
Command = RIGHT_UPPER_DEADBAND - (RIGHT_NOMINAL_DUTY+NEUTRAL_DUTY - Command);
InPWMDeadband = true;
} else {
InPWMDeadband = false;
}
LeftInput = LEFT_NOMINAL_DUTY*Direction+NEUTRAL_DUTY-SpeedReduction;
if(InPWMDeadband) {
RightInput = RIGHT_LOWER_DEADBAND - Command;
} else {
RightInput = (RIGHT_NOMINAL_DUTY-Command)*Direction+NEUTRAL_DUTY-SpeedReduction;
}
if(LeftInput > 99) {
LeftInput = 99;
} else if(LeftInput < 1) {
LeftInput = 1;
}
if(RightInput > 99) {
RightInput = 99;
} else if(RightInput < 1) {
RightInput = 1;
}
UpdatePWM(LeftInput, RightInput); //May be wrong (opposite)
} else if(Command < 0) {
if((LEFT_NOMINAL_DUTY + PWM_SCALING*Command+NEUTRAL_DUTY) < LEFT_UPPER_DEADBAND) {
Command = LEFT_UPPER_DEADBAND - (LEFT_NOMINAL_DUTY+NEUTRAL_DUTY + PWM_SCALING*Command);
InPWMDeadband = true;
} else {
InPWMDeadband = false;
}
RightInput = RIGHT_NOMINAL_DUTY*Direction+NEUTRAL_DUTY-SpeedReduction;
if(InPWMDeadband) {
LeftInput = LEFT_LOWER_DEADBAND - Command;
} else {
LeftInput = (LEFT_NOMINAL_DUTY+PWM_SCALING*Command)*Direction+NEUTRAL_DUTY-SpeedReduction;
}
if(LeftInput > 99) {
LeftInput = 99;
} else if(LeftInput < 1) {
LeftInput = 1;
}
if(RightInput > 99) {
RightInput = 99;
} else if(RightInput < 1) {
RightInput = 1;
}
UpdatePWM(LeftInput, RightInput); //May be wrong (opposite)
}
//If the line is lost, ignore everything before and go straight
if(LineLostRecovery) {
UpdatePWM(50+20*PWM_SCALING, 50+20);
}
//Poll the ultrasonic sensor reading
float pseudoDistance = querydistance();
//Add some logic to get rid of results that don't make sense from the ultrasonic
if(pseudoDistance > LOWER_DISTANCE_THRESHOLD && !(pseudoDistance > 0.069 &&
pseudoDistance < 0.079)) {
distance = pseudoDistance;
} else {
for (int i = 0; i < NUM_PAST_PROXIMITY-1; i+= 1) {
PastProximity[i] = 0.12;
}
}
//Implement a low pass filter for the ultrasonic results
AverageProximity = 0;
for (int i = 0; i < NUM_PAST_PROXIMITY-1; i+= 1) {
PastProximity[i] = PastProximity[i+1];
}
PastProximity[NUM_PAST_PROXIMITY-1] = distance;
for (int i = 0; i < NUM_PAST_PROXIMITY; i+= 1) {
AverageProximity += PastProximity[i];
}
AverageProximity /= NUM_PAST_PROXIMITY;
//If the distance is within a threshold (too close), rotate 180
if (AverageProximity > LOWER_DISTANCE_THRESHOLD && AverageProximity < DISTANCE_THRESHOLD &&
LineLostRecovery) {
StopMotors();
ReturnEvent.EventType = ROTATE180;
//After rotating 180 degrees flush the low pass filter
for(int i = 0; i < NUM_PAST_PROXIMITY; i += 1) {
PastProximity[i] = DISTANCE_THRESHOLD + 0.05;
}
} else {
//Set a timer for the drive loop timeout.
ES_Timer_InitTimer(DRIVE_TIMER, CONTROL_LOOP_TIMEOUT);
}
// do any activity that is repeated as long as we are in this state
}
// return either Event, if you don't want to allow the lower level machine
// to remap the current event, or ReturnEvent if you do want to allow it.
return(ReturnEvent);
}