void Enemy::moveBoss() { Vector aim = movePoint[movePointIdx]; float d = atan2(aim.x - pos.x, aim.y - pos.y); float od = d - deg; if (od > M_PI) od -= M_PI * 2; else if (od < -M_PI) od += M_PI * 2; float aod = fabs(od); if (aod < BOSS_MOVE_DEG) { deg = d; } else if (od > 0) { deg += BOSS_MOVE_DEG; if (deg >= M_PI * 2) deg -= M_PI * 2; } else { deg -= BOSS_MOVE_DEG; if (deg < 0) deg += M_PI * 2; } pos.x += sin(deg) * speed; pos.y += cos(deg) * speed; if (velCnt > 0) { velCnt--; pos.x += vel.x; pos.y += vel.y; vel.x *= 0.92; vel.y *= 0.92; } if (!onRoute) { if (aod < M_PI / 2) { onRoute = true; } } else { if (aod > M_PI / 2) { gotoNextPoint(); } } if (pos.x > fieldLimitX) { pos.x = fieldLimitX; gotoNextPoint(); } else if (pos.x < -fieldLimitX) { pos.x = -fieldLimitX; gotoNextPoint(); } if (pos.y > fieldLimitY) { pos.y = fieldLimitY; gotoNextPoint(); } else if (pos.y < fieldLimitY / 4) { pos.y = fieldLimitY / 4; gotoNextPoint(); } }
void SimpleNavigation::go() { //interface that will be used by the reactive nav to sense the environment and make the robot move //initial position mrpt::poses::CPose2D pose = CPoint2D(); float v = 0; float w = 0; interface->getCurrentPoseAndSpeeds(pose, v,w); double lat = pose.x(); double lon = pose.y(); // solves the tsp problem, in autonomous challenge we need to add some code here to make it work. TSPNavigation nav = TSPNavigation(lat,lon); nav.loadPoints(fileName, !inMeters); //waypoints in the order we want to visit them points = nav.solve(false); gotoNextPoint(); }
void SimpleNavigation::navigationStep() { mrpt::poses::CPose2D curPose; double yaw; double wantedYaw; float cur_w; float cur_v; interface->getCurrentPoseAndSpeeds(curPose, cur_v, cur_w); yaw = curPose.phi();//Robotpose returns yaw in radians LOG_AI(DEBUG4) << "Simple Nav Yaw: " << yaw << endl; wantedYaw = calcBearing(curPose);//AbstractNavigationInterface::calcBearing(curPose.x(), curPose.y(), navParams->target.x, navParams->target.y); LOG_AI(DEBUG4) << "Simple Nav Wanted Yaw: " << wantedYaw << endl; double dist = distance(curPose); double diffrence = wantedYaw - yaw; double minDiff = diffrence; if(diffrence > M_PI) { minDiff = 2*M_PI - diffrence; } else if(diffrence < -M_PI) { minDiff = 2*M_PI + diffrence; } LOG_AI(DEBUG4) << "Simple Nav Angular Difference: " << minDiff << endl; if(!dist > navParams->targetAllowedDistance) { LOG_AI(INFO) << "Reached Way Point" << endl; points.erase(points.begin()); if(points.size() != 0)//this->points) { gotoNextPoint(); } else { state = mrpt::reactivenav::CAbstractReactiveNavigationSystem::IDLE; } } if(abs((minDiff)) > .2) { bool turnRight = minDiff > 0; if(turnRight) { LOG_AI(DEBUG) << "\tTurning Right" << endl; interface->changeSpeeds(.5f,2); } else { LOG_AI(DEBUG) << "\tTurning Left" << endl; interface->changeSpeeds(.5f, -2); } } else { double dist = distance(curPose); LOG_AI(DEBUG) << "\tDriving Towards Target. Remaining Distance: " << dist << endl;//AbstractNavigationInterface::haversineDistance(curPose.x(), curPose.y(), navParams->target.x, navParams->target.y); if(dist > navParams->targetAllowedDistance) { interface->changeSpeeds(1.4,0); LOG_AI(DEBUG) << "\tDriving Towards Target. Remaining Distance: " << dist << endl; } else { LOG_AI(INFO) << "Reached Way Point" << endl; points.erase(points.begin()); if(points.size() != 0)//this->points) { gotoNextPoint(); } else { state = mrpt::reactivenav::CAbstractReactiveNavigationSystem::IDLE; } } } }