void TpsTrajectoryEnsemble::moveTrajectory(int old_key, int new_key) { assert(_trajectory.find(old_key) != _trajectory.end()); TpsTrajectory& old_traj = getTrajectory(old_key); TpsTrajectory& new_traj = getTrajectory(new_key); new_traj.copy(old_traj); _trajectory[old_key]->clear(); delete _trajectory[old_key]; _trajectory.erase(old_key); }
/*** Private functions ***/ void RobotControl::hitBall(Trajectory aTrajectory) { Sleep(adaptTime(getTrajectory())); if (globalj1 == -90) { writeData("PRN 2,(0,0,0,0,0,30)\r"); } else if (globalj1 == 90) { writeData("PRN 2,(0,0,0,0,0,-30)\r"); } BOOST_LOG_TRIVIAL(info) << readData(); }
void TpsTrajectoryEnsemble::loadTrajectories(const char* filename) { std::ifstream is(filename); while (1) { int traj_num; is >> traj_num; if (is.fail()) { break; } TpsTrajectory& traj = getTrajectory(traj_num); traj.read(is); } is.close(); }
void RobotControl::run() { while (isRunning()) { if (hasTrajectory == true) { Time startTime = Clock::universal_time(); writeData(calculateAngles(getTrajectory())); BOOST_LOG_TRIVIAL(info) << readData(); std::cout << "Tijd tot de response: " << (Clock::universal_time() - startTime).total_milliseconds() << "ms" << std::endl << std::endl << std::endl << std::endl << std::endl << std::endl; //hitBall(getTrajectory()); hasTrajectory = false; } } }
void Clothoid::getPath(State a, State b) { cout<<"Start :" <<a.x<<" "<<a.y <<" "<<a.theta<<endl; cout<<"End"<<b.x<<" "<<b.y<<" "<<b.theta<<endl; if (fabs(a.x - b.x) < 1 || fabs(a.y - b.y) < 1) { solution = 0; return; } double beta = atan2((b.y - a.y), (b.x - a.x)); if (fabs(beta - b.theta - a.theta + beta) < 0.001 && fabs(b.theta - a.theta) < PI) { cout << "Symmetric:\n" << endl; start = a; end = b; double alpha = inRange((-start.theta + end.theta)) / 2; double D = calcD(fabs(alpha)); path.sigma = 4 * PI * signum(alpha) * D * D / start.getDistance(end)/start.getDistance(end); if (path.sigma == 0) { path.lengthOfPath = start.getDistance(end); for (double s = 0; s < path.lengthOfPath; s += path.lengthOfPath / 1000) { path.path.push_back(State(start.x + s * cos(start.theta), start.y + s * sin(start.theta), 0)); } paths.push_back(ClothoidPathSegment(path.path, path.lengthOfPath, path.sigma)); return; } else { path.lengthOfPath = 2 * sqrt(fabs(2 * alpha / path.sigma)); getTrajectory(); paths.push_back(ClothoidPathSegment(path.path, path.lengthOfPath, path.sigma)); std::cout << std::endl << std::endl << std::endl; } } else if (a.theta == b.theta) { State p((a.x + b.x) / 2, (a.y + b.y) / 2, 0); double beta = atan2((p.y - a.y), (p.x - a.x)); p.theta = 2 * beta - a.theta; getPath(a, p); getPath(p, b); } else { double alpha = inRange(((-a.theta + b.theta)) / 2); double cc; cc = cos(alpha) / sin(alpha); cout << "c is" << cc << endl; State p(0, 0, 0); p.x = (a.x + b.x + cc * (a.y - b.y)) / 2; p.y = (a.y + b.y + cc * (b.x - a.x)) / 2; double r = p.getDistance(a); double deflection1 = (atan2((p.y - a.y), (p.x - a.x))); double deflection2 = (atan2((p.y - b.y), (p.x - b.x))); double def; State c(0, 0, 0); if (deflection2 > deflection1) { //swap double temp = deflection2; deflection2 = deflection1; deflection1 = temp; c.x = a.x; c.y = a.y; c.theta = a.theta; a.x = b.x; a.y = b.y; a.theta = b.theta; b.x = c.x; b.y = c.y; b.theta = c.theta; } def = ((deflection2 + deflection1)) / 2; alpha = (((-a.theta + b.theta)) / 2); if (alpha < 0) { def = inRange(PI + def); } State q(0, 0, 0); q.x = p.x + r * cos(def); q.y = p.y + r * sin(def); double the = atan2((a.y - q.y), (a.x - q.x)); double the2 = atan2((q.y - b.y), (q.x - b.x)); double beta1 = inRange(2 * the - a.theta); double beta2 = inRange(2 * the2 - b.theta); q.theta = beta1; cout << "Intermediate Point " << q.x << " " << q.y << " " << q.theta << endl; getPath(a, q); getPath(q, b); } }