/*************************************************************************** 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); }