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