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