// TODO this shouldn't be used for checks, should be called from one loop for all bots
// Executes the robot's movement towards the desiredLocation
// Returns
// 1 - success
// 0 - working
// -1 - failure
int Robot::execute() {
currentTime += (1.0/60.0);
// fprintf(stderr, "currentTime %f\n", currentTime);
//fprintf(stderr, "CurrentState: %f, %f, %f\ndesiredLocation: %f, %f, %f\n", CurrentState()[0], CurrentState()[1], CurrentState()[2], desiredLocation[0], desiredLocation[1], desiredLocation[2]);
//if(currentFrame == 0)
{
currentWaypointIndex = 0;
//Pass parameters in millimeters
PathPlanner planner = PathPlanner(playerID, isYellowTeam);
coeffecients = planner.FindPath(CurrentState(), desiredLocation);
//fprintf(stderr, "Size of coeffecients: %d\n", coeffecients.size());
}
currentFrame += 1;
//currentFrame = (currentFrame + 1) % 10;
Eigen::Vector3d desiredState;
desiredState[0] = coeffecients[currentWaypointIndex][0];
desiredState[1] = coeffecients[currentWaypointIndex][1];
desiredState[2] = coeffecients[currentWaypointIndex][2];
fprintf(stderr, "desiredState: %f, %f, %f\n", desiredState[0], desiredState[1], desiredState[2]);
// Computing velocities
_currentVelocity = controller.ComputeCommandVelo(CurrentState(), desiredState, _currentVelocity, Eigen::Vector3d(0,0,0));
vAngular = _currentVelocity[2];
vX = _currentVelocity[0];
vY = _currentVelocity[1];
vTangent = vX * cos(_currentPosition[2]) + vY * sin(_currentPosition[2]);
vTangent = (abs(vTangent)/vTangent) * min(abs(vTangent), _maxVelocity);
vTangent /= 1000.;
vNormal = min(- vX * sin(_currentPosition[2]) + vY * cos(_currentPosition[2]), _maxVelocity);
vNormal = (abs(vNormal)/vNormal) * min(abs(vNormal), _maxVelocity);
vNormal /= 1000.;
//fprintf(stderr, "transformed command: %f, %f, %f\n", vTangent, vNormal, vAngular);
//vAngular = min(abs(vAngular), _maxVelocity);
sendVelocityCommands();
double dist = Eigen::Vector2d((CurrentState() - desiredState)[0], (CurrentState() - desiredState)[1]).norm();
fprintf(stderr, "Distance to desired location: %f \n", dist);
//TODO units?
if(dist < 3) {
++currentWaypointIndex;
if(currentWaypointIndex > coeffecients.size() - 1) {
fprintf(stderr, "Executed move to location!\n");
return 1;
}
}
return 0;
}
void getInputSegments(){
segments.push_back(Line(20, 55, 60, 55));
segments.push_back(Line(20, 5, 20, 55)); //TODO: real input
segments.push_back(Line(60, 5, 20, 5));
segments.push_back(Line(60, 55, 60, 5));
//segments.push_back(Line(20, 5, 40, 63));
planner = PathPlanner(segments);
planner.populateTrajectory();
StateTrajectory* traj = planner.getPathTrajectory();
for (int i = 0; i < traj->size(); i++){
cout << traj->at(i).toString() << endl;
}
}
