diff --git a/devices/DiWheelDrive/linefollow.cpp b/devices/DiWheelDrive/linefollow.cpp index 8d7a0981379a93a42706b8910d7cdbc651ad2277..2d2a4ffb41b6377fab03defe86a698036adeaa99 100644 --- a/devices/DiWheelDrive/linefollow.cpp +++ b/devices/DiWheelDrive/linefollow.cpp @@ -1,5 +1,5 @@ // #include "global.hpp" -#include "linefollow.hpp" +#include "linefollow.hpp" #include <cmath> #include <limits> @@ -71,10 +71,11 @@ int LineFollow::getError(){ error = (FL -targetL + FR - targetR); break; case LineFollowStrategy::MIDDLE: - // Assume that the smallest value means driving in the middle - // targetL = targetR = !(targetL<targetR)?targetR:targetL; - error = (FL -targetL + FR - targetL); - break; + //TODO: Not working! + // Assume that the smallest value means driving in the middle + // targetL = targetR = !(targetL<targetR)?targetR:targetL; + error = (FL -targetL + FR - targetL); + break; case LineFollowStrategy::TRANSITION_L_R: case LineFollowStrategy::TRANSITION_R_L: error = transitionError(FL, FR, targetL, targetR); break; @@ -102,13 +103,13 @@ int LineFollow::getError(){ /** - * Depending on the strategy different behaviours will be triggered. - * FUZZY - standard tracking of black area + * Depending on the strategy different behaviours will be triggered. + * FUZZY - standard tracking of black area * REVERSE - drive back - * @param: rpmSpeed motor speed + * @param: rpmSpeed motor speed */ int LineFollow::followLine(int (&rpmSpeed)[2]){ - + int correctionSpeed = 0; switch (this->strategy) { @@ -125,7 +126,7 @@ int LineFollow::followLine(int (&rpmSpeed)[2]){ rpmSpeed[constants::DiWheelDrive::LEFT_WHEEL] = -1000000*global->forwardSpeed; rpmSpeed[constants::DiWheelDrive::RIGHT_WHEEL] = -1000000*global->forwardSpeed; - + break; default: @@ -158,7 +159,7 @@ int LineFollow::getPidCorrectionSpeed(){ // int correctionSpeed = (int) (global->K_p*error + Ki*accumHist - global->K_d*sloap); int32_t correctionSpeed = (int32_t) (K_p*error + K_i*accumHist + K_d*sloap); oldError = error; - + // Overflow Protection if ((error > 0) && (accumHist > std::numeric_limits<int32_t>::max() - error)){ // Overflow detected @@ -352,7 +353,7 @@ void LineFollow::defuzzyfication(colorMember (&member)[4], int (&rpmFuzzyCtrl)[2 // turn right copyRpmSpeed(rpmTurnRight, rpmFuzzyCtrl); } - // no sensor detects anything + // no sensor detects anything } else { // line is lost -> stop whiteFlag = 1; @@ -437,4 +438,4 @@ void LineFollow::copyRpmSpeed(const int (&source)[2], int (&target)[2]) { target[constants::DiWheelDrive::LEFT_WHEEL] = source[constants::DiWheelDrive::LEFT_WHEEL]; target[constants::DiWheelDrive::RIGHT_WHEEL] = source[constants::DiWheelDrive::RIGHT_WHEEL]; // chprintf((BaseSequentialStream*) &SD1, "Speed left: %d, Speed right: %d\r\n", target[0], target[1]); -} \ No newline at end of file +}