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