/ - Diff - AMiRo-OS - Research for Cognitive Interaction

Revision d314ad6f

devices/DiWheelDrive/linefollow.cpp
1	1	#include "global.hpp"
2		#include "linefollow2.hpp"
	2	#include "linefollow.hpp"
3	3	#include <cmath>
4	4
5	5

     namespace amiro {
       enum LineFollowStrategy{
       EDGE_LEFT,
       TRANSITION_L_R,
       TRANSITION_R_L,
       EDGE_RIGHT,
       EDGE_LEFT,      // driving on the left edge of a black line
       TRANSITION_L_R, // Transition from left to right edge
       TRANSITION_R_L, // transition from right to left edge
       EDGE_RIGHT,     // driving on the right edge of a black line
       REVERSE,
       MIDDLE,
       MIDDLE,         // not working, use FUZZY instead
       FUZZY,
       NONE
       };
-...
+    {
     public:
       // TODO: Documentation
       int biggestDiff = 0;
       Global *global;
       LineFollow(Global *global);
       LineFollow(Global *global, LineFollowStrategy strategy);
       /**
        * Entry method which should be called to follow a line.
+       *
        * @param rpmSpeed speed that will be determained
        * @return white flag, 1 if white is detected
        */
       int followLine(int (&rpmSpeed)[2]);
       /**
        * Setter for LineFollowStrategy which triggers transition behavior.
+       *
        * To prevent random turns while changeing the strategy a transition state is set.
        * In case the strategy needs to be switched immediately use promptStrategyChange().
+       *
        * @param strategy
        */
       void setStrategy(LineFollowStrategy strategy);
       void promptStrategyChange(LineFollowStrategy strategy);
       LineFollowStrategy getStrategy();
-...
       const int greyStartRising = blackStartFalling; // Where grey starts rising
       const int greyOff = whiteOn; // Where grey is completely off again
       private:
         int delta();
         int getError();
         int transitionError(int FL, int FR, int targetL, int targetR);
         int getPidCorrectionSpeed();
         void lineFollowing(int (&proximity)[4], int (&rpmFuzzyCtrl)[2]);
         // void defuzz(colorMember (&member)[4], int (&rpmFuzzyCtrl)[2]);
         Color memberToLed(colorMember member);
         void defuzzyfication(colorMember (&member)[4], int (&rpmFuzzyCtrl)[2]);
         colorMember getMember(float (&fuzzyValue)[3]);
         void fuzzyfication(int sensorValue, float (&fuzziedValue)[3]);
         void copyRpmSpeed(const int (&source)[2], int (&target)[2]);
         char whiteFlag = 0;
         LineFollowStrategy strategy = LineFollowStrategy::EDGE_RIGHT;
         float K_p = 0.002;
         float K_i = 0;
         float K_d = 0;
         int accumHist = 0;
         float oldError = 0;
         int trans = 0;
         int vcnl4020AmbientLight[4] = {0};
         int vcnl4020Proximity[4] = {0};
     private:
       /**
        * Calculate the error from front sensors.
        */
       int getError();
       /**
        * Error calculation while changing from one EDGE_* strategy to another.
        * This prevents the AMiRo from random turns while switching strategies.
+       *
        * @param FL value of left front sensor
        * @param FR value of right front sensor
        * @param targetL left threshold
        * @param targetR right threshold
        */
       int transitionError(int FL, int FR, int targetL, int targetR);
       /**
        * Use the error according to the strategy to calculate the correction speed.
        * Currently only the P part of the PID controller is used to calculate the
        * correction speed.
        */
       int getPidCorrectionSpeed();
       // Fuzzy line following methods--------------
       void lineFollowing(int (&proximity)[4], int (&rpmFuzzyCtrl)[2]);
       Color memberToLed(colorMember member);
       void defuzzyfication(colorMember (&member)[4], int (&rpmFuzzyCtrl)[2]);
       colorMember getMember(float (&fuzzyValue)[3]);
       void fuzzyfication(int sensorValue, float (&fuzziedValue)[3]);
       void copyRpmSpeed(const int (&source)[2], int (&target)[2]);
       int vcnl4020AmbientLight[4] = {0};
       int vcnl4020Proximity[4] = {0};
       // -----------------------------------------
       LineFollowStrategy strategy = LineFollowStrategy::EDGE_RIGHT;
       char whiteFlag = 0;
       int trans = 0;
       // PID controller components ---------------
       float K_p = 0.002;
       float K_i = 0;
       float K_d = 0;
       int accumHist = 0;
       float oldError = 0;
       // ----------------------------------------
     };

     #include "global.hpp"
     #include "linefollow2.hpp"
     #include <cmath>
     LineFollow::LineFollow(Global *global){
         this->global = global;
+    }
     LineFollow::LineFollow(Global *global, LineFollowStrategy strategy){
         this->global = global;
         this-> strategy = strategy;
+    }
     /**
      * Calculate the error from front proxi sensors and fixed threshold values for those sensors.
      */
     int LineFollow::getError(){
         // Get actual sensor data of both front sensors
         int FL = global->vcnl4020[constants::DiWheelDrive::PROX_FRONT_LEFT].getProximityScaledWoOffset();
         int FR = global->vcnl4020[constants::DiWheelDrive::PROX_FRONT_RIGHT].getProximityScaledWoOffset();
         int targetL = global->threshProxyL;
         int targetR = global->threshProxyR;
         int error = 0;
         switch (this->strategy)
+        {
         case LineFollowStrategy::EDGE_RIGHT: case LineFollowStrategy::DOCKING:
             error = -(FL -targetL + FR - targetR);
             break;
         case LineFollowStrategy::EDGE_LEFT:
             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 - targetR);
             break;
         default:
             break;
+        }
         // Debugging stuff ------
         if (global->enableRecord){
             global->senseRec[global->sensSamples].error = error;
             global->senseRec[global->sensSamples].FL = global->vcnl4020[constants::DiWheelDrive::PROX_FRONT_LEFT].getProximityScaledWoOffset();
             global->senseRec[global->sensSamples].FR = global->vcnl4020[constants::DiWheelDrive::PROX_FRONT_RIGHT].getProximityScaledWoOffset();
             global->sensSamples++;
+            }
         // ----------------------
         // Register white values
         if (FL+FR > global->threshWhite){
             whiteFlag = 1;
         }else{
             whiteFlag = 0;
+        }
         return error;
+    }
     int LineFollow::followLine(int (&rpmSpeed)[2]){
         int correctionSpeed = 0;
         switch (this->strategy)
+        {
         case LineFollowStrategy::FUZZY:
           for (int i = 0; i < 4; i++) {
               vcnl4020AmbientLight[i] = global->vcnl4020[i].getAmbientLight();
               vcnl4020Proximity[i] = global->vcnl4020[i].getProximityScaledWoOffset();
+          }
           lineFollowing(vcnl4020Proximity, rpmSpeed);
           break;
         case LineFollowStrategy::DOCKING:
           correctionSpeed = -getPidCorrectionSpeed();
           rpmSpeed[constants::DiWheelDrive::LEFT_WHEEL] = -global->forwardSpeed;
           rpmSpeed[constants::DiWheelDrive::RIGHT_WHEEL] = -global->forwardSpeed;
           break;
         default:
           correctionSpeed = getPidCorrectionSpeed();
           // chprintf((BaseSequentialStream*) &SD1, "Correction: %d, thresh: %d\n",correctionSpeed,  global->threshWhite);
           rpmSpeed[constants::DiWheelDrive::LEFT_WHEEL] =   global->forwardSpeed + correctionSpeed;
           rpmSpeed[constants::DiWheelDrive::RIGHT_WHEEL] = global->forwardSpeed - correctionSpeed;
           break;
+        }
         return whiteFlag;
+    }
     /**
      * Pid controller which returns a corrections speed.
      */
     int LineFollow::getPidCorrectionSpeed(){
         int error = getError();
         int sloap = error - oldError;
         int correctionSpeed = (int) (Kp*error + Ki*accumHist + Kd*sloap);
         oldError = error;
         // accumHist += (int) (0.01 * error);
         if (abs(error) > global->maxDist.error){
             global->maxDist.error = error;
+        }
         return correctionSpeed;
+    }
     void LineFollow::setStrategy(LineFollowStrategy strategy){
         this->strategy = strategy;
+    }
     LineFollowStrategy LineFollow::getStrategy(){
           return this->strategy;
+    }
     void LineFollow::setGains(float Kp, float Ki, float Kd){
         this->Kp = Kp;
         this->Ki = Ki;
         this->Kd = Kd;
+    }
     // Lagacy code, fuzzy following-----------------------------------------
     // Line following by a fuzzy controler
     void LineFollow::lineFollowing(int (&proximity)[4], int (&rpmFuzzyCtrl)[2]) {
       // FUZZYFICATION
       // First we need to get the fuzzy value for our 3 values {BLACK, GREY, WHITE}
       float leftWheelFuzzyMemberValues[3], leftFrontFuzzyMemberValues[3], rightFrontFuzzyMemberValues[3], rightWheelFuzzyMemberValues[3];
       fuzzyfication(proximity[constants::DiWheelDrive::PROX_WHEEL_LEFT], leftWheelFuzzyMemberValues);
       fuzzyfication(proximity[constants::DiWheelDrive::PROX_FRONT_LEFT], leftFrontFuzzyMemberValues);
       fuzzyfication(proximity[constants::DiWheelDrive::PROX_FRONT_RIGHT], rightFrontFuzzyMemberValues);
       fuzzyfication(proximity[constants::DiWheelDrive::PROX_WHEEL_RIGHT], rightWheelFuzzyMemberValues);
       // INFERENCE RULE DEFINITION
       // Get the member for each sensor
       colorMember member[4];
       member[constants::DiWheelDrive::PROX_WHEEL_LEFT] = getMember(leftWheelFuzzyMemberValues);
       member[constants::DiWheelDrive::PROX_FRONT_LEFT] = getMember(leftFrontFuzzyMemberValues);
       member[constants::DiWheelDrive::PROX_FRONT_RIGHT] = getMember(rightFrontFuzzyMemberValues);
       member[constants::DiWheelDrive::PROX_WHEEL_RIGHT] = getMember(rightWheelFuzzyMemberValues);
       // visualize sensors via LEDs
       global->robot.setLightColor(constants::LightRing::LED_WNW, memberToLed(member[constants::DiWheelDrive::PROX_WHEEL_LEFT]));
       global->robot.setLightColor(constants::LightRing::LED_NNW, memberToLed(member[constants::DiWheelDrive::PROX_FRONT_LEFT]));
       global->robot.setLightColor(constants::LightRing::LED_NNE, memberToLed(member[constants::DiWheelDrive::PROX_FRONT_RIGHT]));
       global->robot.setLightColor(constants::LightRing::LED_ENE, memberToLed(member[constants::DiWheelDrive::PROX_WHEEL_RIGHT]));
       // chprintf((BaseSequentialStream*) &SD1, "Left: BLACK: %f, GREY: %f, WHITE: %f\r\n", leftWheelFuzzyMemberValues[BLACK], leftWheelFuzzyMemberValues[GREY], leftWheelFuzzyMemberValues[WHITE]);
       // chprintf((BaseSequentialStream*) &SD1, "Right: BLACK: %f, GREY: %f, WHITE: %f\r\n", rightFuzzyMemberValues[BLACK], rightFuzzyMemberValues[GREY], rightFuzzyMemberValues[WHITE]);
       // DEFUZZYFICATION
       defuzzyfication(member, rpmFuzzyCtrl);
       // defuzz(member, rpmFuzzyCtrl);
+    }
     Color LineFollow::memberToLed(colorMember member) {
       switch (member) {
         case BLACK:
           return Color(Color::GREEN);
         case GREY:
           return Color(Color::YELLOW);
         case WHITE:
           return Color(Color::RED);
         default:
           return Color(Color::WHITE);
+      }
+    }
     void LineFollow::defuzzyfication(colorMember (&member)[4], int (&rpmFuzzyCtrl)[2]) {
         whiteFlag = 0;
       // all sensors are equal
       if (member[constants::DiWheelDrive::PROX_WHEEL_LEFT] == member[constants::DiWheelDrive::PROX_FRONT_LEFT] &&
           member[constants::DiWheelDrive::PROX_FRONT_LEFT] == member[constants::DiWheelDrive::PROX_FRONT_RIGHT] &&
           member[constants::DiWheelDrive::PROX_FRONT_RIGHT] == member[constants::DiWheelDrive::PROX_WHEEL_RIGHT]) {
         // something is wrong -> stop
         copyRpmSpeed(rpmHalt, rpmFuzzyCtrl);
       // both front sensor detect a line
       } else if (member[constants::DiWheelDrive::PROX_FRONT_LEFT] == BLACK &&
           member[constants::DiWheelDrive::PROX_FRONT_RIGHT] == BLACK) {
         // straight
         copyRpmSpeed(global->rpmForward, rpmFuzzyCtrl);
       // exact one front sensor detects a line
       } else if (member[constants::DiWheelDrive::PROX_FRONT_LEFT] == BLACK ||
                  member[constants::DiWheelDrive::PROX_FRONT_RIGHT] == BLACK) {
         // soft correction
         if (member[constants::DiWheelDrive::PROX_FRONT_LEFT] == GREY) {
           // soft right
           copyRpmSpeed(global->rpmSoftRight, rpmFuzzyCtrl);
         } else if (member[constants::DiWheelDrive::PROX_FRONT_LEFT] == WHITE) {
           // hard right
           copyRpmSpeed(global->rpmHardRight, rpmFuzzyCtrl);
         } else if (member[constants::DiWheelDrive::PROX_FRONT_RIGHT] == GREY) {
           // soft left
           copyRpmSpeed(global->rpmSoftLeft, rpmFuzzyCtrl);
         } else if (member[constants::DiWheelDrive::PROX_FRONT_RIGHT] == WHITE) {
           // hard left
           copyRpmSpeed(global->rpmHardLeft, rpmFuzzyCtrl);
+        }
       // both wheel sensors detect a line
       } else if (member[constants::DiWheelDrive::PROX_WHEEL_LEFT] == BLACK &&
                  member[constants::DiWheelDrive::PROX_WHEEL_RIGHT] == BLACK) {
         // something is wrong -> stop
         copyRpmSpeed(rpmHalt, rpmFuzzyCtrl);
       // exactly one wheel sensor detects a line
       } else if (member[constants::DiWheelDrive::PROX_WHEEL_LEFT] == BLACK ||
                  member[constants::DiWheelDrive::PROX_WHEEL_RIGHT] == BLACK) {
         if (member[constants::DiWheelDrive::PROX_WHEEL_LEFT] == BLACK) {
           // turn left
           copyRpmSpeed(rpmTurnLeft, rpmFuzzyCtrl);
         } else if (member[constants::DiWheelDrive::PROX_WHEEL_RIGHT] == BLACK) {
           // turn right
           copyRpmSpeed(rpmTurnRight, rpmFuzzyCtrl);
+        }
       // both front sensors may detect a line
       } else if (member[constants::DiWheelDrive::PROX_FRONT_LEFT] == GREY &&
                  member[constants::DiWheelDrive::PROX_FRONT_RIGHT] == GREY) {
         if (member[constants::DiWheelDrive::PROX_WHEEL_LEFT] == GREY) {
           // turn left
           copyRpmSpeed(rpmTurnLeft, rpmFuzzyCtrl);
         } else if (member[constants::DiWheelDrive::PROX_WHEEL_RIGHT] == GREY) {
           // turn right
           copyRpmSpeed(rpmTurnRight, rpmFuzzyCtrl);
+        }
       // exactly one front sensor may detect a line
       } else if (member[constants::DiWheelDrive::PROX_FRONT_LEFT] == GREY ||
                  member[constants::DiWheelDrive::PROX_FRONT_RIGHT] == GREY) {
         if (member[constants::DiWheelDrive::PROX_FRONT_LEFT] == GREY) {
           // turn left
           copyRpmSpeed(rpmTurnLeft, rpmFuzzyCtrl);
         } else if (member[constants::DiWheelDrive::PROX_FRONT_RIGHT] == GREY) {
           // turn right
           copyRpmSpeed(rpmTurnRight, rpmFuzzyCtrl);
+        }
       // both wheel sensors may detect a line
       } else if (member[constants::DiWheelDrive::PROX_WHEEL_LEFT] == GREY &&
                  member[constants::DiWheelDrive::PROX_WHEEL_RIGHT] == GREY) {
         // something is wrong -> stop
         copyRpmSpeed(rpmHalt, rpmFuzzyCtrl);
       // exactly one wheel sensor may detect a line
       } else if (member[constants::DiWheelDrive::PROX_WHEEL_LEFT] == GREY ||
                  member[constants::DiWheelDrive::PROX_WHEEL_RIGHT] == GREY) {
         if (member[constants::DiWheelDrive::PROX_WHEEL_LEFT] == GREY) {
           // turn left
           copyRpmSpeed(rpmTurnLeft, rpmFuzzyCtrl);
         } else if (member[constants::DiWheelDrive::PROX_WHEEL_RIGHT] == GREY) {
           // turn right
           copyRpmSpeed(rpmTurnRight, rpmFuzzyCtrl);
+        }
       // no sensor detects anything
       } else {
         // line is lost -> stop
         whiteFlag = 1;
         copyRpmSpeed(rpmHalt, rpmFuzzyCtrl);
+      }
         chprintf((BaseSequentialStream*) &SD1, "Fuzzy Speed: Left: %d, Right: %d\n", rpmFuzzyCtrl[0], rpmFuzzyCtrl[1]);
       return;
+    }
     colorMember LineFollow::getMember(float (&fuzzyValue)[3]) {
       colorMember member;
       if (fuzzyValue[BLACK] > fuzzyValue[GREY])
         if (fuzzyValue[BLACK] > fuzzyValue[WHITE])
           member = BLACK;
         else
           member = WHITE;
       else
         if (fuzzyValue[GREY] > fuzzyValue[WHITE])
           member = GREY;
         else
           member = WHITE;
       return member;
+    }
     // Fuzzyfication of the sensor values
     void LineFollow::fuzzyfication(int sensorValue, float (&fuzziedValue)[3]) {
       if (sensorValue < blackStartFalling ) {
         // Only black value
         fuzziedValue[BLACK] = 1.0f;
         fuzziedValue[GREY] = 0.0f;
         fuzziedValue[WHITE] = 0.0f;
       } else if (sensorValue > whiteOn ) {
         // Only white value
         fuzziedValue[BLACK] = 0.0f;
         fuzziedValue[GREY] = 0.0f;
         fuzziedValue[WHITE] = 1.0f;
       } else if ( sensorValue < greyMax) {
         // Some greyisch value between black and grey
         // Black is going down
         if ( sensorValue > blackOff) {
           fuzziedValue[BLACK] = 0.0f;
         } else {
           fuzziedValue[BLACK] = static_cast<float>(sensorValue-blackOff) / (blackStartFalling-blackOff);
+        }
         // Grey is going up
         if ( sensorValue < greyStartRising) {
           fuzziedValue[GREY] = 0.0f;
         } else {
           fuzziedValue[GREY] = static_cast<float>(sensorValue-greyStartRising) / (greyMax-greyStartRising);
+        }
         // White is absent
         fuzziedValue[WHITE] = 0.0f;
       } else if ( sensorValue >= greyMax) {
         // Some greyisch value between grey white
         // Black is absent
         fuzziedValue[BLACK] = 0.0f;
         // Grey is going down
         if ( sensorValue < greyOff) {
           fuzziedValue[GREY] = static_cast<float>(sensorValue-greyOff) / (greyMax-greyOff);
         } else {
           fuzziedValue[GREY] = 0.0f;
+        }
         // White is going up
         if ( sensorValue < whiteStartRising) {
           fuzziedValue[WHITE] = 0.0f;
         } else {
           fuzziedValue[WHITE] = static_cast<float>(sensorValue-whiteStartRising) / (whiteOn-whiteStartRising);
+        }
+      }
+    }
     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]);
+    }

     #ifndef AMIRO_LINEFOLLOWING_H
     #define AMIRO_LINEFOLLOWING_H
     #include <ch.hpp>
     #include "global.hpp"
     #include <amiroosconf.h>
     namespace amiro {
       enum class LineFollowStrategy{
       EDGE_LEFT,
       EDGE_RIGHT,
       DOCKING,
       MIDDLE,
       FUZZY,
       NONE
       };
     enum colorMember : uint8_t {
     	BLACK=0,
     	GREY=1,
     	WHITE=2
     };
     class LineFollow
+    {
     public:
       int biggestDiff = 0;
       Global *global;
       LineFollow(Global *global);
       LineFollow(Global *global, LineFollowStrategy strategy);
       // void calibrateZiegler(float KCrit, int rpmSpeed[2]);
       int followLine(int (&rpmSpeed)[2]);
       // int followLeftEdge(int rpmSpeed[2]);
       // int followRightEdge(int rpmSpeed[2]);
       // int followMiddle(int rpmSpeed[2]);
       void setStrategy(LineFollowStrategy strategy);
       LineFollowStrategy getStrategy();
       void setGains(float Kp, float Ki, float Kd);
       const int rpmTurnLeft[2] = {-10, 10};
       const int rpmTurnRight[2] = {rpmTurnLeft[1],rpmTurnLeft[0]};
       const int rpmHalt[2] = {0, 0};
       // Definition of the fuzzyfication function
       //  | Membership
       // 1|_B__   G    __W__
       //  |    \  /\  /
       //  |     \/  \/
       //  |_____/\__/\______ Sensor values
       // SEE MATLAB SCRIPT "fuzzyRule.m" for adjusting the values
       // All values are "raw sensor values"
       /* Use these values for white ground surface (e.g. paper) */
       const int blackStartFalling = 0x1000; // Where the black curve starts falling
       const int blackOff = 0x1800; // Where no more black is detected
       const int whiteStartRising = 0x2800; // Where the white curve starts rising
       const int whiteOn = 0x6000; // Where the white curve has reached the maximum value
       const int greyMax = (whiteOn + blackStartFalling) / 2; // Where grey has its maximum
       const int greyStartRising = blackStartFalling; // Where grey starts rising
       const int greyOff = whiteOn; // Where grey is completely off again
       private:
         int delta();
         int getError();
         int getPidCorrectionSpeed();
         void lineFollowing(int (&proximity)[4], int (&rpmFuzzyCtrl)[2]);
         // void defuzz(colorMember (&member)[4], int (&rpmFuzzyCtrl)[2]);
         Color memberToLed(colorMember member);
         void defuzzyfication(colorMember (&member)[4], int (&rpmFuzzyCtrl)[2]);
         colorMember getMember(float (&fuzzyValue)[3]);
         void fuzzyfication(int sensorValue, float (&fuzziedValue)[3]);
         void copyRpmSpeed(const int (&source)[2], int (&target)[2]);
         char whiteFlag = 0;
         LineFollowStrategy strategy = LineFollowStrategy::EDGE_RIGHT;
         float Kp = 0.002;
         float Ki = 0;
         float Kd = 0;
         int accumHist = 0;
         float oldError = 0;
         int vcnl4020AmbientLight[4] = {0};
         int vcnl4020Proximity[4] = {0};
     };
     } // end of namespace amiro
     #endif // AMIRO_LINEFOLLOWING_H

Also available in: Unified diff