diff --git a/src/LimitCalib.cpp b/src/LimitCalib.cpp index 15901206c..02b3fc46a 100644 --- a/src/LimitCalib.cpp +++ b/src/LimitCalib.cpp @@ -69,11 +69,9 @@ int main() { navtypes::Vectord<2> targetPosVec; std::array motorNames; - int i = 0; - for (const auto& runningMotor : encMotors) { - motorNames[i] = runningMotor.first; - targetPosVec[i] = runningMotor.second.stdPos; - i++; + for (int i = 0; i < encMotors.size(); i++) { + motorNames[i] = encMotors.at(i)->first; + targetPosVec[i] = encMotors.at(i)->second.stdPos; } // mess with putting the std Pos's in a vector to begin with @@ -86,18 +84,12 @@ int main() { JacobianVelController<2,2> controller([](Eigen::VectorXd x) { return x; }, {}); - bool done = false; - while(!done) { - navtypes::Vectord<2> targetVel = profile.getCommand(currTime); - double d; - navtypes::Vectord<2> targetPos = controller.getCommand(currTime, targetVel, d); - i = 0; - for (const auto& element : sortedKeys) { - robot::setMotorPos(element, targetPos[i]); - i++; - } - if (i == sortedKeys.size()) { - done = true; - } + navtypes::Vectord<2> targetVel = profile.getCommand(currTime); + double d; + navtypes::Vectord<2> targetPos = controller.getCommand(currTime, targetVel, d); + int i = 0; + for (const auto& element : sortedKeys) { + robot::setMotorPos(element, targetPos[i]); + i++; } }