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

Revision 10bf9cc0

     #include "userthread.hpp"
     // #include "userthread.hpp"
     #include "global.hpp"
     #include <cmath>
     #include "linefollow.hpp"
     // #include <cmath>
     // #include "global.hpp"
     using namespace amiro;
     extern Global global;
-...
       lightAllLeds(Color::BLACK);
+    }
     void UserThread::chargeAsLED(){
       uint8_t numLeds = global.robot.getPowerStatus().state_of_charge / 12;
       Color color = Color::GREEN;
       if (numLeds <= 2){
         color = Color::RED;
       }else if(numLeds <= 6){
         color = Color::YELLOW;
+      }
       for (int i=0; i<numLeds; i++){
         lightOneLed(color, i);
         // this->sleep(300);
+      }
       // this->sleep(1000);
       // lightAllLeds(Color::BLACK);
+    }
     // ----------------------------------------------------------------
     void UserThread::getProxySectorVals(uint16_t (&proxVals)[8], uint16_t (&sProx)[8]){
       for (int i=0; i<8; i++){
         sProx[i] = (proxVals[i] < proxVals[(i+1) % 8]) ? proxVals[i] : proxVals[(i+1) % 8];
         // chprintf((BaseSequentialStream*)&global.sercanmux1, "%d: %d, ", i, sProx[i]);
+      }
         // chprintf((BaseSequentialStream*)&global.sercanmux1, "\n");
+    }
     void UserThread::getMaxFrontSectorVal(uint16_t (&sProx)[8], int32_t &sPMax){
       for (int i=2; i<5; i++){
         sPMax = (sPMax < sProx[i]) ? sProx[i] : sPMax;
+      }
+    }
     void UserThread::proxSectorSpeedCorrection(int (&rpmSpeed)[2], uint16_t (&proxVals)[8]){
       int i;
       uint16_t sProx[8];
       int32_t sPMax = 0;
       getProxySectorVals(proxVals, sProx);
       getMaxFrontSectorVal(sProx, sPMax);
       int32_t speedL = rpmSpeed[0] - (sPMax * pCtrl.pFactor);
       int32_t speedR = rpmSpeed[1] - (sPMax * pCtrl.pFactor);
       if(sPMax > pCtrl.threshMid){
           rpmSpeed[0] = 0;
           rpmSpeed[1] = 0;
           pCtrl.staticCont++;
       }else if((speedL > 0) || (speedR > 0)){
         pCtrl.staticCont = 0;
         rpmSpeed[0] = speedL;
         rpmSpeed[1] = speedR;
       }else{
         rpmSpeed[0] = 4000000 + (rpmSpeed[0] - global.rpmForward[0] * 1000000);
         rpmSpeed[1] = 4000000 + (rpmSpeed[1] - global.rpmForward[0] * 1000000);
+      }
       for(i=4; i<5; i++){
         if ((proxVals[i] > pCtrl.threshMid) && (proxVals[i+1] > pCtrl.threshLow)){
           rpmSpeed[0] = -5000000 ;
           rpmSpeed[1] = -5000000 ;
           // pCtrl.staticCont++;
           break;
+        }
+      }
       chargeAsLED();
       // chprintf((BaseSequentialStream*)&global.sercanmux1, "Max: %d factor: %d, Panel: %d SpeedL: %d SpeedR: %d ActualL: %d ActualR: %d\n",sPMax,  pCtrl.pFactor,  sPMax * pCtrl.pFactor, speedL, speedR, rpmSpeed[0], rpmSpeed[1]);
+    }
     // -------------------------------------------------------------------
     void UserThread::preventCollision( int (&rpmSpeed)[2], uint16_t (&proxVals)[8]) {
       if((proxVals[3] > pCtrl.threshLow) || (proxVals[4] > pCtrl.threshLow)){
           rpmSpeed[0] = rpmSpeed[0] / 2;
           rpmSpeed[1] = rpmSpeed[1] / 2;
+      }
       if((proxVals[3] > pCtrl.threshMid) || (proxVals[4] > pCtrl.threshMid)){
           rpmSpeed[0] = rpmSpeed[0] / 4;
           rpmSpeed[1] = rpmSpeed[1] / 4;
+      }
       if((proxVals[3] > pCtrl.threshHigh) || (proxVals[4] > pCtrl.threshHigh)){
           rpmSpeed[0] = 0;
           rpmSpeed[1] = 0;
           utCount.ringProxCount++;
       }else{
         utCount.ringProxCount = 0;
+      }
+    }
     /**
      * Blocks as long as the position changes.
      */
-...
           global.robot.setLightColor(led % 8, Color(Color::BLACK));
           led++;
+      }
       lightAllLeds(Color::BLACK);
+    }
     bool UserThread::checkFrontalObject(){
       uint32_t thresh = 1000;
       uint32_t thresh = pCtrl.threshMid;
       uint32_t prox;
       for(int i=0; i<8; i++){
         prox = global.robot.getProximityRingValue(i);
-...
       // setRpmSpeed(stop);
       checkForMotion();
       int success = 0;
       global.odometry.resetPosition();
       // global.odometry.resetPosition();
       types::position start = global.startPos = global.odometry.getPosition();
       global.motorcontrol.setMotorEnable(false);
       this->sleep(1000);
       types::position stop_ = global.endPos = global.odometry.getPosition();
       // Amiro moved, docking was not successful
       if ((start.x + stop_.x) || (start.y + stop_.y)){
       // if ((start.x + stop_.x)  || (start.y + stop_.y)){
       if (abs(start.x - stop_.x) > 0 /* || (start.y + stop_.y) */){
         lightAllLeds(Color::RED);
         // Enable Motor again if docking was not successful
         global.motorcontrol.setMotorEnable(true);
-...
        */
       // User thread state:
       //  int whiteBuf = 0;
       //  int proxyBuf = 0;
       //  int reverseBuf = 0;
        int correctionStep = 0;
       //  int dist_count = 0;
       //  bool needDistance = false;
        uint16_t rProx[8]; // buffer for ring proxy values
       int rpmSpeed[2] = {0};
       int stop[2] = {0};
       int turn[2] = {5,-5};
       LineFollowStrategy lStrategy = LineFollowStrategy::EDGE_RIGHT;
       for (uint8_t led = 0; led < 8; ++led) {
         global.robot.setLightColor(led, Color(Color::BLACK));
+      }
       running = false;
       LineFollowStrategy lStrategy = LineFollowStrategy::EDGE_RIGHT;
       LineFollow lf(&global);
       /*
        * LOOP
-...
         if (accel_z < -900 /*-0.9g*/) {
           // Start line following when AMiRo is rotated
           if(utState == states::IDLE){
           if(currentState == states::INACTIVE){
             newState = states::FOLLOW_LINE;
           }else{
             newState = states::IDLE;
-...
           global.msgReceived = false;
           // running = true;
           switch(global.lfStrategy){
             case msg_content::START:
               newState = states::FOLLOW_LINE;
               break;
             case msg_content::STOP:
             case msg_content::MSG_START:
               newState = states::CALIBRATION_CHECK;
             break;
             case msg_content::MSG_STOP:
               newState = states::IDLE;
               break;
             case msg_content::EDGE_RIGHT:
             break;
             case msg_content::MSG_EDGE_RIGHT:
               // newState = states::FOLLOW_LINE;
               lStrategy = LineFollowStrategy::EDGE_RIGHT;
               break;
             case  msg_content::EDGE_LEFT:
             break;
             case  msg_content::MSG_EDGE_LEFT:
               // newState = states::FOLLOW_LINE;
               lStrategy = LineFollowStrategy::EDGE_LEFT;
               break;
             case msg_content::FUZZY:
             break;
             case msg_content::MSG_FUZZY:
               // newState = states::FOLLOW_LINE;
               lStrategy = LineFollowStrategy::FUZZY;
               break;
             case msg_content::DOCK:
             break;
             case msg_content::MSG_DOCK:
               newState = states::DETECT_STATION;
               break;
             case msg_content::UNDOCK:
             break;
             case msg_content::MSG_UNDOCK:
               newState = states::RELEASE;
               break;
             case msg_content::CHARGE:
             break;
             case msg_content::MSG_CHARGE:
               newState = states::CHARGING;
               break;
             break;
             case msg_content::MSG_RESET_ODOMETRY:
               global.odometry.resetPosition();
             break;
             case msg_content::MSG_CALIBRATE_BLACK:
               proxCalib.calibrateBlack = true;
               // global.odometry.resetPosition();
               newState = states::CALIBRATION;
             break;
             case msg_content::MSG_CALIBRATE_WHITE:
               proxCalib.calibrateBlack = false;
               newState = states::CALIBRATION;
             break;
             default:
               newState = states::IDLE;
               break;
             break;
+          }
+        }
         // newState = utState;
         // newState = currentState;
         // Get sensor data
         // uint16_t WL = global.vcnl4020[constants::DiWheelDrive::PROX_WHEEL_LEFT].getProximityScaledWoOffset();
-...
+        }
         // int FL = global.vcnl4020[constants::DiWheelDrive::PROX_FRONT_LEFT].getProximityScaledWoOffset();
         // int FR = global.vcnl4020[constants::DiWheelDrive::PROX_FRONT_RIGHT].getProximityScaledWoOffset();
         switch(utState){
         switch(currentState){
           case states::INACTIVE:
             // Dummy state to deactivate every interaction
           break;
           // ---------------------------------------
           case states::CALIBRATION_CHECK:
             // global.robot.calibrate();
             if(global.linePID.BThresh >= global.linePID.WThresh){
               newState = states::CALIBRATION_ERROR;
             }else{
               newState = states::FOLLOW_LINE;
+            }
           break;
           // ---------------------------------------
           case states::CALIBRATION:
             /* Calibrate the global thresholds for black or white.
                 This values will be used by the line follow object
             */
             proxCalib.buf = 0;
             if(proxCalib.calibrateBlack){
               chprintf((BaseSequentialStream*)&global.sercanmux1, "Black Calibration, Place AMiRo on black Surface!\n");
               global.robot.calibrate();
+            }
             for(int i=0; i <= proxCalib.meanWindow; i++){
               proxCalib.buf += global.vcnl4020[constants::DiWheelDrive::PROX_FRONT_LEFT].getProximityScaledWoOffset()
                               + global.vcnl4020[constants::DiWheelDrive::PROX_FRONT_RIGHT].getProximityScaledWoOffset();
               this->sleep(CAN::UPDATE_PERIOD);
+            }
             proxCalib.buf = proxCalib.buf / (2*proxCalib.meanWindow);
             if(proxCalib.calibrateBlack){
               global.linePID.BThresh = proxCalib.buf;
             }else  {
               global.linePID.WThresh  = proxCalib.buf;
+            }
               chprintf((BaseSequentialStream*)&global.sercanmux1, "Black: %d, White: %d!\n", global.linePID.BThresh, global.linePID.WThresh);
             newState = states::IDLE;
           break;
           // ---------------------------------------
           case states::IDLE:
             global.motorcontrol.setMotorEnable(true);
             setRpmSpeed(stop);
             if(/* checkPinVoltage() && */ checkPinEnabled()){
               global.robot.requestCharging(0);
+            }
             // pCtrl.pFactor = 0;
             pCtrl.staticCont = 0;
             utCount.whiteCount = 0;
             utCount.proxyCount = 0;
             utCount.ringProxCount = 0;
             utCount.errorCount = 0;
             utCount.idleCount++;
             if (utCount.idleCount > 10){
               utCount.idleCount = 0;
             // chprintf((BaseSequentialStream*)&global.sercanmux1, "Transmit state %d\n", newState);
             global.robot.transmitState(utState);
+          }
             break;
             newState = states::INACTIVE;
           break;
           // ---------------------------------------
           case states::FOLLOW_LINE:
           // Set correct forward speed to every strategy
-...
             if(lf.followLine(rpmSpeed)){
               utCount.whiteCount++;
               if(utCount.whiteCount >= WHITE_COUNT_THRESH){
                 rpmSpeed[0] = stop[0];
                 rpmSpeed[1] = stop[1];
                 newState = states::IDLE;
               if(utCount.whiteCount >= WHITE_DETETION_TIMEOUT){
                 setRpmSpeed(stop);
                 utCount.whiteCount = 0;
                 newState = states::WHITE_DETECTION_ERROR;
+              }
             }else{
               utCount.whiteCount = 0;
               // setRpmSpeed(rpmSpeed);
+            }
             if(getProxyRingSum() > PROXY_RING_THRESH){
               utCount.proxyCount++;
               if(utCount.proxyCount > WHITE_COUNT_THRESH){
                 rpmSpeed[0] = stop[0];
                 rpmSpeed[1] = stop[1];
                 // newState = states::IDLE;
                 if (continue_on_obstacle){
                   newState = states::TURN;
                   utCount.proxyCount = 0;
                 }else{
                   newState = states::IDLE;
+                }
+              }
             }else{
                 utCount.proxyCount = 0;
+            }
             preventCollision(rpmSpeed, rProx);
             // proxSectorSpeedCorrection(rpmSpeed, rProx);
             if ((rProx[3] + rProx[4]) > RING_PROX_FRONT_THRESH){
               rpmSpeed[0] = stop[0];
               rpmSpeed[1] = stop[1];
             if(utCount.ringProxCount > RING_PROX_DETECTION_TIMEOUT){
               utCount.ringProxCount = 0;
               newState = states::TURN;
+            }
             if (lf.getStrategy() == LineFollowStrategy::FUZZY){
-...
               setRpmSpeed(rpmSpeed);
+            }
             break;
           break;
           // ---------------------------------------
           case states::TURN:
             checkForMotion();
             // Check if only front sensors are active
             if(checkFrontalObject()){
               global.distcontrol.setTargetPosition(0, ROTATION_180, ROTATION_DURATION);
               global.distcontrol.setTargetPosition(0, 2792526, ROTATION_DURATION);
               // BaseThread::sleep(8000);
               checkForMotion();
               newState = states::FOLLOW_LINE;
               lf.promptStrategyChange(LineFollowStrategy::EDGE_LEFT);
               // lf.promptStrategyChange(LineFollowStrategy::EDGE_LEFT);
             }else{
               newState = states::IDLE;
               newState = states::PROXY_DETECTION_ERROR;
+            }
             break;
           break;
           // ---------------------------------------
           case states::DETECT_STATION:
             if (global.forwardSpeed != DETECTION_SPEED){
-...
               checkForMotion();
               newState = states::CORRECT_POSITIONING;
+            }
             break;
           break;
           // ---------------------------------------
           case states::CORRECT_POSITIONING:
             if (global.forwardSpeed != CHARGING_SPEED){
-...
             lf.followLine(rpmSpeed);
             setRpmSpeed(rpmSpeed);
             utCount.stepCount++;
             if (utCount.stepCount >= MAX_CORRECTION_STEPS){
               utCount.stepCount = 0;
             utCount.stateTime++;
             if (utCount.stateTime >= DOCKING_CORRECTION_TIMEOUT){
               utCount.stateTime = 0;
               newState = states::REVERSE;
               setRpmSpeed(stop);
               checkForMotion();
+            }
             break;
           break;
           // ---------------------------------------
           case states::REVERSE:
             if(lf.getStrategy() != LineFollowStrategy::REVERSE){
-...
+            }
             lf.followLine(rpmSpeed);
             setRpmSpeed(rpmSpeed);
             utCount.stateTime++;
             // if ((WL+WR) < PROXY_WHEEL_THRESH){
             // Is of those sensors at it max val means that the AMiRo cant drive back
             // so check if correctly positioned
             //definitely wrong positioned -> correct position directly without rotation
             // Both sensors are at a wall -> potential loading station dock and on magnet -> need to rotate
             // if((rProx[0] >= PROX_MAX_VAL) && (rProx[7] >= PROX_MAX_VAL)){
             //   setRpmSpeed(stop);
             //   checkForMotion();
             //   newState = states::CHECK_POSITIONING;
             // } else
             if((rProx[0] >= PROX_MAX_VAL) || (rProx[7] >= PROX_MAX_VAL)){
               // setRpmSpeed(stop);
               // checkForMotion();
               utCount.stepCount = 0;
               utCount.stateTime = 0;
               newState = states::PUSH_BACK;
             }else if (utCount.stateTime >= REVERSE_DOCKING_TIMEOUT){
               setRpmSpeed(stop);
               utCount.stateTime = 0;
               utCount.errorCount++;
               if (utCount.errorCount >= DOCKING_ERROR_THRESH){
                 newState = states::DOCKING_ERROR;
               }else{
                 newState = states::CORRECT_POSITIONING;
+              }
+            }
             break;
           break;
           // ---------------------------------------
           case states::PUSH_BACK:
             if(lf.getStrategy() != LineFollowStrategy::REVERSE){
-...
             lf.followLine(rpmSpeed);
             setRpmSpeed(rpmSpeed);
             utCount.stepCount++;
             if (utCount.stepCount > PUSH_BACK_COUNT){
             utCount.stateTime++;
             if (utCount.stateTime > PUSH_BACK_TIMEOUT){
               utCount.stateTime = 0;
               newState = states::CHECK_POSITIONING;
+            }
             break;
           break;
           // ---------------------------------------
           case states::CHECK_POSITIONING:
             setRpmSpeed(stop);
             checkForMotion();
             // if((rProx[0] >= PROX_MAX_VAL) && (rProx[7] >= PROX_MAX_VAL)){
             if(checkDockingSuccess()){
               newState = states::CHECK_VOLTAGE;
             }else{
               utCount.errorCount++;
               newState = states::CORRECT_POSITIONING;
               if (utCount.errorCount > DOCKING_ERROR_THRESH){
                   newState = states::ERROR;
               if (utCount.errorCount >= DOCKING_ERROR_THRESH){
                   newState = states::DOCKING_ERROR;
+                }
+            }
             // }
             // else{
             //   newState = CORRECT_POSITIONING;
             // }
             break;
           break;
           // ---------------------------------------
           case states::CHECK_VOLTAGE:
             if(!checkPinEnabled()){
-...
             } else {
               if(checkPinVoltage()){
                 // Pins are under voltage -> correctly docked
                 utCount.errorCount = 0;
                 newState = states::CHARGING;
               }else{
                 utCount.errorCount++;
-...
+                }
                 if (utCount.errorCount > DOCKING_ERROR_THRESH){
                   newState = states::ERROR;
                   newState = states::DOCKING_ERROR;
+                }
+              }
+            }
             break;
           break;
           // ---------------------------------------
           case states::RELEASE_TO_CORRECT:
-...
             global.distcontrol.setTargetPosition(0, -2*ROTATION_20, ROTATION_DURATION);
             checkForMotion();
             global.distcontrol.setTargetPosition(7000, 0, ROTATION_DURATION);
             global.distcontrol.setTargetPosition(1500, 0, ROTATION_DURATION);
             checkForMotion();
             newState = states::CORRECT_POSITIONING;
             break;
           // ---------------------------------------
           case states::ERROR:
             utCount.errorCount = 0;
             // lStrategy = LineFollowStrategy::EDGE_RIGHT;
             newState = states::RELEASE;
             break;
           break;
           // ---------------------------------------
           case states::CHARGING:
             global.motorcontrol.setMotorEnable(false);
             utCount.errorCount = 0;
             // Formulate Request to enable charging
             if(/* checkPinVoltage() && */ !checkPinEnabled()){
               global.robot.requestCharging(1);
-...
             if(checkPinEnabled()){
               showChargingState();
+            }
             break;
           break;
           // ---------------------------------------
           case states::RELEASE:
           if (global.forwardSpeed != DETECTION_SPEED){
-...
               global.distcontrol.setTargetPosition(5000, 0, ROTATION_DURATION);
               checkForMotion();
               // rotate back
               global.distcontrol.setTargetPosition(0, -ROTATION_20, ROTATION_DURATION);
               checkForMotion();
               // global.distcontrol.setTargetPosition(0, -ROTATION_20, ROTATION_DURATION);
               // checkForMotion();
               // global.distcontrol.setTargetPosition(5000, 0, ROTATION_DURATION);
               // checkForMotion();
-...
               // setRpmSpeed(rpmSpeed);
+            }
             // lightAllLeds(Color::BLACK);
             break;
           break;
           // ---------------------------------------
           case states::DOCKING_ERROR:
             newState = states::RELEASE;
           break;
           // ---------------------------------------
           case states::REVERSE_TIMEOUT_ERROR:
             newState = states::IDLE;
           break;
           // ---------------------------------------
           case states::CALIBRATION_ERROR:
             newState = states::IDLE;
           break;
           // ---------------------------------------
           case states::WHITE_DETECTION_ERROR:
             newState = states::IDLE;
           break;
           // ---------------------------------------
           case states::PROXY_DETECTION_ERROR:
             newState = states::IDLE;
           break;
           // ---------------------------------------
           case states::NO_CHARGING_POWER_ERROR:
             newState = states::IDLE;
           break;
           // ---------------------------------------
           case states::UNKNOWN_STATE_ERROR:
             newState = states::IDLE;
           break;
           // ---------------------------------------
           default:
             break;
             newState = states::UNKNOWN_STATE_ERROR;
           break;
+          }
           if (utState != newState){
           if (currentState != newState){
             chprintf((BaseSequentialStream*)&global.sercanmux1, "Transmit state %d\n", newState);
             global.robot.transmitState(newState);
+          }
           utState = newState;
           prevState = currentState;
           currentState = newState;
           if (utCount.stateCount > CAN_TRANSMIT_STATE_THRESH){
               utCount.stateCount = 0;
             // chprintf((BaseSequentialStream*)&global.sercanmux1, "Transmit state %d\n", newState);
             global.robot.transmitState(currentState);
             // global.robot.setOdometry(global.odometry.getPosition());
           }else{
             utCount.stateCount++;
+          }
         this->sleep(CAN::UPDATE_PERIOD);
+      }

     #include <ch.hpp>
     #include <amiroosconf.h>
     #include <amiro/Color.h>
     // #include "global.hpp"
     // #include "linefollow.hpp"
     #include <cmath>
     // Speed when driving towards the docking station
     #define CHARGING_SPEED 5
-...
     #define DIST_THRESH 100
     #define RELEASE_COUNT 200
     // Thresh to determain how much update steps should pass while alining
     #define MAX_CORRECTION_STEPS 300
     // #define MAX_CORRECTION_STEPS 200
     #define DOCKING_CORRECTION_TIMEOUT 200
     #define REVERSE_DOCKING_TIMEOUT 2*DOCKING_CORRECTION_TIMEOUT
     // Thresh for wheel proxy sensors, when summed values fall below the state changes
     #define PROXY_WHEEL_THRESH 18000
     // #define PROXY_WHEEL_THRESH 18000
     // Thresh for detecting obsticles
     #define PROXY_RING_THRESH 15000
     // #define PROXY_RING_THRESH 15000
     #define PUSH_BACK_COUNT 5
     // #define PUSH_BACK_COUNT 5
     #define PUSH_BACK_TIMEOUT 5
     // Thresh for how long (update steps) the front sensors are allowed to detect white
     #define WHITE_COUNT_THRESH 150
     // #define WHITE_COUNT_THRESH 150
     #define WHITE_DETETION_TIMEOUT 150
     // #define RING_PROX_COUNT_THRESH 1000
     #define RING_PROX_DETECTION_TIMEOUT 800
     // Rotation around 180 degrees in microradian
     // #define ROTATION_180 3141592
     #define ROTATION_180 3141592
     // Rotation around -20 degrees in microradian
     #define ROTATION_20 -349065
-...
     #define PROX_MAX_VAL 65430
     // Threshold for failing to dock
     #define DOCKING_ERROR_THRESH 4
     #define DOCKING_ERROR_THRESH 3
     #define CAN_TRANSMIT_STATE_THRESH 50
     namespace amiro {
-...
       // Messages which can be received and trigger state changes
       public:
         enum msg_content{
           STOP,
           START,
           EDGE_LEFT,
           EDGE_RIGHT,
           FUZZY,
           DOCK,
           UNDOCK,
           CHARGE
         };
         // States of user thread state machine
         enum states : uint8_t{
         enum states : int8_t{
           IDLE                = 0,
           FOLLOW_LINE         = 1,
           DETECT_STATION      = 2,
-...
           RELEASE             = 8,
           RELEASE_TO_CORRECT  = 9,
           CORRECT_POSITIONING = 10,
           ERROR               = 11,
           TURN                =12
           TURN                = 12,
           INACTIVE            = 13,
           CALIBRATION         = 14,
           CALIBRATION_CHECK   = 15,
           DOCKING_ERROR       = -1,
           REVERSE_TIMEOUT_ERROR   = -2,
           CALIBRATION_ERROR   = -3,
           WHITE_DETECTION_ERROR   = -4,
           PROXY_DETECTION_ERROR   = -5,
           NO_CHARGING_POWER_ERROR   = -6,
           UNKNOWN_STATE_ERROR   = -7
         };
       struct ut_counter{
           int whiteCount = 0;
           int proxyCount = 0;
           int ringProxCount = 0;
           // int correctionCount = 0;
           int errorCount = 0;
           int idleCount = 0;
           int stateCount = 0;
           int stepCount = 0;
           uint32_t stateTime = 0;
       };
       struct proxy_ctrl {
         int threshLow = 100;
         int threshMid = 500;
         int threshHigh = 1500;
         // int threshHigh = 1500;
         int penalty = 100000;
         int pFactor = 310000;
         int staticCont = 0;
       };
       struct bottom_prox_calibration {
         bool calibrateBlack = true;
         uint32_t buf = 0;
         uint8_t meanWindow = 150;
       };
       // static const struct ut_counter emptyUtCount;
       ut_counter utCount;
       proxy_ctrl pCtrl;
       bottom_prox_calibration proxCalib;
       explicit UserThread();
       virtual ~UserThread();
-...
       bool checkPinVoltage();
       bool checkPinEnabled();
       int getProxyRingSum();
       /**
        * Check sectors around and stop if a thresh in one sector is detected.
        */
       void preventCollision(int (&rpmSpeed)[2], uint16_t (&proxVals)[8]);
       /**
        * Same as prevent collision but also lowers the speed when object is detected.
        */
       void proxSectorSpeedCorrection(int (&rpmSpeed)[2], uint16_t (&proxVals)[8]);
       void getProxySectorVals(uint16_t (&proxVals)[8], uint16_t (&sProx)[8]);
       void getMaxFrontSectorVal(uint16_t (&sProx)[8], int32_t &sPMax);
       void chargeAsLED();
       /**
        * Returns true when front sensors reaching high values
-...
        */
       bool checkFrontalObject();
       states utState = states::IDLE;
       states newState = states::IDLE;
       bool continue_on_obstacle = true;
       /**
        * Check if current position changes when the wheel are deactivated.
+       *
-...
        * was successful (return 1) otherwise a correction is needed (return 0).
        */
       int checkDockingSuccess();
       // State Variables
       states prevState = states::IDLE;
       states currentState = states::IDLE;
       states newState = states::IDLE;
       bool continue_on_obstacle = true;
       uint16_t rProx[8]; // buffer for ring proxy values
       int rpmSpeed[2] = {0};
       int stop[2] = {0};
       int turn[2] = {5,-5};
     };
     } // end of namespace amiro

Also available in: Unified diff