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