/****************************************************************************
 Function
    RunGridlinq

 Parameters
   ES_Event : the event to process

 Returns
   ES_Event, ES_NO_EVENT if no error ES_ERROR otherwise

 Description
   State machine for the Spectre, moves between unpaired and paired. 
    Handles the analysis of the incoming data as described by the 218 
    comm protocol
****************************************************************************/
ES_Event RunGridlinq ( ES_Event CurrentEvent ) {
    ES_Event ReturnEvent = {ES_NO_EVENT};
    GridlinqState NextState = CurrentState;
  
    // GridLinq state machine
    switch(CurrentState) {
        case Detached :
            //if we've lost the IR signal or whatever detects the cable
			if(CurrentEvent.EventType == CableTrigger) {
				//Prepare to enter the running state
				NextState = Running;
				ES_Event StartData = {Awaken, 0};
				ES_PostList00(StartData);
			}
			break;
		
		case Running :
			//do the running shit. Whatever that may be. 
			if(CurrentEvent.EventType == Disengage) {
                NextState = Detached;
            }
			if(CurrentEvent.EventType == WindReceived) {
				importData(Wind);
				printWindData();
			}
			else if(CurrentEvent.EventType == GPSReceived) {
				importData(GPS);
				printGPSData();
			}
			//once complete, set up hibernate
			if(CurrentEvent.EventType == GoToSleep) {
				NextState = Sleep;
				ES_Event StopData = {GoToSleep, 0};
				ES_PostList00(StopData);
			}
			break;
		
		case Sleep :
			// do the necessary sleeping shit. 
			//NextState = Running;
			if(CurrentEvent.EventType == CableTrigger) {
				//Prepare to enter the running state
				NextState = Running;
				ES_Event StartData = {Awaken, 0};
				ES_PostList00(StartData);
			}
			break;
	}		   
    CurrentState = NextState;
    return ReturnEvent;
}
boolean Check4HopperDelta(void)
{
    int CurrentDelta;
    int LeftCount = GetLeftCount();
    int RightCount = GetRightCount();



    CurrentDelta = RightCount + HOPPER_OFFSET - LeftCount ;

    // check for a change in delta, that delta is bigger than 1, and that the motors aren't
    // stalled (conflicting control law).
    if((CurrentDelta!=LastDelta)&&(abs(CurrentDelta)>1))//&&(RightIsStalled==False)&&(LeftIsStalled==False))
    {
        // Post Event.
        ES_Event ThisEvent;
        ThisEvent.EventType = VCONTROL_UPDATE;
        ThisEvent.EventParam = 1;
        ES_PostList00(ThisEvent);
        // Update LastDelta.
        LastDelta = CurrentDelta;
        // Return True.
        return True;
    } else
    {
        return False;
    }

}
boolean Check4Tape(void)
{
    boolean CurrentTapeStatus;
    boolean ReturnVal = False;

    CurrentTapeStatus =  PTU & TAPE_PIN;

    if ( (CurrentTapeStatus != LastTapeStatus) &&
            (CurrentTapeStatus != False) )
    {
        ES_Event ThisEvent;
        printf("Event checker: ZERO_FOUND\n\r");

        ThisEvent.EventType = ZERO_FOUND;
        ThisEvent.EventParam = 1;
        ES_PostList06(ThisEvent);
        // if we're in calibration state over in TankSM?
        ES_PostList00(ThisEvent);

        ReturnVal = True;
    }
    LastTapeStatus = CurrentTapeStatus;

    return ReturnVal;
}
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
}
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);
}
}
//#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;
    }

}
boolean KeyStrokeEvents(void)
{
    ES_Event ThisEvent;

    int KeyStroke;

    ThisEvent.EventType = ES_NO_EVENT;

    if ( kbhit() != 0)       // there was a key pressed
    {

        KeyStroke = getchar();
        switch ( toupper(KeyStroke))
        {
        case 'A' :
            ThisEvent.EventType = HONE_BIN1;
            IsIR_StepperEvent= True;
            break;
        case 'B' :
            ThisEvent.EventType = HONE_BIN2;
            IsIR_StepperEvent= True;
            break;
        case 'C' :
            ThisEvent.EventType = HONE_BIN3;
            IsIR_StepperEvent= True;
            break;
        case 'D' :
            ThisEvent.EventType = HONE_BIN4;
            IsIR_StepperEvent= True;
            break;
        case 'E' :
            ThisEvent.EventType = STOP_STEPPING;
            IsIR_StepperEvent= True;
            break;
        case 'F' :
            ThisEvent.EventType = DEPOSIT_TIME;
            IsThinkTankEvent = True;
            break;
        case 'G' :
            ThisEvent.EventType = FAVORITE_BIN_THREATENED;
            IsThinkTankEvent = True;
            break;
        case 'H' :
            ThisEvent.EventType = BIN_CHOSEN;
            ThisEvent.EventParam = 1;
            IsThinkTankEvent = True;
            break;
        case 'I' :
            ThisEvent.EventType = DEPOSIT_COMMAND;
            /*IsTankEvent = True;*/ break;
        case 'J' :
            ThisEvent.EventType = WALL_CHOSEN;
            ThisEvent.EventParam = LEFT;
            IsThinkTankEvent = True;
            break;
        case 'K' :
            ThisEvent.EventType = EPIC_PUSH_FAIL;
            IsThinkTankEvent = True;
            break;
        case 'L' :
            ThisEvent.EventType = GOAL_ANGLE_ACHIEVED;
            IsThinkTankEvent = True;
            break;
        case 'M' :
            ThisEvent.EventType = WALL_THREATENS_HOPPER;
            IsTankEvent = True;
            break;
        case 'N' :
            ThisEvent.EventType = WALL_NO_LONGER_THREATENS_HOPPER;
            IsTankEvent = True;
            break;
        case 'O' :
            ThisEvent.EventType = RIGHT_ZEROED;
            IsTankEvent = True;
            break;
        case 'P' :
            ThisEvent.EventType = LEFT_ZEROED;
            IsTankEvent = True;
            break;
        case 'Q' :
            ThisEvent.EventType = START_CALIBRATING_HOPPER;
            IsTankEvent = True;
            break;
        case 'R' :
            ThisEvent.EventType = ARRIVED;
            IsThinkTankEvent = True;
            break;
        case 'S' :
            ThisEvent.EventType = ROTATION_COMPLETE;
            IsThinkTankEvent = True;
            break;
        case 'T' :
            ThisEvent.EventType = GO_TO_LOW_COMMAND;
            break;
        case 'U' :
            ThisEvent.EventType = BALLS_IN_PLAY;
            break;
        case 'X' :
            ThisEvent.EventType = POSITION_FOUND;
            /*IsThinkTankEvent = True;*/ break;
        case 'Y' :
            ThisEvent.EventType = FIND_POSITION;
            IsIR_StepperEvent= True;
            break;
        case 'Z' :
            ThisEvent.EventType = ZERO_FOUND;
            break;
        }

        if (IsTankEvent)
        {
            ES_PostList00(ThisEvent);
            IsTankEvent = False;
        } else if (IsIR_StepperEvent)
        {
            ES_PostList06(ThisEvent);
            IsIR_StepperEvent = False;
        }
        else if (IsThinkTankEvent)
        {
            ES_PostList01(ThisEvent);
            IsThinkTankEvent = False;
        }
        else
        {
            ES_PostList02(ThisEvent);
            ES_PostList06(ThisEvent);
        }
        printf("posted\n\r");
        return(True);
    }

    return(False);
}