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