void RedAuto1(void) { int Launch = 0; Transmission(1); BallCounter = 4; ShootingMode = 1; IntakeShoot(6000);//SHOOT 4 PRELOADS // * FAILSAFE POINT ShootingMode = 4; stage = 1; TurnDegree(-10,40,0); // DO WE NEED FAILSAFE HERE? delay(200); Move(2,127); delay(500); clearTimer(T3); while(SensorValue[IR1] == 1) //*FAILSAFE ?WHAT IF WE DONT SEE ANY BALLS { IntakeOnAuto(); Move(2,20); if(time1[T3] > 8000) { FailSafeEngaged = 1; break; } } if(FailSafeEngaged == 1)Red1Fail1();/////////////////////RED1FAIL1 IntakePower(0); Break(100,10); while(MoveDist(-1, 20,0) == 1) // DO WE NEED FAILSAFE HERE? { IntakeOnAuto(); } while(MoveDist(40, 50,0) == 1) //*FAILSAFE ?WHAT IF WE ARE BLOCKED (ROBOT FINISHED ABOUT MID COURT) { if(FailSafeEngaged == 1)Red1Fail1();/////////////////////RED1FAIL1 IntakeOnAuto(); } IntakePower(0); TurnDegree(-45,127,2000); //*FAILSAFE ?WHAT IF WE ARE BLOCKED if(FailSafeEngaged == 1)Red1FailInfinite(); delay(200); SetRPM(2250);//LAUNCHER ON (NEXT SHOOTING RPM) clearTimer(T3); while(LineStatus() == 0) //*FAILSAFE AT WHAT POINT DO WE STOP MOVING? WHAT IF WE ARE OF COURSE? { Move(-2,30); if(time1[T3] > 3500) { FailSafeEngaged = 1; break; } } if(FailSafeEngaged == 1)Red1FailInfinite(); Break(100,-20); delay(200); while(MoveDist(7, 30,0) == 1){} // DO WE NEED FAILSAFE HERE? clearTimer(T3); while(LineStatus() == 0) // TURN UNTILL IT SEES LINE (WHAT IF WE NEVER SEE LINE) { Move(1,22); if(time1[T3] > 3000) { FailSafeEngaged = 1; break; } } if(FailSafeEngaged == 1)Red1FailInfinite(); while(LineStatus() == 1 && LineStatusOffset() == 0) //*FAILSE WHAT IF WE GET OFF THE LINE ( WHAT IF ROBOT BLOCKS PATH AND WE ARE STILL ON THE LINE) { FollowLine(40); if(LineStatus() == 1 && LineStatusOffset() == 1) Launch = 1;//WE REACHED THE CROSS(MIDDLE OF FIELD SET FLAG } if(LineStatus() == 1 && LineStatusOffset() == 1) Launch = 1; Break(100,20); StopDrive(); delay(300); if(Launch) //IF LAUNCH FLAG WAS SATISFIED EARLIER THEN WE CAN LAUNCH BALLS { IntakePower(127); // SHOOT MID COURT delay(3000); } stage = 1; ShootingMode = 3; DistanceCalculator(1); while(SensorValue[IR1] == 1 && LineStatus() == 1 && DistanceCalculator(0) < 55) //*FAILSE WHAT IF WE GET OFF THE LINE ( WHAT IF ROBOT BLOCKS PATH AND WE ARE STILL ON THE LINE) { FollowLine(35); IntakeOnAuto(); } Break(100,20); StopDrive(); while(stage == 1) // WHAT IF TWO BALLS ARE STUCK FOREVER? TIMER? { IntakeOnAuto(); } while(LineStatus() == 1 && DistanceCalculator(0) < 55)//*FAILSE WHAT IF WE GET OFF THE LINE ( WHAT IF ROBOT BLOCKS PATH AND WE ARE STILL ON THE LINE) { FollowLine(35); IntakeOnAuto(); } MoveTime(300,127); StopDrive(); IntakePower(127);///SHOT LAST BALLS BAR SHOTS delay(4000); stage = 1; /////////// KillALL(); while(1); }
/*----------------------------------------------------------------------------*/ void BlueAuto2(void) { ///4 PRELOADS 3 PILE /// STARTING TYLE IS FURTHERST TYLE int YouAreGoodToCount = 0; Transmission(1); BallCounter = 4; ShootingMode = 1; IntakeShoot(6000);//SHOOT 4 PRELOADS // * FAILSAFE POINT ShootingMode = 4; DistanceCalculatorTurn(1); while(DistanceCalculatorTurn(0) < 15) //*FAILSE WHAT IF WE GET OFF THE LINE ( WHAT IF ROBOT BLOCKS PATH AND WE ARE STILL ON THE LINE) { Move(-1, 50); } Break2(100,-10); delay(200); stage = 1; SetRPM(2200); DistanceCalculator(1); while(DistanceCalculator(0) < 12) //*FAILSE WHAT IF WE GET OFF THE LINE ( WHAT IF ROBOT BLOCKS PATH AND WE ARE STILL ON THE LINE) { Move(2, 127); } clearTimer(T3); while(SensorValue[IR1] == 1 ) //*FAILSE WHAT IF WE GET OFF THE LINE ( WHAT IF ROBOT BLOCKS PATH AND WE ARE STILL ON THE LINE) { IntakeOnAuto(); Move(2, 20); if(time1[T3] > 6000) { FailSafeEngaged = 1; break; } } if(FailSafeEngaged == 1)Blue1Fail1();/////////////////////RED1FAIL1 IntakePower(0); Break(100,10); while(MoveDist(-1, 20,0) == 1) // DO WE NEED FAILSAFE HERE? { IntakeOnAuto(); } while(MoveDist(35, 40,3000) == 1)//17 // DO WE NEED FAILSAFE HERE? { IntakeOnAuto(); if(FailSafeEngaged == 1)Blue1FailInfinite();/////////////////////RED1FAIL1 } IntakePower(0); while(LineStatus() == 0) { Move(-2, 100); } Break(100,-10); while(MoveDist(3, 30,0) == 1); delay(200); SetRPM(2200); while(LineStatus() == 0) //TURN UNTIL YOU SEE THE LINE { Move(1, 20); } DistanceCalculator(1); while(LineStatus() == 1) //*FAILSE WHAT IF WE GET OFF THE LINE ( WHAT IF ROBOT BLOCKS PATH AND WE ARE STILL ON THE LINE) { FollowLine(40); if(LineStatusOffset() == 1) { DistanceCalculator(1); YouAreGoodToCount = 1; } if(DistanceCalculator(0) > 18 && YouAreGoodToCount == 1) { DistanceCalculator(1); break; } } if(DistanceCalculator(0) < 50 && YouAreGoodToCount == 0)Blue1FailInfinite();//**FAIL GOOD Break(100,20); delay(500); IntakePower(127);///SHOT FIRST PILE delay(3000); ShootingMode = 3; StopDrive(); stage = 1; DistanceCalculator(1); while(SensorValue[IR1] == 1 && LineStatus() == 1) //*FAILSE WHAT IF WE GET OFF THE LINE ( WHAT IF ROBOT BLOCKS PATH AND WE ARE STILL ON THE LINE) { FollowLine(35); IntakeOnAuto(); } Break(100,20); StopDrive(); clearTimer(T3); while(stage == 1) // WHAT IF TWO BALLS ARE STUCK FOREVER? TIMER? { if(time1[T3] > 10000) { break; } IntakeOnAuto(); } clearTimer(T3); while(LineStatus() == 1 && DistanceCalculator(0) < 37 )//*FAILSE WHAT IF WE GET OFF THE LINE ( WHAT IF ROBOT BLOCKS PATH AND WE ARE STILL ON THE LINE) { FollowLine(35); IntakeOnAuto(); if(time1[T3] > 10000) { break; } } MoveTime(500,127); StopDrive(); delay(500); IntakePower(127);///SHOT BAR SHOTS delay(3500); IntakePower(0); SetRPM(2200); stage = 1; //////////////////////////////////////////////////////SHOT 8 COMPLETE DistanceCalculator(1); while(DistanceCalculator(0) < 50) //*FAILSE WHAT IF WE GET OFF THE LINE ( WHAT IF ROBOT BLOCKS PATH AND WE ARE STILL ON THE LINE) { Move(-2, 127); } Break(200,-15); delay(200); DistanceCalculatorTurn(1); clearTimer(T3); while(DistanceCalculatorTurn(0) < 62) //*FAILSE WHAT IF WE GET OFF THE LINE ( WHAT IF ROBOT BLOCKS PATH AND WE ARE STILL ON THE LINE) { Move(-1, 100); if(time1[T3] > 5000) { FailSafeEngaged = 1; break; } } if(FailSafeEngaged == 1)Blue1FailInfinite(); Break2(100,10); delay(300); DistanceCalculator(1); clearTimer(T3); while(DistanceCalculator(0) < 24) //*FAILSE WHAT IF WE GET OFF THE LINE ( WHAT IF ROBOT BLOCKS PATH AND WE ARE STILL ON THE LINE) { Move(2, 127); if(time1[T3] > 5000) { FailSafeEngaged = 1; break; } } if(FailSafeEngaged == 1)Blue1FailInfinite(); clearTimer(T3); while(SensorValue[IR1] == 1 )//PILE BY THE WALL { IntakeOnAuto(); Move(2, 20); if(time1[T3] > 5000) { FailSafeEngaged = 1; break; } } if(FailSafeEngaged == 1)Blue1FailInfinite(); IntakePower(0); Break(100,10); while(MoveDist(-1, 20,0) == 1) // DO WE NEED FAILSAFE HERE? { IntakeOnAuto(); } while(MoveDist(19, 35,2000) == 1) // DO WE NEED FAILSAFE HERE? { IntakeOnAuto(); } IntakePower(0); ///////////////////////////////////////////////////////////////////////////////////PICK UP 3RD SetRPM(2200); clearTimer(T3); while(LineStatus() == 0 ) { Move(-2, 127); if(time1[T3] > 5000) { FailSafeEngaged = 1; break; } } if(FailSafeEngaged == 1)Blue1FailInfinite(); //**FAIL GOOD Break(200,-15); while(MoveDist(3, 40,0) == 1); clearTimer(T3); while(LineStatus() == 0) // TURN UNTILL IT SEES LINE (WHAT IF WE NEVER SEE LINE) { Move(1,22); if(time1[T3] > 5000) { FailSafeEngaged = 1; break; } } if(FailSafeEngaged == 1)Blue1FailInfinite(); //**FAIL GOOD DistanceCalculator(1); clearTimer(T3); while(LineStatus() == 1 && DistanceCalculator(0) < 12)//*FAILSE WHAT IF WE GET OFF THE LINE ( WHAT IF ROBOT BLOCKS PATH AND WE ARE STILL ON THE LINE) { FollowLine(35); if(time1[T3] > 5000) { FailSafeEngaged = 1; break; } } if(FailSafeEngaged == 1)Blue1FailInfinite(); //**FAIL GOOD Break(100,20); StopDrive(); delay(500); IntakePower(127);///SHOT LAST BALLS BAR SHOTS delay(3000); TurnDegree(-45,100,2000); while(MoveDist(24, 40,0) == 1); ShootingMode = 4; KillALL(); while(1); }
void AntColonyOptimizationAlgorithm::doIteration() { VertexIterator it, it_end; boost::tie(it, it_end) = boost::vertices(g_); // list of vertices on the map VertexVector vertices(it, it_end); // index stands for ant id // VertexVector places the ant has already visited std::vector<VertexVector> travels; travels.reserve(vertices.size()); // at the very beggining suppose that ant has visited one city - // the one it is located at the moment std::for_each(vertices.begin(), vertices.end(), VertexAdder(travels)); std::vector<VertexVector>::iterator first(travels.begin()), last(travels.end()); for (; first != last; ++first) { //for every ant lets do travel VertexVector& tabu = *first; //we have not visited all the cities yet if (tabu.size() < vertices.size()) { // current city, the ant is located at const VertexDescriptor source = tabu.back(); VertexProbabilityMap probability_map; // investigate all the possible paths, which we have not visited yet // and find which is the best OutEdgeIterator begin, end; boost::tie(begin, end) = boost::out_edges(source, g_); for (; begin != end; ++begin) { const EdgeDescriptor edge = *begin; const VertexDescriptor target = boost::target(edge, g_); // check whether we have already visited the target if (tabu.end() == std::find(tabu.begin(), tabu.end(), target)) { //we have not visited it, so let's do calculation double pheromone_amount = pheromone_amount_map_[edge]; pheromone_amount = pow(pheromone_amount, alpha_); const double distance = edge_weight_map_[edge]; double desirability = 1 / distance; desirability = pow(desirability, beta_); probability_map.insert(std::make_pair(target, pheromone_amount * desirability)); } // we have checked whether we are through we that target } // fine, we have checked through all the pathes // now we need to choose the best according to probability values const double total_probability = std::for_each(probability_map.begin(), probability_map.end(), Accumulator(0.0)).value(); VertexProbabilityMap::const_iterator probability_it(probability_map.begin()), probability_it_end(probability_map.end()); VertexDescriptor target = probability_it->first; double probability = probability_it->second / total_probability; ++probability_it; for (; probability_it != probability_it_end; ++probability_it) { double current_probability = probability_it->second / total_probability; if (current_probability > probability) { target = probability_it->first; probability = current_probability; } } // target is the best vertex to go from source // lets go there tabu.push_back(target); } // we have done one step for one ant } // wow, we have visited all the cities by all the ants // and came to the initial beggining city // update the rate of pheromone on the edges // find total length every ant has gone std::vector<double> distances; distances.reserve(travels.size()); std::transform(travels.begin(), travels.end(), std::back_inserter(distances), DistanceCalculator(g_, edge_weight_map_)); // we have found total distance of every ant std::vector<double>::iterator min_it = std::min_element(distances.begin(), distances.end()); if (minimal_distance_ < 0 || minimal_distance_ > *min_it) { std::iterator_traits<std::vector<double>::iterator>::difference_type location = std::distance(distances.begin(), min_it); solution_ = travels[location]; minimal_distance_ = *min_it; } // find the left pheromone after each of ant PheromoneAmountMap left_pheromone_amount; const int Q = 100; typedef std::vector<VertexVector>::size_type size_type; for (size_type index = 0; index < travels.size(); ++index) { const double distance = distances[index]; const VertexVector& visited = travels[index]; VertexVector::const_iterator begin(visited.begin()), end(visited.end()); --end; EdgeDescriptor edge; bool found = false; for (; begin != end; ++begin) { boost::tie(edge, found) = boost::edge(*it, *boost::next(it), g_); left_pheromone_amount[edge] += (Q / distance); } boost::tie(edge, found) = boost::edge(vertices.back(), vertices.front(), g_); left_pheromone_amount[edge] += (Q / distance); } ++current_iteration_; //left pheromone values have been found std::for_each(pheromone_amount_map_.begin(), pheromone_amount_map_.end(), PheromoneAmountMapUpdater(rho_, left_pheromone_amount)); if (current_iteration_ >= iteration_number_) { is_finished_ = true; } report_data_.algorithmName = "Ant colony optimization"; report_data_.numIterations = current_iteration_; report_data_.optimalDistance = minimal_distance_; report_data_.result = solution_; }