/***************************************************************************
public functions
***************************************************************************/
void InitPIControl(void){
counterL = 0;
avePeriodL=0;
for(uint8_t i=0; i<aveNum; i++){
periodL[i] = 0xffffffff;
}
counterR = 0;
avePeriodR=0;
for(uint8_t i=0; i<aveNum; i++){
periodR[i] = 0xffffffff;
}
rpmL=0;
rpmR=0;
IntegralTermL=0.0;
IntegralTermR=0.0;
SpeedControlState.LeftDirection = FOR;
SpeedControlState.RightDirection = FOR;
SpeedControlState.RequestedDutyL = 0;
SpeedControlState.RequestedDutyR = 0;
absCounterR = 0;
absCounterL = 0;
lastDistSofarL = 0.0;
lastDistSofarR = 0.0;
lastPeriodL = 0;
lastPeriodR = 0;
// CurrentState =CONTROL;
ES_Timer_InitTimer(ENCODER_TIMER, ONE_SEC/100);
ES_Timer_InitTimer(INPUT_STOP_TIMER, ONE_SEC/10);
printf("\r\n init pi control");
}
/****************************************************************************
Function
RunLance
Parameters
ES_Event : the event to process
Returns
ES_Event, ES_NO_EVENT if no error ES_ERROR otherwise
Description
When a deploy lance event is recieved, the robot uses a servo to extend
the lance and a timer is used to wait the 3 seconds to retract lance
and 1 second wait until the lance is able to be extended again.
Author
P. Sherman, 03/10/14, 20:40
J. Edward Carryer, 01/15/12, 15:23
****************************************************************************/
ES_Event RunLance( ES_Event ThisEvent )
{
ES_Event ReturnEvent;
ReturnEvent.EventType = ES_NO_EVENT; // assume no errors
//Deploy Lance if event is posted and lance is in valid state
if(ThisEvent.EventType == Deploy_Lance && CurrentState == Retracted)
{
SetServo(LANCE_SERVO, DEPLOY_WIDTH);
ES_Timer_InitTimer(Lance_Timer, 3*ONE_SEC);
CurrentState = Deployed;
}
//Timeout Event recieved
else if(ThisEvent.EventType == ES_TIMEOUT)
{
if(CurrentState == Deployed)
{
SetServo(LANCE_SERVO, RETRACT_WIDTH);
ES_Timer_InitTimer(Lance_Timer, ONE_SEC);
CurrentState = Inactive;
} else if (CurrentState == Inactive)
{
CurrentState = Retracted;
}
}
return ReturnEvent;
}
ES_Event RunLidarService( ES_Event ThisEvent )
{
// ES_Event New_Event;
ES_Event ReturnEvent;
ReturnEvent.EventType = ES_NO_EVENT; // assume no errors
switch (CurrentLidarState)
{
case InitLidar: // If current state is initial Psedudo State
if (ThisEvent.EventType == ES_INIT)// only respond to ES_Init
{
CurrentLidarState = LidarStart;
printf("\r\n Lidar Inited \r\n");
//ES_Timer_InitTimer(LIDAR_TIMER, 100);
}
break;
case LidarStart:
if (ThisEvent.EventType == GET_DISTANCE)
{ //printf("\r\n GET_DISTANCE received in Lidar");
if (Period == 0){
HWREG(GPIO_PORTE_BASE+(GPIO_O_DATA + ALL_BITS)) &= BIT2LO;
TriggerLow = true;
CurrentLidarState = LidarMeasure;
ES_Timer_InitTimer(LIDAR_TIMER, 50);
ReturnEvent.EventType = ES_NO_EVENT;
}
}
break;
case LidarMeasure:
if (ThisEvent.EventType == ES_TIMEOUT && ThisEvent.EventParam == LIDAR_TIMER)
{
if (Period2Return != 0){
//Distance = (float)Period2Return/(float)800;
Distance = (float)((Period2Return*25/20000)*2.02-21.87);
ES_Event Event2Post;
Event2Post.EventType = DISTANCE_FOUND;
PostMasterSM(Event2Post);
TriggerLow = false;
printf("\r\n Lidar Measure result is %f", Distance);
HWREG(GPIO_PORTE_BASE+(GPIO_O_DATA + ALL_BITS)) |= BIT2HI;
Period = 0;
CurrentLidarState = LidarStart;
ReturnEvent.EventType = ES_NO_EVENT;
}
} else {
ES_Timer_InitTimer(LIDAR_TIMER, 50);
ReturnEvent.EventType = ES_NO_EVENT;
}
break;
} // end switch on Current State
return ReturnEvent;
}
/****************************************************************************
Function
InitEncoderService
Parameters
uint8_t : the priorty of this service
Returns
bool, false if error in initialization, true otherwise
Description
Saves away the priority, initializes TIVA pins, sets up init capture,
starts RPM timer
****************************************************************************/
bool InitEncoderService ( uint8_t Priority )
{
ES_Event ThisEvent;
MyPriority = Priority;
// Setup init capture
InitInputCapturePeriod( );
// Setup periodic timer
InitPeriodicInt( );
// Start RPM timer
ES_Timer_InitTimer(RPMtimer, RPMtime);
// Post the initial transition event
ThisEvent.EventType = ES_INIT;
if (ES_PostToService( MyPriority, ThisEvent) == true)
{
return true;
}else
{
return false;
}
}
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 DuringWaitingState( 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) )
{
ES_Timer_InitTimer(COM_TIMER, WAIT_TIME);
ES_Event Event;
Event.EventType = BotUpdate;
PostUpdateSM(Event);
}
else if ( Event.EventType == ES_EXIT )
{
}else
{
}
return(ReturnEvent);
}
ES_Event RunLEDControl(ES_Event ThisEvent) {
ES_Event returnVal;
returnVal.EventType = ES_NO_EVENT;
// if the flash timer timed out
if (ThisEvent.EventType == ES_TIMEOUT && ThisEvent.EventParam == LED_FLASH_TIMER) {
// printf("LED_FLASH_TIMER expired\r\n");
// if currentFlashState is ON, use bitmask to turn flashing LEDs off
if (currentFlashState == ON) {
//printf("On");
currentLEDVals &= ~flashingLEDs;
currentFlashState = OFF;
// printf("currentLEDVals: %#10x\r\n",currentLEDVals);
}
// if currentFlashState is OFF, use bitmask to turn flashing LEDs on
else {
// printf("Off");
currentLEDVals |= flashingLEDs;
currentFlashState = ON;
// printf("currentLEDVals: %#10x\r\n",currentLEDVals);
}
// toggle the currentFlashState & SR_Write to the shift register in either case
LED_SR_Write(currentLEDVals);
// restart the timer
ES_Timer_InitTimer(LED_FLASH_TIMER, FLASH_TIME);
}
// printf("CurrentLEDVals: %#10x\r\n",LED_SR_GetCurrentRegister());
return returnVal;
}
ES_Event RunBeaconService(ES_Event ThisEvent)
{
ES_Event ReturnEvent;
ReturnEvent.EventType = ES_NO_EVENT; // assume no errors
ES_EventTyp_t curEvent;
switch (ThisEvent.EventType) {
case ES_INIT:
{
//Initialize statics
beacon_state = BEACON_ON;
beacon_samples = 0xFF;
//Begin the sample loop timer
ES_Timer_InitTimer(BEACON_TIMER, BEACON_SAMPLE_PERIOD);
break;
}
case ES_TIMEOUT:
{
//Only respond to the track wire timer
if(ThisEvent.EventParam == BEACON_TIMER) {
//Update the states
debounceBeaconDetector();
//Check for a state change
uint8_t beacon_event = Beacon_StateChange();
//Generate event upon state change
if(beacon_event) {
ReturnEvent.EventType = BEACON_EVENT;
ReturnEvent.EventParam = beacon_state;
//Broadcast event
PostTopLevelHSM(ReturnEvent);
//Update the state
prev_state = beacon_state;
}
//Reset the timer
ES_Timer_InitTimer(BEACON_TIMER, BEACON_SAMPLE_PERIOD);
}
break;
}
}
return ReturnEvent;
}
//Takes a priority number, returns True.
bool InitializeStartService(uint8_t Priority)
{
//Initialize the MyPriority variable with the passed in parameter.
MyPriority = Priority;
puts("Finished_Initialize_Start_Service\n\r");
ES_Timer_InitTimer(CHECK_TIMER,20);
return true;
}
bool InitializeGetOnLine(uint8_t priority) {
//Set MyPriority to priority.
MyPriority = priority;
//Set CurrentState to AlignWithBeacon
CurrentState = WaitingToStart;
//Generate timeout
ES_Timer_InitTimer(DRIVE_TIMER_GET_ON_LINE, 1);
//Return true
return true;
}
/****************************************************************************
Function
pulseLow
Parameters
uint8_t : the track number to pulse low
Returns
nothing
Description
Takes in a track number and pulls Low the Tiva port that is connected
to that track number on the Adafruit FX Sound Board. Audio Track pins
are #defined at the top of this file.
Notes
****************************************************************************/
void pulseLow(uint8_t track)
{
switch (track)
{
case 1:
HWREG(GPIO_PORTF_BASE+(GPIO_O_DATA + ALL_BITS)) &= ~AUDIO_TRACK01;
ES_Timer_InitTimer(AUDIO_TIMER, ADAFRUIT_AUDIO_PULSE);
break;
case 2:
HWREG(GPIO_PORTF_BASE+(GPIO_O_DATA + ALL_BITS)) &= ~AUDIO_TRACK02;
ES_Timer_InitTimer(AUDIO_TIMER, ADAFRUIT_AUDIO_PULSE);
break;
case 3:
HWREG(GPIO_PORTC_BASE+(GPIO_O_DATA + ALL_BITS)) &= ~AUDIO_TRACK03;
ES_Timer_InitTimer(AUDIO_TIMER, ADAFRUIT_AUDIO_PULSE);
break;
}
}
/****************************************************************************
Function
StartTemplateSM
Parameters
None
Returns
None
Description
Does any required initialization for this state machine
Notes
Author
J. Edward Carryer, 2/18/99, 10:38AM
****************************************************************************/
void StartFollowLine(ES_Event CurrentEvent) {
// to implement entry to a history state or directly to a substate
// you can modify the initialization of the CurrentState variable
// otherwise just start in the entry state every time the state machine
// is started
if (ES_ENTRY_HISTORY != CurrentEvent.EventType) {
CurrentState = FollowWaiting;
}
ES_Timer_InitTimer(DRIVE_TIMER, 1);
// call the entry function (if any) for the ENTRY_STATE
RunFollowLine(CurrentEvent);
}
/****************************************************************************
Function
RunIRemitter
Parameters
ES_Event : the event to process
Returns
ES_Event, ES_NO_EVENT if no error ES_ERROR otherwise
Description
When the service receives a Reload Balls event, a timer is started and
interrurts are enabled to handle the pulses required to request a new
ball. Once the timer is finished, the robot will request more balls until
a total of 5 have been requested
Notes
Author
A. Han, L. Kim, 02/20/14, 16:20
J. Edward Carryer, 01/15/12, 15:23
****************************************************************************/
ES_Event RunIRemitter( ES_Event ThisEvent )
{
ES_Event ReturnEvent, NewEvent;
ReturnEvent.EventType = ES_NO_EVENT; // assume no errors
//Reload Balls Event Recieved. Start Timer and interrupts to create pulese
if (ThisEvent.EventType == RELOAD_BALLS)
{
count = 0;
ES_Timer_InitTimer(IRemitterTimer, 3000);
IRemitter_PORT |= IRemitter_PIN;
ReloadLED_PORT |= ReloadLED_PIN; //Turn the Reload LED on
InitTimer();
}
/* Timeout event. If 5 balls have been requested, disable interrupts
to stop pulses
*/
if (ThisEvent.EventType == ES_TIMEOUT)
{
if (ballCount < 5)
{
count = 0;
ES_Timer_InitTimer(IRemitterTimer, 3000);
IRemitter_PORT |= IRemitter_PIN;
ReloadLED_PORT |= ReloadLED_PIN; //Turn the Reload LED ON
InitTimer();
}
else
{
ballCount = 0;
ReloadLED_PORT &= ~ReloadLED_PIN; //Turn the Reload LED OFF
TIM0_TIE &= ~(_S12_C6I|_S12_C7I); //disable oc4,5 interrupt
NewEvent.EventType = StartShootingMotors;
PostShoot(NewEvent);
}
}
return ReturnEvent;
}
static ES_Event DuringAttack_t( 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
// Stop positioning when in the attack SM
ES_Timer_StopTimer(POSITION_CHECK);
// after that start any lower level machines that run in this state
//StartLowerLevelSM( Event );
StartAttackSM(Event);
// 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);
ReturnEvent = RunAttackSM(Event);
// repeat for any concurrently running state machines
// now do any local exit functionality
// Count the number of shots, and don't start a next shot timer
// if we are out of ammo
shotCounter++;
if (shotCounter < 5)
{
ES_Timer_InitTimer(ATTACK_PHASE_TIMER, NEXT_SHOT_T);
}
// Reset positioning
ES_Event ResetEvent;
ResetEvent.EventType = ES_RESET_DESTINATION;
PostMasterSM(ResetEvent);
}else
// do the 'during' function for this state
{
// run any lower level state machine
// ReturnEvent = RunLowerLevelSM(Event);
ReturnEvent = RunAttackSM(Event);
// repeat for any concurrent lower level machines
// 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 sendControlReq(uchar hzvNum)
{
sendData[0]=0x03; //send 0x03, 0xED message to control
sendData[1]=0xED;
currHzvNum=hzvNum;
if(currHzvNum<NUM_HZVS)
{
sendMessage(sendData,2,0x21,0x80 + currHzvNum);
ES_Timer_InitTimer(COMMAND_TIMER,200); //start the retry timer.
printf("control req: 21%x\r\n",0x80+currHzvNum);
} else
{
printf("invalid boat num");
}
}
/******************** Module Code ***********************/
bool InitLEDControl(uint8_t Priority) {
LED_SR_Init();
MyPriority = Priority;
// start currentFlashState as off
currentFlashState = OFF;
// clear currentLEDVals
currentLEDVals = 0;
// clear flashingLEDs
flashingLEDs = 0;
// start flash timer
ES_Timer_InitTimer(LED_FLASH_TIMER, FLASH_TIME);
return true;
}
/**
* @Function RunTemplateService(ES_Event ThisEvent)
* @param ThisEvent - the event (type and param) to be responded.
* @return Event - return event (type and param), in general should be ES_NO_EVENT
* @brief This function is where you implement the whole of the service,
* as this is called any time a new event is passed to the event queue.
* @note Remember to rename to something appropriate.
* Returns ES_NO_EVENT if the event have been "consumed."
* @author J. Edward Carryer, 2011.10.23 19:25 */
ES_Event RunTestBeaconDetectorService(ES_Event ThisEvent)
{
ES_Event ReturnEvent;
ReturnEvent.EventType = ES_NO_EVENT; // assume no errors
static unsigned char IRstate[] = {0,0,0,0,0,0}, LastIRState = 0x00;
static uint8_t index = 0;
/********************************************
in here you write your service code
*******************************************/
// printf("ll");
if (ThisEvent.EventType == ES_TIMEOUT)// only respond to ES_Init
{
ES_Timer_InitTimer(IRtimer, IRtimervalue); // IR sensor check every 5ms
// ES_Timer_StartTimer(IRtimervalue);
// No hardware initialization or single time setups, those
// go in the init function above.
//
// This section is used to reset service for some reason
IRstate[index] = Dagobot_ReadIR();
BOOL flag = FALSE;
int i;
//for(i=1; i<2; i++){
if(LastIRState != IRstate[index] ){
// debug("here");
// printf("\nhere");
LastIRState = IRstate[index];
flag = TRUE;
}
//}
if(flag) {
// printf("\nhere");
ThisEvent.EventType = SNIPED;
ThisEvent.EventParam = IRstate[index];
PostDagobotHSM(ThisEvent);
}
}
index++;
if(index == 5) index = 0;
return ReturnEvent;
}
/***************************************************************************
Interrupt Responses
***************************************************************************/
void CannonEncoder_InterruptResponse(void){
uint32_t ThisCapture;
// start by clearing the source of the interrupt, the input capture event
clearCaptureInterrupt(CANNON_ENCODER_INTERRUPT_PARAMATERS);
// now grab the captured value and calculate the period
ThisCapture = captureInterrupt(CANNON_ENCODER_INTERRUPT_PARAMATERS);
//Update the Period based on the difference between the two rising edges
Period = ThisCapture - LastCapture;
// update LastCapture to prepare for the next edge
LastCapture = ThisCapture;
//Restart the Timer for the Cannon being Stopped
ES_Timer_InitTimer(CANNON_STOPPED_TIMER, CANNON_STOPPED_T);
}
void sendMessage(uchar* data, uchar len, uchar addrMSB, uchar addrLSB)
{
static uchar options = 0;
int i;
uchar chksum=0;
if(canSend==0)
return; //cant send yet, waiting for status to come back
canSend=0;
for(i=0;i<len;i++) //save this message away
{
lastData[i]=data[i];
lastLen=len;
lastMSB=addrMSB;
lastLSB=addrLSB;
}
ES_Timer_InitTimer(COMMAND_TIMER,200); //start the retry timer.
frameID++;
sendByte(0x7E); //1 start delimiter
sendByte(0); //2 MSB is always 0
sendByte(5+len); //3 Length of frame data is 5 bytes ( api id, frame id, destaddr msb, dest addr lsb, option) + message length
//-----Frame Data-------
sendByte(0x01); //4 API ID for sending
chksum+=0x01; //add this to check sum
if(frameID==0) //0 means something special, avoid 0.
frameID++;
sendByte(frameID); //5
chksum+=frameID;
sendByte(addrMSB); //6 - MSB
chksum+=addrMSB;
sendByte(addrLSB); //7 - LSB
chksum+=addrLSB;
sendByte(options); //disable ACK (try 0x01 if this isnt working)
chksum+=options; //options 0 being sent.
for(i=0;i<len;i++)
{
sendByte(data[i]);
chksum+=data[i];
}
chksum = 0xFF - chksum; //checksum + sum = 0xFF
sendByte(chksum);
printf("send success %i\r\n",frameID);
}
//Service Run Function
ES_Event RunStartService (ES_Event ThisEvent)
//The EventType field of ThisEvent will be one of: ES_TIMEOUT
{
ES_Event ReturnEvent;
ReturnEvent.EventType = ES_NO_EVENT;
uint32_t *QueryDRS; // To store the data received from querying the DRS
static uint8_t LastGameStatus=0;
if (ThisEvent.EventType==ES_TIMEOUT && ThisEvent.EventParam==CHECK_TIMER)
{
QueryDRS = QueryReceivedData(); // Call the function to receive the DRS data
uint8_t CurrentGameStatus = (*(QueryDRS+3)&(BIT3HI|BIT4HI));
if (CurrentGameStatus!=LastGameStatus)
{
if (CurrentGameStatus == BIT3HI)
{
// Flag Dropped
ES_Event NewEvent;
NewEvent.EventType = GameStart;
PostRobotSM(NewEvent);
printf("Game Started\n\r");
}
else if (CurrentGameStatus == (BIT3HI|BIT4HI))
{
// Caution Flag
ES_Event NewEvent;
NewEvent.EventType = CautionFlag;
PostRobotSM(NewEvent);
printf("Game Stopped\n\r");
}
}
LastGameStatus=CurrentGameStatus; // Updating last game status
ES_Timer_InitTimer(CHECK_TIMER,20);
}
return ReturnEvent;
}
void main(void) {
ES_Return_t ErrorType;
uint8_t i;
BOARD_Init();
// When doing testing, it is usefull to annouce just which program
// is running.
printf("Starting Timer Service Test Harness \r\n");
printf("using the 2nd Generation Events & Services Framework\n\r");
// Your hardware initialization function calls go here
// now initialize the Events and Services Framework and start it running
ErrorType = ES_Initialize();
timerServiceTestingState = init;
for (i = 0; i <= TIMERS_USED; i++) {
ES_Timer_InitTimer(i, (i + 1)*1000); //for second scale
}
if (ErrorType == Success) {
ErrorType = ES_Run();
}
//if we got to here, there was an error
switch (ErrorType) {
case FailedPointer:
printf("Failed on NULL pointer");
break;
case FailedInit:
printf("Failed Initialization");
break;
default:
printf("Other Failure");
break;
}
for (;;)
;
};
static ES_Event DuringRev_t( 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
// Start the timer for us to enter the attack phase
ES_Timer_InitTimer(ATTACK_PHASE_TIMER, ATTACK_PHASE_T);
// rev the cannon up to speed
setTargetCannonSpeed(REV_SPEED);
// 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
}
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
// 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);
}
/****************************************************************************
Function
InitJSRcommand
Parameters
uint8_t : the priorty of this service
Returns
bool, false if error in initialization, true otherwise
Description
Saves away the priority, and does any
other required initialization for this service
Notes
Author
J. Edward Carryer, 01/16/12, 10:00
****************************************************************************/
bool InitJSRcommand ( uint8_t Priority )
{
ES_Event ThisEvent;
MyPriority = Priority;
// post the initial transition event
DDRS |= BIT7HI; // To control SS line manually
RED_DARK_ADDRESS &= ~RED_DARK_PIN; // Set pin as input
RED_DARK_PORT &= ~RED_DARK_PIN; //Set button low
InitSPI();
readFrom = 4;// Looking for the 4th command from JSR
InitVariables();
QueryCommand = STATUS_QUERY;
ES_Timer_InitTimer(JSRtimer, 3); // Start JSRtimer for 2ms.
ThisEvent.EventType = ES_INIT;
if (ES_PostToService( MyPriority, ThisEvent) == true)
return true;
else
return false;
}
static ES_Event DuringWaitingState( 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
ES_Timer_InitTimer(COM_TIMER, WAIT_TIME);
// 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
// no lower Level SMs here
}
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
printf("\r\n In WAITING");
// 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);
}
//The interrupt response for our phototransistor
void PhotoTransistor_InterruptResponse(void)
{
// Clear Interrupt
clearCaptureInterrupt(PHOTOTRANSISTOR_INTERRUPT_PARAMATERS);
// Determine Capture time
uint32_t ThisCapture = captureInterrupt(PHOTOTRANSISTOR_INTERRUPT_PARAMATERS);
// Calculate period
uint32_t Period = ((ThisCapture - LastCapture) * MICROSECONDS_DIVISOR ) / TICKS_PER_MS;
//Store the Last Cpature
LastCapture = ThisCapture;
//Iterate Through the different frequency options
for (int i = 0; i < NUMBER_BEACON_FREQUENCIES; i++)
{
// If the period matches a beacon period
if (ToleranceCheck(Period, beacons[i].period, PERIOD_MEASURING_ERROR_TOLERANCE))
{
// If we're searching for a beacon
if (Bucketing)
{
buckets[i]++;
}
// If we're supposed to align to the bucket, and the number of pulses we've seen for
// this beacon is greater than our threshold
if ((AligningToBucket) && (buckets[i] >= NUMBER_PULSES_FOR_BUCKET))
{
// If this is the beacon corresponding to our target bucket
if (((MyColor() == COLOR_BLUE) && (i == BEACON_INDEX_NW)) || ((MyColor() == COLOR_RED) && (i == BEACON_INDEX_SE)))
{
// stop our drive
clearDriveAligningToBucket();
// Post an aligned event
ES_Event AlignedEvent;
AlignedEvent.EventType = ES_ALIGNED_TO_BUCKET;
PostMasterSM(AlignedEvent);
// stop aligning
AligningToBucket = false;
// reset all buckets
for (int j = 0; j < NUMBER_BEACON_FREQUENCIES; j++)
{
buckets[j] = 0;
}
// reset average information
ResetAverage();
// return to searching for beacons
Bucketing = true;
LastBeacon = NULL_BEACON;
ES_Timer_StopTimer(AVERAGE_BEACONS_TIMER);
return;
}
}
// If we're not aligning to a bucket, and the number of pulses we've seen for this
// beacon is greater than our threshold
else if (buckets[i] >= NUMBER_PULSES_TO_BE_ALIGNED && LastBeacon == NULL_BEACON && !AligningToBucket)
{
// store the beacon to be recorded
LastBeacon = i;
// reset all buckets
for (int j = 0; j < NUMBER_BEACON_FREQUENCIES; j++)
{
buckets[j] = 0;
}
// restart the timer to evaluate the beacon
ES_Timer_InitTimer(AVERAGE_BEACONS_TIMER, AVERAGE_BEACONS_T);
Bucketing = false;
}
// increment the sum of all encoder angles for this beacon
sums[i] += GetPeriscopeAngle();
// increment the number of pulses seen for this beacon
numSamples[i]++;
// If the evaluation timer is already running, restart it
ES_Timer_SetTimer(AVERAGE_BEACONS_TIMER, AVERAGE_BEACONS_T);
}
}
}
ES_Event RunTankSM( ES_Event CurrentEvent )
{
unsigned char MakeTransition = False;/* are we making a state transition? */
TankState_t NextState = CurrentState;
ES_Event EntryEventKind = { ES_ENTRY, 0 };// default to normal entry to new state
switch( CurrentState )
{
case CALIBRATING_HOPPER :
// During function time, gives low-level SMs priority in dealing w/event
CurrentEvent = DuringCalibration(CurrentEvent);
// we only process 1 event in this state, the "Balls in Play" flag to start the game
if ( CurrentEvent.EventType == CALIBRATION_COMPLETE)
{
NextState = WAITING_FOR_START;//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;
RIGHT_OR_FRONT_HOPPER_PWM=0;
LEFT_OR_REAR_HOPPER_PWM=0;
InitializeCheckers();
}
break;
case WAITING_FOR_START :
// During function time, gives low-level SMs priority in dealing w/event
CurrentEvent = DuringWaitingForStart(CurrentEvent);
// we only process 1 event in this state, the "Balls in Play" flag to start the game
if ( CurrentEvent.EventType == BALLS_IN_PLAY)
{
// Execute action function for state one : event one
#ifndef RUN_HOPPER_CALIBRATION_ROUTINE
ES_Event TriangulateEvent;
TriangulateEvent.EventType = FIND_POSITION;
TriangulateEvent.EventParam = 1;
ES_PostList05(TriangulateEvent);
ES_Timer_InitTimer(MAIN_GAME_TIMER, TWO_MINUTES);
puts("Whelp, we started that timer.\n\r");
#endif
NextState = PLAYING;//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;
}
break;
case PLAYING :
// During function time, gives low-level SMs priority in dealing w/event
CurrentEvent = DuringPlaying(CurrentEvent);
// we only process 1 event in this state, the "Timer Out" flag to end the game
switch ( CurrentEvent.EventType )
{
case ES_TIMEOUT :
// See if it's the game timer
if ( CurrentEvent.EventParam == MAIN_GAME_TIMER )
{
puts("And, it expired. Sweet.\n\r");
NextState = DISABLED;//Decide what the next state will be
Drive(ZERO_SPEED);
LEFT_OR_REAR_HOPPER_PWM = 0;
RIGHT_OR_FRONT_HOPPER_PWM = 0;
PTU &= ~BLUE_LED;
PTU &= ~RED_LED;
TIM0_TIE = TIM0_TIE & ~_S12_C7I; //disable OC4 (stepper) interrupt
TIM0_TCTL1 = TIM0_TCTL1 & ~_S12_OL7;
// 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;
}
break;
case MASTER_RESET :
// See if it's the game timer
if ( CurrentEvent.EventParam == MAIN_GAME_TIMER )
{
puts("And, it expired. Sweet.\n\r");
NextState = DISABLED;//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;
}
break;
}
break;
case DISABLED :
// During function time, gives low-level SMs priority in dealing w/event
CurrentEvent = DuringHolding(CurrentEvent);
// we only process 1 event in this state, the "Master Reset" flag which will probably never happen
if ( CurrentEvent.EventType == MASTER_RESET)
{
puts("Whoa! Who pushed Master Reset? And how did you do it?.\n\r");
NextState = WAITING_FOR_START;//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;
}
break;
}
// If we are making a state transition
if (MakeTransition == True)
{
// Execute exit function for current state
CurrentEvent.EventType = ES_EXIT;
RunTankSM(CurrentEvent);
CurrentState = NextState; //Modify state variable
// Execute entry function for new state
// this defaults to ES_ENTRY
RunTankSM(EntryEventKind);
}
// in the absence of an error the top level state machine should
// always return ES_NO_EVENT
CurrentEvent.EventType = ES_NO_EVENT;
return(CurrentEvent);
}
/**
* Runs the SendByte state machine.
* @param CurrentEvent: the event to process
* @return an event to return
*/
ES_Event RunSendByteSM(ES_Event CurrentEvent) {
bool MakeTransition = false; // are we making a state transition?
SendByteState_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 WaitingByte: {
CurrentEvent = DuringWaitingByte(CurrentEvent);
if (CurrentEvent.EventType != ES_NO_EVENT) {
if (CurrentEvent.EventType == SendByte) {
sendData(CurrentEvent.EventParam); // write the byte to the SSI data register
//printf("%X\r\n",CurrentEvent.EventParam); // DEBUG *****************************************************
NextState = WaitingForEOT;
MakeTransition = true;
}
}
}
break;
case WaitingForEOT: {
//printf("\r\n XX");
CurrentEvent = DuringWaitingForEOT(CurrentEvent);
//printf("\r\n 1");
if (CurrentEvent.EventType != ES_NO_EVENT) {
if (CurrentEvent.EventType == EndOfTransmit) {
storeData(); // store received data
NextState = WaitingForTimeout;
MakeTransition = true;
ES_Timer_InitTimer(DRS_TIMER, WAIT_TIME); // start DRS timer
}
}
}
break;
case WaitingForTimeout: {
//printf("\r\n XXX");
CurrentEvent = DuringWaitingForTimeout(CurrentEvent);
if (CurrentEvent.EventType != ES_NO_EVENT) {
if ((CurrentEvent.EventType == ES_TIMEOUT) && (CurrentEvent.EventParam == DRS_TIMER)) {
//printf("Got timeout\r\n"); //******************************************************************************************************************
//NextState = WaitingByte;
//MakeTransition = true;
MakeTransition = false;
ReturnEvent = CurrentEvent;
CurrentState = WaitingByte;
}
}
}
break;
}
/* If we are making a state transition */
if (MakeTransition == true) {
/* Execute exit function for current state */
CurrentEvent.EventType = ES_EXIT;
RunSendByteSM(CurrentEvent);
CurrentState = NextState; // modify state variable
/* Execute entry function for new state */
/* This defaults to ES_ENTRY */
RunSendByteSM(EntryEventKind);
}
return ReturnEvent;
}
/**
* @Function RunRoachFSM(ES_Event ThisEvent)
* @param ThisEvent - the event (type and param) to be responded.
* @return Event - return event (type and param), in general should be ES_NO_EVENT
* @brief This is the function that implements the actual state machine; in the
* roach case, it is a simple ping pong state machine that will form the
* basis of your more complicated exploration. This function will be called
* recursively to implement the correct order for a state transition to be:
* exit current state -> enter next state using the ES_EXIT and ES_ENTRY
* events.
* @author Gabriel H Elkaim, 2013.09.26 15:33 */
ES_Event RunMotorYFSM(ES_Event ThisEvent) {
uint8_t makeTransition = FALSE;
Motor_Y_State_t nextState;
ES_Tattle(); // trace call stack
static unsigned int eventParam = 0;
static unsigned int i = 0;
static unsigned int j = 0;
switch (CurrentState) {
case InitQState:
if (ThisEvent.EventType == ES_INIT) {
nextState = selfCalibrateY;
makeTransition = TRUE;
}
break;
case selfCalibrateY:
switch (ThisEvent.EventType) {
case ES_ENTRY:
break;
case CALIBRATED:
if (ThisEvent.EventParam == MOTOR_X) {
Stepper_Init(MOTOR_Y, 77);
reverseY(4000);
ES_Timer_InitTimer(MOTOR_Y_TIMER, 5000);
}
break;
case Y_LIMIT:
if (ThisEvent.EventParam == Y_MIN_MASK) {
initialY = getFilteredAD(Y_SLIDER);
newPosition = AD_SCALE_Y * initialY;
currentPosition = 0;
setStepsTaken(MOTOR_Y, 0);
positionDifference = currentPosition - newPosition;
if (positionDifference < 0) {
//forwardX(abs(positionDifference));
} else if (positionDifference > 0) {
1; //do nothing, already at zero
}
Stepper_Halt(MOTOR_Y);
}
break;
//case Y_LIMIT:
case ES_TIMEOUT:
if (ThisEvent.EventParam == MOTOR_Y_TIMER) {
initialY = getFilteredAD(Y_SLIDER);
//Stepper_Resume(MOTOR_Y);
newPosition = AD_SCALE_Y * initialY;
currentPosition = 0;
setStepsTaken(MOTOR_Y, 0);
positionDifference = currentPosition - newPosition;
if (positionDifference < 0) {
//forwardY(abs(positionDifference));
} else if (positionDifference > 0) {
1; //do nothing, already at zero
}
nextState = waitingY;
makeTransition = TRUE;
}
break;
case ES_EXIT:
Stepper_Halt(MOTOR_Y);
ES_Timer_StopTimer(MOTOR_Y_TIMER);
break;
default:
break;
}
break;
case waitingY:
if (ThisEvent.EventType != ES_NO_EVENT) {
switch (ThisEvent.EventType) {
case ES_ENTRY:
break;
case XY_CHANGE:
nextState = movingBufferY;
makeTransition = TRUE;
memEventY = ThisEvent;
break;
case PRESETCHANGE:
//Coming from presetFSM
//nextState = movingY_Preset;
//makeTransition = TRUE;
break;
case ES_EXIT:
Stepper_Resume(MOTOR_Y);
setStepsRate(MOTOR_Y, 77);
//ES_Timer_StopTimer(MOTOR_Y_TIMER);
break;
default:
break;
}
}
break;
case movingBufferY:
if (ThisEvent.EventType != ES_NO_EVENT) {
switch (ThisEvent.EventType) {
case ES_ENTRY:
ES_Timer_InitTimer(MOTOR_Y_TIMER, 200);
break;
case XY_CHANGE:
nextState = movingBufferY;
makeTransition = TRUE;
memEventY = ThisEvent;
break;
case ES_TIMEOUT:
if (ThisEvent.EventParam = MOTOR_Y_TIMER) {
nextState = movingY;
makeTransition = TRUE;
}
break;
case ES_EXIT:
setStepsRate(MOTOR_Y, 77);
break;
default:
break;
}
}
break;
case movingY:
switch (ThisEvent.EventType) {
case ES_ENTRY:
Stepper_Init(MOTOR_Y, 77);
newPosition = AD_SCALE_Y * memEventY.EventParam;
currentPosition = getStepsTaken(MOTOR_Y);
positionDifference = currentPosition - newPosition;
if (positionDifference < 0) {
yDirection = FORWARD;
forwardY(abs(positionDifference));
} else {
yDirection = REVERSE;
reverseY(abs(positionDifference));
}
break;
case ALMOSTOUTOFSTEPS:
setStepsRate(MOTOR_Y, 15);
break;
case OUTOFSTEPS:
Stepper_Halt(MOTOR_Y);
ES_Timer_InitTimer(MOTOR_Y_TIMER, 50);
setStepsRate(MOTOR_Y, 77);
break;
case ES_TIMEOUT:
if (ThisEvent.EventParam == MOTOR_Y_TIMER) {
Stepper_Halt(MOTOR_Y);
nextState = waitingY;
makeTransition = TRUE;
}
break;
case XY_CHANGE:
nextState = movingBufferY;
makeTransition = TRUE;
memEventY = ThisEvent;
break;
case ES_EXIT:
lastYDirection = yDirection;
setStepsRate(MOTOR_Y, 77);
ES_Timer_StopTimer(MOTOR_Y);
break;
default:
nextState = waitingY;
makeTransition = TRUE;
break;
}
break;
case movingBufferY2:
//ThisEvent = RunmovingYSubFSM(ThisEvent);
if (ThisEvent.EventType != ES_NO_EVENT) {
switch (ThisEvent.EventType) {
case ES_ENTRY:
//printf("movingBuffer Enter:%d\n", memEventY.EventParam);
ES_Timer_InitTimer(MOTOR_Y_TIMER, 200);
break;
case XY_CHANGE:
//printf("Y_CHANGE\n");
//determine if a change of direction is about to occur
newPosition = AD_SCALE_Y * ThisEvent.EventParam;
currentPosition = getStepsTaken(MOTOR_Y);
positionDifference = currentPosition - newPosition;
if (positionDifference < 0) {
yDirection = FORWARD;
} else {
yDirection = REVERSE;
}
//if direction change, start slowing down
memEventY = ThisEvent;
break;
case ES_TIMEOUT:
if (ThisEvent.EventParam = MOTOR_Y_TIMER) {
printf("yDirection:%d, lastYDirection:%d\n", yDirection, lastYDirection);
if (yDirection != lastYDirection) {
printf("directionChange!\n");
//setStepsRate(MOTOR_Y, 15);
//make an event instead!
//ES_Timer_InitTimer(MOTOR_Y, 150);
//while (!(getStepsRate(MOTOR_Y) < 20));
nextState = changingDirection;
makeTransition = TRUE;
} else {
nextState = movingY;
makeTransition = TRUE;
}
}
break;
case ES_EXIT:
//Stepper_Init(MOTOR_Y,500);
break;
default:
break;
}
}
break;
case changingDirection:
switch (ThisEvent.EventType) {
case ES_ENTRY:
//printf("movingBuffer Enter:%d\n", memEventY.EventParam);
//ES_Timer_InitTimer(MOTOR_Y_TIMER, 50);
Stepper_ChangeStepRate(MOTOR_Y, 15);
break;
case SPEEDMATCH:
printf("speedmatch!\n");
nextState = movingY;
makeTransition = TRUE;
break;
case XY_CHANGE:
//printf("Y_CHANGE\n");
nextState = movingBufferY2;
makeTransition = TRUE;
memEventY = ThisEvent;
break;
case ES_TIMEOUT:
if (ThisEvent.EventParam = MOTOR_Y_TIMER) {
nextState = movingY;
makeTransition = TRUE;
}
break;
case ES_EXIT:
//Stepper_Init(MOTOR_Y,500);
break;
default:
break;
}
break;
default:
break;
}
if (makeTransition == TRUE) {
RunMotorYFSM(EXIT_EVENT);
lastState = CurrentState;
CurrentState = nextState;
RunMotorYFSM(ENTRY_EVENT);
}
//printf("Leaving RunMotorYFSM with CurrentState = %d\n", CurrentState);
ES_Tail(); // trace call stack end
return ThisEvent;
}
/****************************************************************************
Function
RunJSRcommand
Parameters
ES_Event : the event to process
Returns
ES_Event, ES_NO_EVENT if no error ES_ERROR otherwise
Description
Code for receiving and parsing message received through SPI bus from JSR
Notes
Author
A. Han, 02/20/14, 17:30
J. Edward Carryer, 01/15/12, 15:23
****************************************************************************/
ES_Event RunJSRcommand( ES_Event ThisEvent )
{
static unsigned char dummy;
ES_Event ReturnEvent;
ReturnEvent.EventType = ES_NO_EVENT; // assume no errors
if (RED_DARK_PORT&RED_DARK_PIN == RED_DARK_PIN) //Hi
{
RELOAD_STATUS = RED_RELOAD_STATUS; //RED KNIGHT
DontChangeKnightFlag = true;
}
else if (DontChangeKnightFlag == false)
{
RELOAD_STATUS = DARK_RELOAD_STATUS; //DARK KNIGHT
}
switch (ThisEvent.EventType)
{
case QUERY4STATUS:
readFrom = 4;// Looking for the 4th command from JSR
InitVariables();
QueryCommand = STATUS_QUERY;
break;
case QUERY4SCORE :
readFrom = 3;// Looking for the 3rd and 4th command from JSR
InitVariables();
QueryCommand = SCORE_QUERY;
break;
case ES_TIMEOUT : // re-start timer & announce
if ((SPISR & _S12_SPTEF) == _S12_SPTEF) //if query can be sent
{
i++;
if (i==1)
{
PTS &= BIT7LO; // Set SS line low to recieve 4 bytes
SPIDR = QueryCommand; //Send Status Query
} else
{
SPIDR = 0x00;
}
}
break;
case FLAGSET:
if ((SPISR & _S12_SPIF) == _S12_SPIF) //If Data can be received
{
ByteInfo = SPIDR;
dummy = (BIT0HI|BIT1HI|BIT2HI)&(ByteInfo) ; //Reading Bit 0,1,2
if (i<readFrom)
{
dummy = SPIDR;
}
else if (i == readFrom)
{
HeadStatus = ByteInfo & HEAD_STATUS;
ReloadStatus = ByteInfo & RELOAD_STATUS;
command1 = dummy;//SPIDR;
if (command1 != LastCommand1)
{
ES_Event ThisEvent;
LastCommand1 = command1;
ThisEvent.EventType = NEW_COMMAND_RECEIVED;
ThisEvent.EventParam = command1;
PostBot(ThisEvent);
}
}
else if (i > readFrom)
{
command2 = dummy;//SPIDR;
if (command2 != LastCommand2)
{
ES_Event ThisEvent;
LastCommand2 = command2;
ThisEvent.EventType = NEW_COMMAND_RECEIVED;
ThisEvent.EventParam = command2;
PostBot(ThisEvent);
}
}
if(ES_Timer_InitTimer(JSRtimer, 3)==ES_Timer_OK)// Start JSRtimer 3ms
if (i==4)
{
// Set SS line high after receiving 4 bytes
i=0;
PTS |= BIT7HI;
}
}
break;
}
return ReturnEvent;
}
/****************************************************************************
Function
RunZoneFourSM
Parameters
ES_Event: the event to process
Returns
ES_Event: an event to return
Description
add your description here
Notes
uses nested switch/case to implement the machine.
Author
J. Edward Carryer, 2/11/05, 10:45AM
****************************************************************************/
ES_Event RunZoneFourSM( ES_Event CurrentEvent )
{
bool MakeTransition = false;/* are we making a state transition? */
ZoneFourState_t NextState = CurrentState;
ES_Event EntryEventKind = { ES_ENTRY, 0 };// default to normal entry to new state
ES_Event ReturnEvent = CurrentEvent; // assume we are not consuming event
switch ( CurrentState )
{
case FOLLOW_TAPE_ZONE_FOUR :
printf("FOLLOW_TAPE_ZONE_FOUR \r\n");
CurrentEvent = DuringFollowTape(CurrentEvent);
if ( CurrentEvent.EventType != ES_NO_EVENT )
{
switch (CurrentEvent.EventType)
{
case CenterOfObstacle:
printf("Center Of Obstacle \r\n");
StopMotor();
NextState = ALIGN_WITH_OBSTACLE;
MakeTransition = true;
EntryEventKind.EventType = ES_ENTRY_HISTORY;
ReturnEvent.EventType = ES_NO_EVENT;
break;
}
}
break;
//really driving forward
case BACK_AWAY_FROM_OBSTACLE :
printf("BACK_AWAY_FROM_OBSTACLE \r\n");
CurrentEvent = DuringBackAwayFromObstacle(CurrentEvent);
if ( CurrentEvent.EventType != ES_NO_EVENT )
{
switch (CurrentEvent.EventType)
{
case PositionReached :
printf("Position Reached \r\n");
StopMotor();
ES_Timer_InitTimer(BACK_UP_TIMER,POST_BACK_UP_TIME);
break;
case ES_TIMEOUT :
if(CurrentEvent.EventParam == BACK_UP_TIMER){
NextState = ALIGN_WITH_OBSTACLE_ROUND_TWO;
MakeTransition = true;
EntryEventKind.EventType = ES_ENTRY_HISTORY;
ReturnEvent.EventType = ES_NO_EVENT;
}
break;
}
}
break;
case ALIGN_WITH_OBSTACLE_ROUND_TWO :
printf("ALIGN_WITH_OBSTACLE_ROUND_TWO \r\n");
CurrentEvent = DuringAlignWithObstacleRoundTwo(CurrentEvent);
if ( CurrentEvent.EventType != ES_NO_EVENT )
{
switch (CurrentEvent.EventType)
{
case TurnComplete :
printf("Turn Complete \r\n");
StopMotor();
setZonesForAlign(0, 0,true);
//enableSpeedControl(true, INTO_RAMP_SPEED, false); //was 100
//enableSpeedControl(true, INTO_RAMP_SPEED, true); //was 100
SetPWMDutyDrive(-99, -99);
//enableSpeedControl(true, INTO_RAMP_SPEED,false);
break;
}
}
break;
case ALIGN_WITH_OBSTACLE :
printf("ALIGN_WITH_OBSTACLE \r\n");
CurrentEvent = DuringAlignWithObstacle(CurrentEvent);
if ( CurrentEvent.EventType != ES_NO_EVENT )
{
switch (CurrentEvent.EventType)
{
case TurnComplete:
//case PositionReached:
printf("Turn Complete \r\n");
StopMotor();
NextState = BACK_AWAY_FROM_OBSTACLE;
//enableSpeedControl(true, 100, false);
MakeTransition = true;
EntryEventKind.EventType = ES_ENTRY_HISTORY;
ReturnEvent.EventType = ES_NO_EVENT;
break;
}
}
break;
}
if (MakeTransition == true)
{
CurrentEvent.EventType = ES_EXIT;
RunZoneFourSM(CurrentEvent);
CurrentState = NextState;
RunZoneFourSM(EntryEventKind);
}
return(ReturnEvent);
}
