示例#1
0
void wait(unsigned long waitTime)
{
   
   unsigned long startTime = ES_Timer_GetTime();
   unsigned long currTime= ES_Timer_GetTime();
   printf("Wait %i ms\r\n",(int) waitTime);
   while(currTime-startTime<waitTime && currTime>=startTime)
   {
      driveDebug();
      currTime=ES_Timer_GetTime();
   }   
}
示例#2
0
uchar Check4ControlReq(void)
{
   long currTime=ES_Timer_GetTime();
   ES_Event ThisEvent;
   uchar retVal=0;
   uchar hzvNum;


   if(currTime-lastContTime > TIME_PER_PHONE_CHECK || currTime<lastContTime)
   {
      hzvNum=getKeyboardReq();
      if(hzvNum<NUM_HZVS)
      {
         ThisEvent.EventType=ES_ControlBypass;
         ThisEvent.EventParam=0x0A;
         PostLabFSM(ThisEvent);
         retVal=1;
         lastContTime=currTime;
      } 
      
      hzvNum=getPhoneReq(); //returns NUM_HZVS if no phone req is made
      if(hzvNum<NUM_HZVS) //we got a valid hzvNum to target 
      {
       ThisEvent.EventType=ES_ControlReq;
       ThisEvent.EventParam= hzvNum;
       PostLabFSM(ThisEvent);
       retVal=1;
       lastContTime=currTime;
      } 
  }
  return retVal;
}
示例#3
0
uchar Check4Ack(void)
{
   long currTime=ES_Timer_GetTime();
   ES_Event ThisEvent;
   uchar retVal=0;

   if(currTime-lastRcvTime > TIME_PER_EV_CHECK || currTime<lastRcvTime)
   {
      if(gotAck())
      {
         ThisEvent.EventType=ES_Ack;
         PostLabFSM(ThisEvent);
         retVal=1;
         lastRcvTime=currTime;
         printf("ACK\r\n");
      } 
      else if(gotControlAck())
      {
         ThisEvent.EventType=ES_ControlAck;
         PostLabFSM(ThisEvent);
         retVal=1;
         lastRcvTime=currTime;
         printf("CONT ACK\r\n");
      }
  }
  return retVal;
}
boolean CheckButtonEvents(void)
{
//Local ReturnVal = False
    boolean ReturnVal = False;
    unsigned int CurrentTime = ES_Timer_GetTime();
//Get current button state
    char CurrentButtonState = PTP&BIT0HI;
//If more than the debounce interval has elapsed since the last sampling
    if((CurrentTime-TimeOfLastButtonSample)>DEBOUNCE_TIME)
    {
//puts("latch");
        //If the current button state is different from the LastButtonState
        if ((CurrentButtonState != LastButtonState) &&(CurrentButtonState != 0))
        {

            //PostEvent ButtonDown (whether down or up)
            ES_Event ThisEvent;
            ThisEvent.EventType = BUTTON_PRESSED;
            ThisEvent.EventParam = 1;
            ES_PostList00(ThisEvent);
            //printf("button fired.\n\r");
            //Record current time as TimeOfLastSample
            TimeOfLastButtonSample = ES_Timer_GetTime();


            //Set ReturnVal = True
            ReturnVal = True;

            //Set LastButtonState to the current button state

            //Endif
        }



    }
// update variables
    LastButtonState = CurrentButtonState;
//Return ReturnVal
    return ReturnVal;
//End of CheckButtonEvents
}
示例#5
0
/****************************************************************************
 Function
     generateNewKey

 Description
     Randomly generates a new, 32 byte key

****************************************************************************/
uint8_t * GenerateNewKey( void){
	currentCipherPosition = 0;
	uint16_t TimeSeed;
	TimeSeed = ES_Timer_GetTime();
	srand(TimeSeed);
	for(int i = 0; i < KEY_LENGTH; i ++){
		uint8_t nextByte = rand();
		*(encryptionKey + i) = nextByte;
	}
	return encryptionKey;
}
示例#6
0
void initCheckers(void)
{
   lastHandshake=ES_Timer_GetTime();
   lastRcvTime=ES_Timer_GetTime();
   classifyStartTime=ES_Timer_GetTime();
   currHit[LEFT]=0;
   currHit[RIGHT]=0; 
   bg[LEFT]=0;
   bg[RIGHT]=0;
   speedState[0]=0;
   speedState[1]=0;
   currHit[0]=ES_Timer_GetTime();
   currHit[1]=ES_Timer_GetTime();
   prevHit[0]=ES_Timer_GetTime();
   prevHit[1]=ES_Timer_GetTime();
}
示例#7
0
uchar Check4Handshake(void)
{
   long currTime=ES_Timer_GetTime();
   ES_Event ThisEvent;
   uchar retVal=0;

   if(currTime-lastHandshake > TIME_PER_EV_CHECK || currTime<lastRcvTime)
   {
      if(gotHandshake())
      {
       ThisEvent.EventType=ES_Handshake;
       PostLabFSM(ThisEvent);
       retVal=1;
       lastRcvTime=currTime;
       printf("Handshake\r\n");
      } 
  }
  return retVal;
}
示例#8
0
char getBucket(uchar i)
{
   long currTime=ES_Timer_GetTime();
   long dt=currHit[i]-prevHit[i];
   if(currTime-currHit[i]>1000 || currHit[i]>currTime) //how long has passed since last hit
   {
      return 0; //havent seen this bucket in awhile
   } 
   else
   {
      if(dt<300) //FAST hit time
         return 3;
      else if(dt<500) //MED hit time.
         return 2;
      else //SLOW hit time
         return 1;
   }
   
}
示例#9
0
/**
 * @Function RunTimerService(ES_Event ThisEvent)
 * @param ES_Event - the event to process
 * @return ES_NO_EVENT or ES_ERROR 
 * @brief  accepts the timer events and updates the state arrays
 * @author Max Dunne   2013.01.04 */
ES_Event RunTimerService(ES_Event ThisEvent) {
    ES_Event ReturnEvent;
    
    ReturnEvent.EventType = ES_NO_EVENT; // assume no errors

    switch (ThisEvent.EventType) {
        case ES_INIT:
            break;
        case ES_TIMEOUT:
        case ES_TIMERACTIVE:
        case ES_TIMERSTOPPED:
            UserTimerStates[ThisEvent.EventParam] = ThisEvent.EventType;
            break;
        default:
            //ReturnEvent.EventType = ES_ERROR;
            break;
    }

#ifdef TIMER_SERVICE_TEST
    {
        uint8_t i;
        switch (timerServiceTestingState) {
            case init:
                if (ThisEvent.EventType == ES_INIT) {
                    printf("Timer Module INITED succesfully\r\n");
                    break;
                }
                printf("Timer %d had event %d at time %d\r\n", ThisEvent.EventParam, ThisEvent.EventType, ES_Timer_GetTime());
                if ((ThisEvent.EventParam == (TIMERS_USED - 1)) && (ThisEvent.EventType == ES_TIMERACTIVE)) {
                    timerServiceTestingState = expired;
                    printf("Testing timer user functions [expired][stopped][active]{state}\r\n");
                }
                break;

            case expired:
                for (i = 0; i < TIMERS_USED; i++) {
                    printf("(%d):[%d,%d,%d]{%d} ", i, IsTimerExpired(i), IsTimerStopped(i), IsTimerActive(i), GetUserTimerState(i));
                }
                printf("\r\n");
                if ((ThisEvent.EventParam == (TIMERS_USED - 1)) && (ThisEvent.EventType == ES_TIMEOUT)) {
                    timerServiceTestingState = runstop;
                    ES_Timer_InitTimer(0, 500);
                    for (i = 1; i < TIMERS_USED; i++) {
                        ES_Timer_SetTimer(i, 1000);
                        ES_Timer_StartTimer(i);
                    }
                }
                break;

            case runstop:
                printf("Timer %d had event %d at time %d\r\n", ThisEvent.EventParam, ThisEvent.EventType, ES_Timer_GetTime());
                if ((ThisEvent.EventParam == (0)) && (ThisEvent.EventType == ES_TIMEOUT)) {
                    for (i = 1; i < TIMERS_USED; i++) {
                        ES_Timer_StopTimer(i);
                    }
                }
                if ((ThisEvent.EventParam == (TIMERS_USED - 1)) && (ThisEvent.EventType == ES_TIMERSTOPPED)) {
                    printf("Testing of User Timer Functions is complete.\r\n");
                }

                break;
            default:
                ReturnEvent.EventType = ES_ERROR;
                break;
        }

    }
#endif
    //ES_Tail();
    return ReturnEvent;
}
示例#10
0
/****************************************************************************
 Function
    RunDisarmFSM

 Parameters
   ES_Event : the event to process

 Returns
   ES_Event, ES_NO_EVENT if no error ES_ERROR otherwise

 Description
   The state machine for the overall disarming process - state machine for
	 keypad, etc. are elsewhere
 Notes

****************************************************************************/
ES_Event RunDisarmFSM( ES_Event ThisEvent )
{
  ES_Event ReturnEvent;
  ReturnEvent.EventType = ES_NO_EVENT; // assume no errors
	
	// initial state: all the LEDs are off.
	static char LEDs[8] = {OFF, OFF, OFF, OFF, OFF, OFF , OFF, OFF}; 
	static bool tower_rotate_direction = true;
	
  switch ( CurrentState )
  {
    case Armed :
			switch ( ThisEvent.EventType ) {
				case ES_INIT :
					// intializing timer to be at 2mS rate (for 60 second count down)
					ES_Timer_Init(ES_Timer_RATE_2mS);	
				
					printf("Arming...\r\n");
					// sets the armed line to +5V
					setArmed(); 
					printf(" Setting all tower LEDS off...\r\n");
					LEDShiftRegInit();
					// all LEDs are off when armed
					setLED (LEDs); 
				
					printf(" Generating random passwords...\r\n");
				  // generate random password for keyboard input
					randomizePasswords();
					printArmedMessage();
				
					printf(" Lowering flag...\r\n");
				  // lower the flag when armed
					lowerFlag();
					printf(" Locking the keys\r\n");
					// lock the key when armed
					lockKeys();
		
					printf(" Setting tower to 0...\r\n\r\n");
					// the tower doesn't lean when armed
					setTowerToZero();
					printf("STATE: Armed\r\n\r\n");
					
					rotateTowerLeft();
					ES_Timer_InitTimer(PANIC_TIMER, 350);
					// demonstrate panic and start timing
					ES_Timer_StartTimer(PANIC_TIMER);

					break;
        
				case THREE_HANDS_ON :
					
					printf("EVENT: Three hands detected.\r\n");
				
					printf(" Setting Tower Tier 1 LED on...\r\n");
					LEDs[Tier1] = ON;
					// light up the LED for bottom layer to show success in task 1
					setLED(LEDs);
				
					printf(" Begin printing LCD passcode...\r\n");
					resetLCDmessage();
					printLCDmessage();
					// sends out a message every 2 seconds (2mS timer rate)
					ES_Timer_InitTimer(MESSAGE_TIMER, 1000);
					ES_Timer_StartTimer(MESSAGE_TIMER);

					printf(" Starting 60s disarm timer...\r\n");
					//the timer can only go up to 32000
					ES_Timer_InitTimer(DISARM_TIMER, 30000);
					ES_Timer_StartTimer(DISARM_TIMER);
					//begins to run the timing motor
					unwindTimingMotor(); 
				
					// grabs current time to store for rewinding the timer motor
					startTime = ES_Timer_GetTime();

					ES_Event ThisEvent;
					ThisEvent.EventType = PLAY_TRACK;
					// play sound track 01
					ThisEvent.EventParam = 1; 					
					PostAdafruitAudioService(ThisEvent);
					
					printf(" Transitioning to Stage 1...\r\n\r\n");
					CurrentState = Stage1;
					printf("STATE: Stage1\r\n\r\n");
					break;
	
				case ES_TIMEOUT :
					// if panic timer expires
					if (ThisEvent.EventParam == PANIC_TIMER) {
						if (tower_rotate_direction) {
							tower_rotate_direction = false;
							rotateTowerRight();
						} else {
							tower_rotate_direction = true;
							rotateTowerLeft();
						}
						ES_Timer_InitTimer(PANIC_TIMER, 350);
						ES_Timer_StartTimer(PANIC_TIMER);
					}
					// if timing motor rewind timer expires
					if (ThisEvent.EventParam == REWIND_TIMER) {
						// stop timing motor when the ball reaches the top
						stopTimingMotor();
					}

				default:
					;
			}
		break;

    case Stage1 :
      switch ( ThisEvent.EventType )
      {
				case ES_TIMEOUT :
					if (ThisEvent.EventParam == DISARM_TIMER) {
						// if the disarm timer expires
						printf("EVENT: Time has run out!\r\n");
						printTimeUp();
						// gets the time when the game ended
						endTime = ES_Timer_GetTime(); 
						// calculate how much rewinding needs to be done
						ES_Timer_InitTimer(REWIND_TIMER, (endTime-startTime)/2);
						ES_Timer_StartTimer(REWIND_TIMER);
						// begins rewinding the clock motor
						rewindTimingMotor(); 
						
						ThisEvent.EventType = ES_INIT;
						PostDisarmFSM(ThisEvent);
						// go back to armed state
						CurrentState = Armed;
					}
					// if vibration timer expires
					if (ThisEvent.EventParam == VIBRATION_TIMER) {
						printf(" Turning off vibration pulse...\r\n\r\n");
						vibrationMotorOff();
					}
					// if message timer expires
					if (ThisEvent.EventParam == MESSAGE_TIMER) {
						printf("EVENT: Printing out the next message...\r\n");
						printLCDmessage();
						ES_Timer_InitTimer(MESSAGE_TIMER, 1000);
						ES_Timer_StartTimer(MESSAGE_TIMER);
					}
        break;
					
				// one or more hands have been released
				case THREE_HANDS_OFF :
					printf("EVENT: One or more hands have been released.\r\n");
					printf(" Clearing the LCD screen\r\n");
					printArmedMessage();
					printf(" Setting Tower Tier 1 LED off...\r\n");
					// task 1 is not completed 
					LEDs[Tier1] = OFF;
					// turn off the LED for bottom layer
				  setLED(LEDs);
				
					printf(" Transitioning to Stage1_Stagnated...\r\n\r\n");
					CurrentState = Stage1_Stagnated;
					printf("STATE: Stage1_Stagnated\r\n\r\n");
        break;
				
				// if the user entered the correct password
				case CORRECT_PASSWORD_ENTERED :
					printf("EVENT: The correct password has been entered.\r\n");
					printf(" Unlocking the keys\r\n");
					// unlock the key and move to task 3
					unlockKeys();
					printAuthorizedMessage();
				
					printf(" Setting Tower Tier 2 LED on...\r\n");
					LEDs[Tier2] = ON;
					// turn on LED on the second tier to show success
				  setLED(LEDs);
				
					printf(" Playing audio: Wahoo!...\r\n");
				  ES_Event ThisEvent;
					ThisEvent.EventType = PLAY_TRACK;
					// play sound track 01
					ThisEvent.EventParam = 1; 					
					PostAdafruitAudioService(ThisEvent);	
					printf(" Transitioning to Stage2...\r\n\r\n");
					// set the current state to state 2
					CurrentState = Stage2;
					printf("STATE: Stage2\r\n\r\n");
          break;
				
				// if the user entered incorrect password
				case INCORRECT_PASSWORD_ENTERED :
					printf("EVENT: The incorrect password has been entered.\r\n");
					printIncorrectMessage();
					ES_Timer_InitTimer(MESSAGE_TIMER, 1000);
					ES_Timer_StartTimer(MESSAGE_TIMER);
					printf(" Generating vibration pulse...\r\n");
          break;

        default :
            ; 
      }
      break;
		
		case Stage1_Stagnated :
      switch ( ThisEvent.EventType )
      {
        case ES_TIMEOUT :
					// if disarm timer expires
          if (ThisEvent.EventParam == DISARM_TIMER) {
						printf("EVENT: Time has run out!\r\n");
						printTimeUp();
						endTime = ES_Timer_GetTime();
						// calculate how much rewinding needs to be done
						ES_Timer_InitTimer(REWIND_TIMER, (endTime-startTime)/2);
						ES_Timer_StartTimer(REWIND_TIMER);
						// begins rewinding the timing motor
						rewindTimingMotor(); 
						
						ThisEvent.EventType = ES_INIT;
						PostDisarmFSM(ThisEvent);
						// go back to armed state
						CurrentState = Armed;
					}
          break;
				
				// if all three tape sensors are covered
				case THREE_HANDS_ON :
					printf("EVENT: Three hands detected.\r\n");
				
					printf(" Setting Tower Tier 1 LED on...\r\n");
					LEDs[Tier1] = ON;
					// light up the LED for bottom layer to show success in task 1
					setLED(LEDs);
				
					printf(" Begin printing LCD passcode...\r\n");
					resetLCDmessage();
					printLCDmessage();
					// print message from LCD
					ES_Timer_InitTimer(MESSAGE_TIMER, 1000);
					ES_Timer_StartTimer(MESSAGE_TIMER);
				
					// play feedback audio wahoo
					ES_Event ThisEvent;
					ThisEvent.EventType = PLAY_TRACK;
					// play sound track 01
					ThisEvent.EventParam = 1; 					
					PostAdafruitAudioService(ThisEvent);

					printf(" Transitioning to Stage1...\r\n\r\n");
					// set current stage to stage 1
					CurrentState = Stage1;
					printf("STATE: Stage1\r\n\r\n");
          break;

        default :
            ; 
      }
      break;
			
		case Stage2 :
      switch ( ThisEvent.EventType )
      {
        case ES_TIMEOUT :
					// if the disarm timer expires
          if (ThisEvent.EventParam == DISARM_TIMER) {
						printf("EVENT: Time has run out!\r\n");
						printTimeUp();
						// gets the time when the game ended
						endTime = ES_Timer_GetTime();
						// calculate how much rewinding needs to be done
						ES_Timer_InitTimer(REWIND_TIMER, (endTime-startTime)/2);
						ES_Timer_StartTimer(REWIND_TIMER);
						// begins rewinding the clock moto
						rewindTimingMotor(); r
						
						ThisEvent.EventType = ES_INIT;
						PostDisarmFSM(ThisEvent);
						CurrentState = Armed;
          }
					break;
					
				// if the key is inserted
				case KEY_INSERTED:
					printf("EVENT: Key has been inserted.\r\n");
				
					printf(" Setting Tower Tier 3 LED on...\r\n");
					LEDs[Tier3] = ON;
					// light up LED on tier 3 to show successful completion of task 3
					setLED(LEDs);
				
					printf(" Setting Dial LED on...\r\n");
					LEDs[pot] = ON;
					setLED(LEDs);
				
					printf(" Playing audio: Wahoo!...\r\n");
					ES_Event ThisEvent;
					ThisEvent.EventType = PLAY_TRACK;
					// play sound track 01
					ThisEvent.EventParam = 1; 					
					PostAdafruitAudioService(ThisEvent);
				
					printf("Initializing the pot value...\r\n");
					setPotZero();
					printf(" Transitioning to Stage3...\r\n\r\n");
					// set current state to state 3
					CurrentState = Stage3;
					printf("STATE: Stage3\r\n\r\n");
          break;

        default :
            ; 
      }
      break;			

		case Stage3 :
      switch ( ThisEvent.EventType )
      {
        case ES_TIMEOUT :
					// if the disarm timer expires
					if (ThisEvent.EventParam == DISARM_TIMER) {  
						printf("EVENT: Time has run out!\r\n");
						printTimeUp();
						endTime = ES_Timer_GetTime();
						// calculate how much rewinding needs to be done
						ES_Timer_InitTimer(REWIND_TIMER, (endTime-startTime)/2);
						ES_Timer_StartTimer(REWIND_TIMER);
						// begins rewinding the clock motor
						rewindTimingMotor(); 
						
						ThisEvent.EventType = ES_INIT;
						PostDisarmFSM(ThisEvent);
						// set current state to armed
						CurrentState = Armed;
					}
					if (ThisEvent.EventParam == FAST_LEDS) {  
						printf("EVENT: \r\n");
						ThisEvent.EventType = CORRECT_VALUE_DIALED;
						PostDisarmFSM(ThisEvent);
					}
          break;
				
				// if the pot is dialed to correct value 
				case CORRECT_VALUE_DIALED :
					printf("EVENT: The correct pot value has been dialed.\r\n");
					// Sets the armed line to 0V
					setUnarmed(); 
					printf(" Setting Tower Tier 4-6 LED on with delay...\r\n");
					
					ES_Timer_InitTimer(FAST_LEDS, 150);
					static int i = Tier4;
					if (i<=Tier6){
						printf("\n\r looping for LED i + %d\n\r", i);
						// turn on all the remaining LEDs one by one
						LEDs[i] = 0;
						setLED(LEDs);
						ES_Timer_StartTimer(FAST_LEDS);
						i++;
						break;
					}
					printf(" Raising the flag...\r\n");
					// raise flag to show hope and joy
					raiseFlag();
					printf(" Playing audio: victory song...\r\n");
					ES_Event ThisEvent;
					ThisEvent.EventType = PLAY_TRACK;
					// play sound track 03
					ThisEvent.EventParam = 3; 					
					PostAdafruitAudioService(ThisEvent);	
					printf(" Starting 30s post-disarm timer...\r\n");
					ES_Timer_InitTimer(POST_DISARM_TIMER, 30000);
					printf(" Raising ball and feather...\r\n");
					// stop falling ball
					stopTimingMotor();
					// get the time when the disarment ends and rearm DDM
					endTime = ES_Timer_GetTime();
					ES_Timer_InitTimer(REWIND_TIMER, (endTime-startTime)/2);
					ES_Timer_StartTimer(REWIND_TIMER);
					// begins rewinding the clock motor
					rewindTimingMotor(); 
					
					printf(" Transitioning to Stage4...\r\n\r\n");
					// set current stage to stage 4
					CurrentState = Stage4;
					printf("STATE: Stage4\r\n\r\n");
          break;

        default :
            ; 
      }
      break;	

		case Stage4 :
      switch ( ThisEvent.EventType )
      {
        case ES_TIMEOUT :
					// if timer expires after successful disarment
					if (ThisEvent.EventParam == POST_DISARM_TIMER) {
						printf("EVENT: Post-disarm timer expired.\r\n");
						ThisEvent.EventType = ES_INIT;
						PostDisarmFSM(ThisEvent);
						// set current state to armed
						CurrentState = Armed;
					}
					// if rewind timer expires
					if (ThisEvent.EventParam == REWIND_TIMER) {
						// stop timing motor rewinding
						stopTimingMotor();
					}
          break;

        default :
            ; 
      }
      break;	

    default :
      ;
  }
  return ReturnEvent;
}
void PathPlanner(void)
{
if(QueryTankSM()!=DISABLED)
{

   static float CurrentPosition[3] = {0,0,0}; 
   static float CurrentWallLine[2];
   static int CurrentWallAngle;
   
   static ActivityState_t CurrentGoal;
   ES_Event ThinkTankEvent;
   ES_Event GameMonitorEvent;
   
   srand(ES_Timer_GetTime());
   
   if( CurrentGoal != QueryActivityControlSM())
   {
      //printf("clearing checkpoints\n\r");
     
	  wigglecounter = 0;
	  BinCheckPoints = 0;
	  WallCheckPoints = 0;
      
      
      // GOOD?
      ThinkTankEvent.EventType = WAIT_COMMAND;
      ThinkTankEvent.EventParam = WAIT_FOR_POSITION_FOUND;
      CurrentGoal = QueryActivityControlSM();
            
   } else
   {

   
   //ES_Event TankEvent;
   
   //CurrentGoal = QueryActivityControlSM();
   CurrentWallLine[0]= GetWallSlope(); 
   CurrentWallLine[1]= GetWallIntercept();
   CurrentWallAngle = GetWallAngle();
   UpdateCurrentPosition();
   CurrentPosition[X_COORDINATE] = GetX();
   CurrentPosition[Y_COORDINATE] = GetY();
   CurrentPosition[THETA] = GetTheta();
   //UpdateWallPoints(); // before getting staging points for wall
   
    switch (CurrentGoal)
   {
   //ES_Event ActionEvent;
      
      case RASTER :
      {
         static char i;
         
         //ThinkTankEvent.EventType = GO_COMMAND;
         //ThinkTankEvent.EventParam = 100;
         
          //Calibration test code
         /*
         if (i < 5)
         {
            ThinkTankEvent.EventType = GO_COMMAND;
            ThinkTankEvent.EventParam = STEP_DISTANCE; 
            i++;  
         } else if (i<6)
         {
            ThinkTankEvent.EventType = TURN_COMMAND;
            ThinkTankEvent.EventParam = 1080; 
            i++;  
         } else if (i<7)
         {
            ThinkTankEvent.EventType = TURN_COMMAND;
            ThinkTankEvent.EventParam = -1080; 
            i++;
         } else
         {
            ThinkTankEvent.EventType = WAIT_COMMAND;
            ThinkTankEvent.EventParam = WAIT_FOR_POSITION_FOUND; 
            i=0;
         }
         */ 
  
         
         // go forward thrice, turn randomly once, unless threatened by wall, then flee for safety.
         
         if (CurrentLocationIsSafe( CurrentPosition[0], CurrentPosition[1], CurrentPosition[2], CurrentWallAngle) == True)  //our current location is a safe one
         {
            
            if (i < 3)
            {
               if (SafeToTravel((float)STEP_DISTANCE/100.,CurrentPosition[0],CurrentPosition[1],CurrentPosition[2], CurrentWallAngle) == True)
               {
                  ThinkTankEvent.EventType = GO_COMMAND;
                  ThinkTankEvent.EventParam = STEP_DISTANCE;
                  
                  //SetMotorTimer(STEP_DISTANCE);
               } else
               {
                  ThinkTankEvent.EventType = TURN_COMMAND;
                  ThinkTankEvent.EventParam = (rand()%360) - 180; 
                  //SetMotorTimer(angle);
               }
               i++; 
            } else if (i<4)
            {
              ThinkTankEvent.EventType = WAIT_COMMAND;
              ThinkTankEvent.EventParam = WAIT_FOR_POSITION_FOUND;
              i++; 
            } else
            {
               ThinkTankEvent.EventType = TURN_COMMAND;
               ThinkTankEvent.EventParam = (rand()%360) - 180; 
               //SetMotorTimer(angle); 
               i = 0;    
            }
         } else
         {
            //go to safe point
            ThinkTankEvent = GenerateNextStep_EscapeWallPath(CurrentPosition,CurrentWallAngle);
         }
         
         
      }
      break;
      
      case CONTROL_LEFT_WALL :
         //printf("Wall Third Point is x: %d y: %d \n\r", PushPointX(),PushPointY());
         /*
         if(GetSeparationDistance( -- parameters  for push point -- ) < 6)
         {
            WallCheckPoints = 2;
         }
         */
         if (WallCheckPoints < 1)
         {
            float GoalPoint[2];
            //float NextGoalPoint[2];
            GoalPoint[0] = WallFirstPointX();
            GoalPoint[1] = WallFirstPointY();
            //NextGoalPoint[0] = WallStagingPointX(LEFT);
            //NextGoalPoint[1] = WallStagingPointY(LEFT);
            
            ThinkTankEvent = GenerateNextStep_GoalPoint(CurrentPosition,GoalPoint,WAIT_FOR_POSITION_FOUND);   


            
         } else if (WallCheckPoints < 2)
         {
            float GoalPoint[2];
            //float NextGoalPoint[2];
            GoalPoint[0] = WallStagingPointX(LEFT);
            GoalPoint[1] = WallStagingPointY(LEFT);
            //NextGoalPoint[0] = PushPointX();
            //NextGoalPoint[1] = PushPointY();
            
            ThinkTankEvent = GenerateNextStep_GoalPoint(CurrentPosition,GoalPoint,WAIT_FOR_POSITION_FOUND); 
         } else if  (WallCheckPoints < 3)
         {
            float GoalPoint[2];
            //float NextGoalPoint[2];
            GoalPoint[0] = PushPointX();
            GoalPoint[1] = PushPointY();
            // change this!!
            //NextGoalPoint[0] = PushPointX();
            //NextGoalPoint[1] = PushPointY();
            
            ThinkTankEvent = GenerateNextStep_GoalPoint(CurrentPosition,GoalPoint,WAIT_FOR_POSITION_FOUND);
         }  else
         {
            // THINK THROUGH THIS
            ThinkTankEvent.EventType = WAIT_COMMAND;
            ThinkTankEvent.EventParam = WAIT_FOR_A_TENTH_SECOND;
            //CurrentGoal = STAY_PUT (will start process over again)
            //WallCheckPointThree = False;
         }
         
      break;
      
      case CONTROL_RIGHT_WALL :
         //printf("Wall Third Point is x: %d y: %d \n\r", PushPointX(),PushPointY());
         /*
         if(GetSeparationDistance( -- parameters  for staging point -- ) < 8)
         {
            WallCheckPoints = 2;
         }
         */
         if (WallCheckPoints < 1)
         {
            float GoalPoint[2];
            //float NextGoalPoint[2];
            GoalPoint[0] = WallFirstPointX();
            GoalPoint[1] = WallFirstPointY();
            //NextGoalPoint[0] = WallStagingPointX(LEFT);
            //NextGoalPoint[1] = WallStagingPointY(LEFT);
            
            ThinkTankEvent = GenerateNextStep_GoalPoint(CurrentPosition,GoalPoint,WAIT_FOR_POSITION_FOUND);   


            
         } else if (WallCheckPoints < 2)
         {
            float GoalPoint[2];
            //float NextGoalPoint[2];
            GoalPoint[0] = WallStagingPointX(RIGHT);
            GoalPoint[1] = WallStagingPointY(RIGHT);
            //NextGoalPoint[0] = PushPointX();
            //NextGoalPoint[1] = PushPointY();
            
            ThinkTankEvent = GenerateNextStep_GoalPoint(CurrentPosition,GoalPoint,WAIT_FOR_POSITION_FOUND); 
         } else if  (WallCheckPoints < 3)
         {
            float GoalPoint[2];
            //float NextGoalPoint[2];
            GoalPoint[0] = PushPointX();
            GoalPoint[1] = PushPointY();
            // change this!!
            //NextGoalPoint[0] = PushPointX();
            //NextGoalPoint[1] = PushPointY();
            
            ThinkTankEvent = GenerateNextStep_GoalPoint(CurrentPosition,GoalPoint,WAIT_FOR_POSITION_FOUND);
         }  else
         {
            // THINK THROUGH THIS
            ThinkTankEvent.EventType = WAIT_COMMAND;
            ThinkTankEvent.EventParam = WAIT_FOR_A_TENTH_SECOND;
            //CurrentGoal = STAY_PUT (will start process over again)
            //WallCheckPointThree = False;
         }
         
      break;
      
      
      case DEPOSIT_IN_BIN_ONE :
         
         if (BinCheckPoints < 1)
         {
			ThinkTankEvent = GenerateNextStep_RadialPath(CurrentPosition,RADIUS_OF_SAFETY-2*HALF_BOT_LENGTH,WAIT_FOR_POSITION_FOUND);   
            
         } else if (BinCheckPoints < 2)
         {
            float GoalPoint[2];
            //float NextGoalPoint[2];
            GoalPoint[0] = StagingPointForBinOneX;
            GoalPoint[1] = StagingPointForBinOneY;
            //NextGoalPoint[0] = BinOneDepositX;
            //NextGoalPoint[1] = BinOneDepositY;
            GameMonitorEvent.EventType = START_AVERAGING;
			ThinkTankEvent = GenerateNextStep_GoalPoint(CurrentPosition,GoalPoint,WAIT_FOR_POSITION_FOUND); 
         } else if (BinCheckPoints < 3)
		 {
			ThinkTankEvent.EventType = WAIT_COMMAND;
			ThinkTankEvent.EventParam = WAIT_FOR_POSITION_FOUND; 
			BinCheckPoints++;
		} else if (BinCheckPoints < 4)
		 {
			ThinkTankEvent.EventType = WAIT_COMMAND;
			ThinkTankEvent.EventParam = WAIT_FOR_POSITION_FOUND; 
			BinCheckPoints++;
		} else if (BinCheckPoints < 5)
         {
            
			GameMonitorEvent.EventType = STOP_AVERAGING;
			
			ThinkTankEvent.EventType = WAIT_COMMAND;
			ThinkTankEvent.EventParam = WAIT_FOR_A_TENTH_SECOND;
			
			BinCheckPoints++;
		} else if (BinCheckPoints < 6) 
		{
			float GoalPoint[2];
            //float NextGoalPoint[2];
            GoalPoint[0] = DepositPointForBinOneX;
            GoalPoint[1] = DepositPointForBinOneY;
			//UpdateCurrentPosition();
			//CurrentPosition[X_COORDINATE] = GetX();
			//CurrentPosition[Y_COORDINATE] = GetY();
			//CurrentPosition[THETA] = GetTheta();
         ThinkTankEvent =  GenerateNextStep_GoalPoint(CurrentPosition,GoalPoint,WAIT_FOR_A_TENTH_SECOND);    /* checkpoint */     
         }  else
         {
            ThinkTankEvent.EventType = WAIT_COMMAND;
            ThinkTankEvent.EventParam = WAIT_FOR_A_TENTH_SECOND;
            Drive(GENTLE_FORWARD);
         }
      break;
      
      case DEPOSIT_IN_BIN_TWO :
         
         if (BinCheckPoints < 1)
         {
			ThinkTankEvent = GenerateNextStep_RadialPath(CurrentPosition,RADIUS_OF_SAFETY-2*HALF_BOT_LENGTH,WAIT_FOR_POSITION_FOUND);   
            
         } else if (BinCheckPoints < 2)
         {
            float GoalPoint[2];
            //float NextGoalPoint[2];
            GoalPoint[0] = StagingPointForBinTwoX;
            GoalPoint[1] = StagingPointForBinTwoY;
            //NextGoalPoint[0] = BinOneDepositX;
            //NextGoalPoint[1] = BinOneDepositY;
            GameMonitorEvent.EventType = START_AVERAGING;
			ThinkTankEvent = GenerateNextStep_GoalPoint(CurrentPosition,GoalPoint,WAIT_FOR_POSITION_FOUND); 
         } else if (BinCheckPoints < 3)
		 {
			ThinkTankEvent.EventType = WAIT_COMMAND;
			ThinkTankEvent.EventParam = WAIT_FOR_POSITION_FOUND; 
			BinCheckPoints++;
		} else if (BinCheckPoints < 4)
		 {
			ThinkTankEvent.EventType = WAIT_COMMAND;
			ThinkTankEvent.EventParam = WAIT_FOR_POSITION_FOUND; 
			BinCheckPoints++;
		} else if (BinCheckPoints < 5)
         {
            
			GameMonitorEvent.EventType = STOP_AVERAGING;
			
			ThinkTankEvent.EventType = WAIT_COMMAND;
			ThinkTankEvent.EventParam = WAIT_FOR_A_TENTH_SECOND;
			
			BinCheckPoints++;
		} else if (BinCheckPoints < 6) 
		{
			float GoalPoint[2];
            //float NextGoalPoint[2];
            GoalPoint[0] = DepositPointForBinTwoX;
            GoalPoint[1] = DepositPointForBinTwoY;
			//UpdateCurrentPosition();
			//CurrentPosition[X_COORDINATE] = GetX();
			//CurrentPosition[Y_COORDINATE] = GetY();
			//CurrentPosition[THETA] = GetTheta();
         ThinkTankEvent =  GenerateNextStep_GoalPoint(CurrentPosition,GoalPoint,WAIT_FOR_A_TENTH_SECOND);    /* checkpoint */     
         }  else
         {
            ThinkTankEvent.EventType = WAIT_COMMAND;
            ThinkTankEvent.EventParam = WAIT_FOR_A_TENTH_SECOND;
            Drive(GENTLE_FORWARD);
         }
      break;
      
      
      case DEPOSIT_IN_BIN_THREE :
         if (BinCheckPoints < 1)
         {
			ThinkTankEvent = GenerateNextStep_RadialPath(CurrentPosition,RADIUS_OF_SAFETY-2*HALF_BOT_LENGTH,WAIT_FOR_POSITION_FOUND);   
            
         } else if (BinCheckPoints < 2)
         {
            float GoalPoint[2];
            //float NextGoalPoint[2];
            GoalPoint[0] = StagingPointForBinThreeX;
            GoalPoint[1] = StagingPointForBinThreeY;
            //NextGoalPoint[0] = BinOneDepositX;
            //NextGoalPoint[1] = BinOneDepositY;
            GameMonitorEvent.EventType = START_AVERAGING;
			ThinkTankEvent = GenerateNextStep_GoalPoint(CurrentPosition,GoalPoint,WAIT_FOR_POSITION_FOUND); 
         } else if (BinCheckPoints < 3)
		 {
			ThinkTankEvent.EventType = WAIT_COMMAND;
			ThinkTankEvent.EventParam = WAIT_FOR_POSITION_FOUND; 
			BinCheckPoints++;
		} else if (BinCheckPoints < 4)
		 {
			ThinkTankEvent.EventType = WAIT_COMMAND;
			ThinkTankEvent.EventParam = WAIT_FOR_POSITION_FOUND; 
			BinCheckPoints++;
		} else if (BinCheckPoints < 5)
         {
            
			GameMonitorEvent.EventType = STOP_AVERAGING;
			
			ThinkTankEvent.EventType = WAIT_COMMAND;
			ThinkTankEvent.EventParam = WAIT_FOR_A_TENTH_SECOND;
			
			BinCheckPoints++;
		} else if (BinCheckPoints < 6)
		{
		   float GoalPoint[2];
         //float NextGoalPoint[2];
         GoalPoint[0] = DepositPointForBinThreeX;
         GoalPoint[1] = DepositPointForBinThreeY;

		   //UpdateCurrentPosition();
			//CurrentPosition[X_COORDINATE] = GetX();
			//CurrentPosition[Y_COORDINATE] = GetY();
			//CurrentPosition[THETA] = GetTheta();
			ThinkTankEvent =  GenerateNextStep_GoalPoint(CurrentPosition,GoalPoint,WAIT_FOR_A_TENTH_SECOND);    /* checkpoint */     
         }  else if (BinCheckPoints < 7)
         {
            
         
            ThinkTankEvent.EventType = WAIT_COMMAND;
            ThinkTankEvent.EventParam = WAIT_FOR_A_TENTH_SECOND;
            Drive(GENTLE_FORWARD);
            wigglecounter++;
            
            if (30 < wigglecounter)
            {
               ES_Event DummyEvent;
               DummyEvent.EventType = DEPOSIT_COMMAND;
               DummyEvent.EventParam = 1;
               ES_PostList00(DummyEvent);
               //printf("dpt tmr end - DUMP\n\r");
               
               
               BinCheckPoints++;
               wigglecounter = 0;
            }
            
         } else
         {
            static int done_wiggling;
            
            ThinkTankEvent.EventType = WAIT_COMMAND;
            ThinkTankEvent.EventParam = WAIT_FOR_A_WIGGLE_TIME;
            
            
               
            if(done_wiggling < 22)   
            {
               if (wigglecounter == 0)
               {
                  float param = 50;
                  Drive(0);
                  Rotate(param);//*GENTLE_FORWARD);  
                  puts("-"); 
                  ThinkTankEvent.EventParam = 3*WAIT_FOR_A_TENTH_SECOND;  // 4?
               } else if (wigglecounter == 1)
               {
                  float param = -75;
                  puts("+");
                  Drive(0);
                  Rotate(param);//GENTLE_FORWARD);      
               } else if(wigglecounter == 2) 
               {
                  float param = 50;
                  Drive(0);
                  Rotate(param);//*GENTLE_FORWARD);  
                  puts("-"); 
                  ThinkTankEvent.EventParam = 3*WAIT_FOR_A_TENTH_SECOND;
               } else if(wigglecounter == 3)
               {
                  float param = -75;
                  puts("+");
                  Drive(0);
                  Rotate(param);//GENTLE_FORWARD);
                  wigglecounter = 0;
               }
            
            } else if (done_wiggling == 6)
            {
              
               ES_Event DummyEvent;
                Drive(0);
               DummyEvent.EventType = DEPOSIT_COMPLETE;
               DummyEvent.EventParam = 1;
               //ES_PostList02(DummyEvent);   
            }
            
            wigglecounter++;
            done_wiggling++;
         }
      break;
      
      case DEPOSIT_IN_BIN_FOUR :
         if (BinCheckPoints < 1)
         {
			ThinkTankEvent = GenerateNextStep_RadialPath(CurrentPosition,RADIUS_OF_SAFETY-2*HALF_BOT_LENGTH,WAIT_FOR_POSITION_FOUND);   
            
         } else if (BinCheckPoints < 2)
         {
            float GoalPoint[2];
            //float NextGoalPoint[2];
            GoalPoint[0] = StagingPointForBinFourX;
            GoalPoint[1] = StagingPointForBinFourY;
            //NextGoalPoint[0] = BinOneDepositX;
            //NextGoalPoint[1] = BinOneDepositY;
            GameMonitorEvent.EventType = START_AVERAGING;
			ThinkTankEvent = GenerateNextStep_GoalPoint(CurrentPosition,GoalPoint,WAIT_FOR_POSITION_FOUND); 
         } else if (BinCheckPoints < 3)
		 {
			ThinkTankEvent.EventType = WAIT_COMMAND;
			ThinkTankEvent.EventParam = WAIT_FOR_POSITION_FOUND; 
			BinCheckPoints++;
		} else if (BinCheckPoints < 4)
		 {
			ThinkTankEvent.EventType = WAIT_COMMAND;
			ThinkTankEvent.EventParam = WAIT_FOR_POSITION_FOUND; 
			BinCheckPoints++;
		} else if (BinCheckPoints < 5)
         {
            
			GameMonitorEvent.EventType = STOP_AVERAGING;
			
			ThinkTankEvent.EventType = WAIT_COMMAND;
			ThinkTankEvent.EventParam = WAIT_FOR_A_TENTH_SECOND;
			
			BinCheckPoints++;
		} else if (BinCheckPoints < 6)
		{
		   float GoalPoint[2];
         //float NextGoalPoint[2];
         GoalPoint[0] = DepositPointForBinFourX;
         GoalPoint[1] = DepositPointForBinFourY;

		   //UpdateCurrentPosition();
			//CurrentPosition[X_COORDINATE] = GetX();
			//CurrentPosition[Y_COORDINATE] = GetY();
			//CurrentPosition[THETA] = GetTheta();
			ThinkTankEvent =  GenerateNextStep_GoalPoint(CurrentPosition,GoalPoint,WAIT_FOR_A_TENTH_SECOND);    /* checkpoint */     
         }  else if (BinCheckPoints < 7)
         {
            
         
            ThinkTankEvent.EventType = WAIT_COMMAND;
            ThinkTankEvent.EventParam = WAIT_FOR_A_TENTH_SECOND;
            Drive(GENTLE_FORWARD);
            wigglecounter++;
            
            if (30 < wigglecounter)
            {
               ES_Event DummyEvent;
               DummyEvent.EventType = DEPOSIT_COMMAND;
               DummyEvent.EventParam = 1;
               ES_PostList00(DummyEvent);
               //printf("dpt tmr end - DUMP\n\r");
               
               
               BinCheckPoints++;
               wigglecounter = 0;
            }
            
         } else
         {
            static int done_wiggling;
            
            ThinkTankEvent.EventType = WAIT_COMMAND;
            ThinkTankEvent.EventParam = WAIT_FOR_A_WIGGLE_TIME;
            
            
               
            if(done_wiggling < 22)   
            {
               if (wigglecounter == 0)
               {
                  float param = 50;
                  Drive(0);
                  Rotate(param);//*GENTLE_FORWARD);  
                  puts("-"); 
                  ThinkTankEvent.EventParam = 3*WAIT_FOR_A_TENTH_SECOND;  // 4?
               } else if (wigglecounter == 1)
               {
                  float param = -75;
                  puts("+");
                  Drive(0);
                  Rotate(param);//GENTLE_FORWARD);      
               } else if(wigglecounter == 2) 
               {
                  float param = 50;
                  Drive(0);
                  Rotate(param);//*GENTLE_FORWARD);  
                  puts("-"); 
                  ThinkTankEvent.EventParam = 3*WAIT_FOR_A_TENTH_SECOND;
               } else if(wigglecounter == 3)
               {
                  float param = -75;
                  puts("+");
                  Drive(0);
                  Rotate(param);//GENTLE_FORWARD);
                  wigglecounter = 0;
               }
            
            } else if (done_wiggling == 6)
            {
              
               ES_Event DummyEvent;
                Drive(0);
               DummyEvent.EventType = DEPOSIT_COMPLETE;
               DummyEvent.EventParam = 1;
               //ES_PostList02(DummyEvent);   
            }
            
            wigglecounter++;
            done_wiggling++;
         }
      break;
      
     
      
      
      
   }
  
  
   }
/*  
   switch(ThinkTankEvent.EventType)
{
   case(TURN_COMMAND)   :
   printf("Current Event is TURN_COMMAND\n\r");
   break;
   case(GO_COMMAND)  :
   printf("Current Event is GO_COMMAND\n\r");
   break;
   case(WAIT_COMMAND)  :
   printf("Current Event is WAIT_COMMAND\n\r");
   break;
   case(ARRIVED) :
   printf("Current Event is whimsical.\n\r");
   default:
   printf("epic fail\n\r");
   
   
}
*/

if(QueryTankSM()!=DISABLED)

   ES_PostList00(ThinkTankEvent);
   ES_PostList04(ThinkTankEvent);
   ES_PostList03(GameMonitorEvent);
   printf("We're at X = %f, Y= %f, and Theta = %f, Wall is at %d\n\r", CurrentPosition[0],CurrentPosition[1],CurrentPosition[2],CurrentWallAngle);

   //ES_PostList02(TankEvent);
   //printf("event param = %f",ThinkTankEvent.EventParam);
}
}
/**
 * @Function RunTemplateSubHSM(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 heirarchical state
 *        machine, as this is called any time a new event is passed to the event
 *        queue. 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.
 * @note Remember to rename to something appropriate.
 *       The lower level state machines are run first, to see if the event is dealt
 *       with there rather than at the current level. ES_EXIT and ES_ENTRY events are
 *       not consumed as these need to pass pack to the higher level state machine.
 * @author J. Edward Carryer, 2011.10.23 19:25
 * @author Gabriel H Elkaim, 2011.10.23 19:25 */
ES_Event RunAlignCollect_SubHSM(ES_Event ThisEvent) {
    uint8_t makeTransition = FALSE; // use to flag transition
    SubAlignCollect_SubHSMState_t nextState; // <- change type to correct enum

    ES_Tattle(); // trace call stack
    ES_Event Tcontrol;

    static int aligning_start_time, aligning_end_time;
    static int aligning_difference_time = 0;
    static int  back_tape_flag = 0;

    switch (CurrentState) {
        case InitPSubState: // If current state is initial Psedudo State
            if (ThisEvent.EventType == ES_INIT)// only respond to ES_Init
            {
                nextState = Buffer_State;
                makeTransition = TRUE;
                ThisEvent.EventType = ES_NO_EVENT;
            }
            break;

        case Buffer_State:
            if (ThisEvent.EventType != ES_NO_EVENT) { // An event is still active
                switch (ThisEvent.EventType) {

                    case ES_ENTRY:
                        //PivotRight(newspeed);
                        break;

                    case ES_EXIT:
                        break;

                    case FORCED:
                        nextState = Checking;
                        makeTransition = TRUE;
                        ThisEvent.EventType = ES_NO_EVENT;
                        //                        ThisEvent.EventParam = ThisEvent.EventParam;
                        break;


                    case ES_TIMEOUT:
                        //                        makeTransition = TRUE;
                        //                        ThisEvent.EventType = ES_NO_EVENT;
                        break;

                    default:
                        break;
                }
            }
            break;

        case Checking:
            if (ThisEvent.EventType != ES_NO_EVENT) { // An event is still active
                switch (ThisEvent.EventType) {

                    case ES_ENTRY:
                         PivotRight(speed*1.2);//(speed*.5);
                        //                        TurnRight(speed);
                        //                        StraightForward(speed);
                        //                        ES_Timer_InitTimer(replungetimer, 300);
                        break;

                    case ES_EXIT:
                        break;

                    case TAPE:
                        if ((ThisEvent.EventParam & 0x01) == 0x01) { // B
                            printf("\nback on\n");
                            aligning_start_time = ES_Timer_GetTime();
                            back_tape_flag = 1;
                            int i;
                            for(i = 0; i < 100000; i++)
                            {
                                asm("nop");
                            }
                            //                            nextState = Pivoting_Right_AC;
                            //                            makeTransition = TRUE;
                            ThisEvent.EventType = ES_NO_EVENT;
                        } else if (((ThisEvent.EventParam & 0b000001) == 0b000000) && back_tape_flag)   { // Back tape off T

                            aligning_end_time = ES_Timer_GetTime();
                            FullStop();
                            printf("\nback tape off\n");
                            int i;
                            for(i = 0; i < 100000; i++)
                            {
                                asm("nop");
                            }
                            aligning_difference_time = aligning_end_time - aligning_start_time;
//                            printf("\n\naligning_diff_time %d\n", aligning_difference_time);
                            nextState = Pivoting_Left_AC;
                            makeTransition = TRUE;
                            ThisEvent.EventType = ES_NO_EVENT;
                        }//                        if ((ThisEvent.EventParam & 0b110000) == 0b110000) { // LL, L: if miss most of the stem/land on T top
                            //                            nextState = Pivoting_Right_AC;
                            //                            makeTransition = TRUE;
                            //                            ThisEvent.EventType = ES_NO_EVENT;
                            //                        }
                            //                        else if ((ThisEvent.EventParam & 0b111100) == 0b111100) { // LL, L,F, R: if hit T stem head-on
                            //                            nextState = Turning_Right_AC;
                            //                            makeTransition = TRUE;
                            //                            ThisEvent.EventType = ES_NO_EVENT;
                            //                        } else if ((ThisEvent.EventParam & 0b100000) == 0b100000) { // LL entry
                            //                            nextState = Turning_Left_AC;
                            //                            makeTransition = TRUE;
                            //                            ThisEvent.EventType = ES_NO_EVENT;
                            //                        }
                        else {
                            ThisEvent.EventType = ES_NO_EVENT;
                        }
                        //                        else if ((ThisEvent.EventParam & T_Left) == T_Left) {
                        //                            nextState = Pivoting_Right;
                        //                            makeTransition = TRUE;
                        //                            ThisEvent.EventType = ES_NO_EVENT;
                        //                        } else if ((ThisEvent.EventParam & T_Right) == T_Right) {
                        //                            nextState = Pivoting_Left;
                        //                            makeTransition = TRUE;
                        //                            ThisEvent.EventType = ES_NO_EVENT;
                        //                        }
                        break;
                    case ES_TIMEOUT:
                        //                         nextState = Pivoting_Right_AC;
                        //                            makeTransition = TRUE;
                        //                            ThisEvent.EventType = ES_NO_EVENT;
                        //                        makeTransition = TRUE;
                        //                        ThisEvent.EventType = ES_NO_EVENT;
                        break;

                    default:
                        break;
                }
            }
            break;

        case Pivoting_Right_AC:
            if (ThisEvent.EventType != ES_NO_EVENT) {
                switch (ThisEvent.EventType) {
                    case ES_ENTRY:
                        PivotRight(tspeed * 1);
                        ES_Timer_InitTimer(replungetimer, 900);
                        break;

                        //                    case TAPE:
                        //                        if ((ThisEvent.EventParam & 0x01) == 0x01) { // B
                        //                            nextState = Reverse_Adjusting_Right;
                        //                            makeTransition = TRUE;
                        //                            ThisEvent.EventType = ES_NO_EVENT;
                        //                        }
                        //                        else {
                        //                            ThisEvent.EventType = ES_NO_EVENT;
                        //                        }
                        //                        break;


                    case ES_EXIT:
                        FullStop();
                        break;

                    case ES_TIMEOUT:
                        //                        nextState = Lost;
                        //                        makeTransition = TRUE;
                        //                        ThisEvent.EventType = ES_NO_EVENT;
                        if (ThisEvent.EventParam == replungetimer) {
                            nextState = Reverse_Adjusting_Right;
                            makeTransition = TRUE;
                            ThisEvent.EventType = ES_NO_EVENT;
                        }
                        break;

                    default:
                        break;
                }
            }
            break;

        case Pivoting_Left_AC:
            if (ThisEvent.EventType != ES_NO_EVENT) {
                switch (ThisEvent.EventType) {
                    case ES_ENTRY:
                        PivotLeft_WH(speed * .7);
                        int spintime = aligning_difference_time / 2;
                        //                        printf("\n\n%dcheck spin time\n", spintime);
                        ES_Timer_InitTimer(replungetimer, spintime);
                        break;

                    case ES_EXIT:
                        FullStop();
                        break;

                    case ES_TIMEOUT:
                        //                        nextState = Lost;
                        //                        makeTransition = TRUE;
                        //                        ThisEvent.EventType = ES_NO_EVENT;
                        if (ThisEvent.EventParam == replungetimer) {
                            FullStop();
                            //                            printf("\n\ncheck\n");
                            nextState = Reverse_Adjusting_Right;
                            makeTransition = TRUE;
                            ThisEvent.EventType = ES_NO_EVENT;
                        }
                        break;

                    default:
                        break;
                }
            }
            break;

        case Turning_Left_AC:
            //ThisEvent = Run_AA_LineFollowing_SubHSM(ThisEvent); // run sub-state machine for this state
            if (ThisEvent.EventType != ES_NO_EVENT) {
                switch (ThisEvent.EventType) {
                    case ES_ENTRY:
                        TurnLeft(speed);
                        break;

                    case TAPE:
                        if ((ThisEvent.EventParam & 0b000100) == 0b000100) { // R
                            nextState = Turning_Right_AC; //TODO: straight on
                            makeTransition = TRUE;
                            ThisEvent.EventType = ES_NO_EVENT;
                        } else {
                            ThisEvent.EventType = ES_NO_EVENT;
                        }
                        break;

                    case ES_EXIT:
                        FullStop();
                        break;

                    case ES_TIMEOUT:
                        //                        ThisEvent.EventType = ES_NO_EVENT;

                    default: break;
                }
            }
            break;

        case Turning_Right_AC:
            if (ThisEvent.EventType != ES_NO_EVENT) {
                switch (ThisEvent.EventType) {
                    case ES_ENTRY:
                        TurnRight(speed);
                        break;
                        //        case BUMPED:
                        //                nextState = Wall_Hugging;
                        //                makeTransition = TRUE;
                        //                ThisEvent.EventType = ES_NO_EVENT;
                        //                break;

                    case TAPE:
                        if ((ThisEvent.EventParam & 0x01) == 0x01) { // B
                            nextState = Reverse_Adjusting_Right;
                            makeTransition = TRUE;
                            ThisEvent.EventType = ES_NO_EVENT;
                        } else {
                            ThisEvent.EventType = ES_NO_EVENT;
                        }
                        break;


                    case ES_EXIT:
                        FullStop();
                        break;

                    case ES_TIMEOUT:
                        //                        nextState = Lost;
                        //                        makeTransition = TRUE;
                        //                        ThisEvent.EventType = ES_NO_EVENT;
                        break;

                    default: break;
                }
            }
            break;

        case Stopping_AC:
            FullStop();
            break;

        case Reversing_AC:
            if (ThisEvent.EventType != ES_NO_EVENT) {
                switch (ThisEvent.EventType) {
                    case ES_ENTRY:
                        StraightReverse(speed * 1.3);
                        ES_Timer_InitTimer(ammofindtimer, ammofindtimervalue);
                        break;

                    case ES_EXIT:
                        ES_Timer_StopTimer(ammofindtimer);
                        ES_Timer_StopTimer(portalsearchingtimer);
                        break;

                    case ES_TIMEOUT:
                        if (ThisEvent.EventParam == ammofindtimer) {
                            ES_Timer_InitTimer(portalsearchingtimer, 600);
                            StraightForward(speed);
                            ThisEvent.EventType = ES_NO_EVENT;
                        } else if (ThisEvent.EventParam == portalsearchingtimer) {
                            FullStop();
                            Tcontrol.EventType = AMMO;
                            Tcontrol.EventParam = 0;
                            ThisEvent.EventType = ES_NO_EVENT;
                            PostDagobotHSM(Tcontrol);
                        }

                        break;

                    default:
                        break;
                }
            }
            break;

        case Forwarding_AC:
            if (ThisEvent.EventType != ES_NO_EVENT) {
                switch (ThisEvent.EventType) {
                    case ES_ENTRY:
                        StraightForward(speed);
                        break;

                    case TAPE:
                        if ((ThisEvent.EventParam & 0b001000) == 0b000000) { // Front off the T
                            nextState = Reversing_AC;
                            makeTransition = TRUE;
                            ThisEvent.EventType = ES_NO_EVENT;
                        } else {
                            ThisEvent.EventType = ES_NO_EVENT;
                        }
                        break;

                    case ES_EXIT:
                        break;

                    case ES_TIMEOUT:
                        break;

                    default:
                        break;
                }
            }
            break;

        case Reverse_Adjusting_Right:
            if (ThisEvent.EventType != ES_NO_EVENT) {
                switch (ThisEvent.EventType) {
                    case ES_ENTRY:
                        //                        GentleLeft(-6 - .3); // backwards left = front of robot pivoting right
                        GentleRight(-6 - .3);
                        ES_Timer_InitTimer(replungetimer, replungetimervalue);
                        break;

                        //                    case TAPE:
                        //                        if ((ThisEvent.EventParam & 0b111110) == 0b000000) { // F
                        //                            nextState = Stopping_AC;
                        //                            makeTransition = TRUE;
                        //                            ThisEvent.EventType = ES_NO_EVENT;
                        //                        } else {
                        //                            ThisEvent.EventType = ES_NO_EVENT;
                        //                        }
                        //                        break;

                    case ES_EXIT:
                        ES_Timer_StopTimer(replungetimer);
                        break;

                    case ES_TIMEOUT:
                        if (ThisEvent.EventParam == replungetimer) {
                            nextState = Forwarding_AC;
                            makeTransition = TRUE;
                            ThisEvent.EventType = ES_NO_EVENT;
                        }
                        break;

                    default:
                        break;
                }
            }
            break;

        case Adjusting_Right_AC:
            if (ThisEvent.EventType != ES_NO_EVENT) { // An event is still active
                switch (ThisEvent.EventType) {
                    case ES_ENTRY:
                        GentleRight(speed);
                        break;

                    case TAPE:
                        if ((ThisEvent.EventParam & 0b011100) == 0b011100) { // L, C, R
                            nextState = Reverse_Adjusting_Right;
                            makeTransition = TRUE;
                            ThisEvent.EventType = ES_NO_EVENT;
                        } else {
                            ThisEvent.EventType = ES_NO_EVENT;
                        }
                        break;


                    case ES_EXIT:
                        break;
                }
            }
            break;

        case Reverse_Turning_Left:
            if (ThisEvent.EventType != ES_NO_EVENT) { // An event is still active
                switch (ThisEvent.EventType) {
                    case ES_ENTRY:
                        //ES_Timer_InitTimer(pivotingtimer, pivotingtimervalue);
                        ReverseLeft(speed); // fiz thinks this means reverse right // previously 5
                        ES_Timer_InitTimer(ammofindtimer, ammofindtimervalue);
                        break;

                        //                    case TAPE:
                        //                        if ((ThisEvent.EventParam & 0b011100) == 0b011100){ // L, C, R
                        //                            nextState = Reverse_Adjusting_Right;
                        //                            makeTransition = TRUE;
                        //                            ThisEvent.EventType = ES_NO_EVENT;
                        //                        }
                        //                        break;
                        //
                    case ES_TIMEOUT:
                        if (ThisEvent.EventParam == ammofindtimer) {
                            Tcontrol.EventType = MISSED_T;
                            Tcontrol.EventParam = 0;
                            PostDagobotHSM(Tcontrol);
                            ThisEvent.EventType = ES_NO_EVENT;
                        }
                        //                        else if (plungecount == 4){
                        //                            debug("Oh, shit!");
                        //                            nextState = Stopping_AC;
                        //                            makeTransition = TRUE;
                        //                            ThisEvent.EventType = ES_NO_EVENT;
                        //                            //TODO: Deal with later!
                        //                            // post event to pop into higher state
                        //                        }
                        break;

                    case ES_EXIT:
                        ES_Timer_StopTimer(ammofindtimer);
                        break;
                }
            }
            break;

        default: // all unhandled states fall into here
            break;
    } // end switch on Current State

    if (makeTransition == TRUE) {
        RunAlignCollect_SubHSM(EXIT_EVENT); // <- rename to your own Run function
        CurrentState = nextState;
        RunAlignCollect_SubHSM(ENTRY_EVENT); // <- rename to your own Run function
    }

    ES_Tail(); // trace call stack end
    return ThisEvent;
}
示例#13
0
uchar Check4Drum(void)
{
   static uchar timePerDrumCheck=50;
   long currTime=ES_Timer_GetTime();
   static uchar state =0; //0 is noiseDataCollection, 1 is Drumming
   static int numPts=0;
   static uchar lastRevState;
   static uchar currRevState;
   static uint drum[2];
   static uint hits[2];
   static int i;
   static uchar recording=0;
   static uchar hitInc=0;
   if(! (currTime-lastDrumTime > timePerDrumCheck || currTime<lastDrumTime) )
      return 0; //exit immediately if not enough time has passed
      
   drum[LEFT]=(int) ADS12_ReadADPin(drumLPin);
   drum[RIGHT]=(int) ADS12_ReadADPin(drumRPin); 
   
   if(state==0) 
   {
      if(!recording) 
      {
         if(numPts>=NUM_IN_A_ROW)
         {
            recording=1;
            numPts=0;
            bg[0]=0;
            bg[1]=0;
         } 
         
         if(absDiff(drum[LEFT],bg[LEFT]) > 10 || absDiff(drum[RIGHT],bg[RIGHT]) > 10 || bg[LEFT]<300 || bg[RIGHT] < 300)
         {
            printf("discard %i %i\r\n",bg[LEFT],bg[RIGHT]);
            bg[LEFT]=drum[LEFT]; 
            bg[RIGHT]=drum[RIGHT];
             
         }
         else
         {
            numPts++; //uncommented below
         }
      } 
      if(recording)   //recording
      {
         bg[0]+=drum[0];
         bg[1]+=drum[1];
         bgSaves[numPts]=drum[LEFT]; //11 to 30 ->0 to 19
         bgSaves[numPts+NUM_SAMPLES]=drum[RIGHT]; //11 to 30 -> 20 -> 39
         printf("pts: %lu, %lu\r\n", bg[0], bg[1]);
         numPts++;
      }
      //numPts++;
      
      
      //------------CalcBGNoise----------------------
      
      if(numPts>=NUM_SAMPLES)//actually 20 points here because we ignore first 10 (see above) (2.5 seconds total because thsi is 20Hz function)
      {
         bg[LEFT]/=numPts; //calculate background noise
         bg[RIGHT]/=numPts; 
         printf("bg is: %lu, %lu\r\n",bg[LEFT],bg[RIGHT]);
         state=1;
         timePerDrumCheck=5;
         hits[LEFT]=0;
         hits[RIGHT]=0;
         lastRevState=revDown();
         bgDone=1;
      }
   }
      //-----------CalcBGNoiseComplete--------------
   
      //-------ReadAnalogDrumHit-------------
   
      else if(state==1)
      {
         currRevState=revDown();
         for(i=0;i<2;i++) //Check if we have any good analog readings
         {
            if(currRevState!=lastRevState)
            {
               currHit[i]=0; //change direction, immediately set currHit to 0 so it wont trigger.
            }
            //if(absDiff(drum[i],bg[i]) > NOISE_THRESHOLD) //noise thresholding
            if(absDiff(drum[i],600) > NOISE_THRESHOLD) //noise thresholding HARDCODE
            {
              
              //printf("%i-%i\r\n",i,drum[i]);
              if(!hitInc)
              {
               hitInc=1;
               classifyStartTime=currTime; //starting classification
              }
              hits[i]++;
            }
         }
      //-------ReadAnalogDrumHitComplete-----
      
      //----Classify hit---    
         if( ((currTime>classifyStartTime + CLASSIFY_TIME)||classifyStartTime>currTime) && hitInc) //timer expired
         {
             hitInc=0; //end classify period
             if(hits[0]>=MIN_HITS_PER_CLASSIFY || hits[1] >= MIN_HITS_PER_CLASSIFY) 
             {    //begin classifying
                printf("class ify %i,%i",hits[LEFT],hits[RIGHT]);
                if(hits[LEFT] > hits[RIGHT]) //if there are more lefts than rights, good we know what this is.
                {
                   registerHit(LEFT,currTime);
                   if(hits[RIGHT] > hits[LEFT]/2) //probably hit both at once
                   {
                      registerHit(RIGHT,currTime);
                   }
                   
                } 
                else
                {
                   registerHit(RIGHT,currTime);
                   if(hits[LEFT] > hits[RIGHT]/2) //probably hit both at once
                   {
                      registerHit(LEFT,currTime);
                   } 
                }
             }
             
             hits[LEFT]=0;
             hits[RIGHT]=0;
         }
       //----Classify Hit END---
      }
   lastRevState=currRevState; 
   lastDrumTime=currTime;
   return 0;
}
示例#14
0
uchar getPhoneReq(void)
{
    static uchar ct=0;
    static long lastCtTime=0;
    static uchar firstDigit=0;
    long currTime=ES_Timer_GetTime();
    static uchar state=1;
    static uchar counting=0;
    uchar retVal=NUM_HZVS;
    static uchar lastPhoneState=1; //default is HI with pulldown
    uchar currPhoneState= phoneContact();
    
    if( !currPhoneState && lastPhoneState==1) //looking for falls
    {
       ct++; //count the rise (opens)
       lastCtTime=currTime;
       printf("PHONE TICK\r\n");
    }
    lastPhoneState= currPhoneState; //set lastPhoneState
    if(handsetContact()) //its 5
    {
      ct=0; //they put the handset down
      state=1; //reset state
      //printf("handset cont\r\n");
    }
    if((currTime-lastCtTime > TIME_AFTER_LAST_TICK || lastCtTime>currTime) && ct>0)
    {
      if(state==1)
      {
         if(!(ct==1 || ct==10)) //got a number greater than 1 on first shot, return it
         {
            retVal=ct;
         } else //got a 1 or 0, store it and move to next state
         {
            firstDigit=ct;
            state=2; //going to return numHZVs
         }
      } 
      else //state is 2nd digit
      {
         if(firstDigit==10)
            firstDigit=0; //10 counts is actually a zero.
         if(ct==10)
            ct=0;
         retVal=10*firstDigit+ct; //save boat number
         if(retVal>=NUM_HZVS) //bad boat num
         {
            //printf("bad boat number %i\r\n", (int) retVal);
            ct=0;
            state=1;
            return NUM_HZVS; //exit early
         } 
         else //good boat num
         {
             ct=0;
             state=1; //will return retVal
         }
      }
      ct=0;
    }
   /* if(retVal!=16)
      printf("retval%i\r\n",retVal);*/
    return retVal;
}
//#else
boolean Check4HopperStall(void)
{

    // local current variables for comparisons.
    int CurrentLeft = GetLeftCount();
    int CurrentRight = GetRightCount();
    boolean RightIsCurrentlyStalled=False;
    boolean LeftIsCurrentlyStalled=False;
    int CurrentTime=ES_Timer_GetTime();

    // if the left count has changed, note the time.
    if (CurrentLeft!=LastLeft)
    {
        LastTimeLeftChange = ES_Timer_GetTime();
        LastLeft = CurrentLeft;
    }
    // if the right count has changed, note the time.
    if (CurrentRight!=LastRight)
    {
        LastTimeRightChange = ES_Timer_GetTime();
        LastRight = CurrentRight;
    }

    // if the time since the last change in a moter is greater than
    // stall time, we'll say that the motor is in a stall state,
    // provided that we are actively trying to drive it with a non-zero
    // PWM value.

    if(((CurrentTime-LastTimeRightChange)>STALL_TIME)&&(GetPWMRightHopper()>10))
    {
        RightIsCurrentlyStalled = True;
    } else
    {
        RightIsCurrentlyStalled = False;
    }

    if (((CurrentTime-LastTimeLeftChange)>STALL_TIME)&&(GetPWMRightHopper()>10))
    {
        LeftIsCurrentlyStalled = True;
        //printf(".");
    } else
    {
        LeftIsCurrentlyStalled = False;
    }

    // if either stall state has changed
    if (((RightIsCurrentlyStalled!=RightIsStalled) || (LeftIsCurrentlyStalled!=LeftIsStalled))    )
        //&& ((QueryHopperDriveSM()==GOING_TO_HIGH_POSITION_NEITHERHIGH)||
        //(QueryHopperDriveSM()==GOING_TO_HIGH_POSITION_RIGHTHIGH)||
        //(QueryHopperDriveSM()==GOING_TO_LOW_POSITION_NEITHERLOW)))
    {
        //printf(".");

        // Post the appropriate event.
        if ((RightIsCurrentlyStalled==True)&&(LeftIsCurrentlyStalled==True))
        {
            ES_Event StallEvent;
            StallEvent.EventType = SCONTROL_UPDATE;
            StallEvent.EventParam = BOTH;
            ES_PostList00(StallEvent);
        } else if (RightIsCurrentlyStalled==True)
        {
            ES_Event StallEvent;
            StallEvent.EventType = SCONTROL_UPDATE;
            StallEvent.EventParam = RIGHT;
            ES_PostList00(StallEvent);
        } else if (LeftIsCurrentlyStalled==True)
        {
            ES_Event StallEvent;
            StallEvent.EventType = SCONTROL_UPDATE;
            StallEvent.EventParam = LEFT;
            ES_PostList00(StallEvent);
        } else
        {
            // Don't post a "no longer stalled" event.  I think just not posting has the same effect.
        }
        // Update Variables
        LeftIsStalled = LeftIsCurrentlyStalled;
        RightIsStalled = RightIsCurrentlyStalled;

        // Return true.
        return True;
    } else
    {
        // no change to report.
        return False;
    }

}
void InitializeCheckers( void)
{
    LastBallStatus = 0x00;

    LastBin1Status = 0x00;
    LastBin2Status = 0x00;
    LastBin3Status = 0x00;
    LastBin4Status = 0x00;

    LastWallAngleStatus = 0x00;
    LastTimeRightChange = ES_Timer_GetTime();
    LastTimeLeftChange = ES_Timer_GetTime();

    // Stepper/IR init
    LastHoneStatus_Bin1 = False;
    LastRStatus_Bin1 = False;
    LastLStatus_Bin1 = False;
    LastHoneStatus_Bin2 = False;
    LastRStatus_Bin2 = False;
    LastLStatus_Bin2 = False;
    LastHoneStatus_Bin3 = False;
    LastRStatus_Bin3 = False;
    LastLStatus_Bin3 = False;
    LastHoneStatus_Bin4 = False;
    LastRStatus_Bin4 = False;
    LastLStatus_Bin4 = False;
    LastAnyBeacons = False;
    LastTapeStatus = False;

    Beacon_Bin1_R = False;
    Beacon_Bin1_L = False;
    Beacon_Bin2_R = False;
    Beacon_Bin2_L = False;
    Beacon_Bin3_R = False;
    Beacon_Bin3_L = False;
    Beacon_Bin4_R = False;
    Beacon_Bin4_L = False;

    LastLeft = 0;
    LastRight = 0;

    LastXPosition = 0;
    LastYPosition = 0;
    LastBotOrientation = 0;

    LastLeft = GetLeftCount();
    LastRight = GetRightCount();
    LastTimeRightChange=ES_Timer_GetTime();
    LastTimeLeftChange=ES_Timer_GetTime();
    LastDelta=LastRight-LastLeft;
    RightIsStalled=False;
    LeftIsStalled=False;

    WallThreatState = False;


    LastButtonState = PTP&BIT0HI;
    TimeOfLastButtonSample = ES_Timer_GetTime();


}