void __fastcall TfrmProximityForm::SetPosition(Position_t Position)
{
switch(Position)
{
case poUnknown:
break;
case poNear:
FNearCount = Min(FNearCount + 1, 2);
FFarCount = 0;
FSoFarCount = 0;
if ( (FNearCount >= 2) && (FCurrentPosition != poNear))
UpdateCurrentPosition(poNear);
break;
case poFar:
FNearCount = 0;
FFarCount = Min(FFarCount + 1, 2);
FSoFarCount = 0;
if ((FFarCount >= 2) && (FCurrentPosition != poFar))
UpdateCurrentPosition(poFar);
break;
case poSoFar:
FNearCount = 0;
FFarCount = 0;
FSoFarCount = Min(FSoFarCount + 1, 2);
if ((FSoFarCount >= 2) && (FCurrentPosition != poSoFar))
UpdateCurrentPosition(poSoFar);
break;
}
}
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);
}
}
