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

Revision 27d4e1fa

       int success = 0;
       global.odometry.resetPosition();
       types::position start = global.startPos = global.odometry.getPosition();
       global.motorcontrol.toggleMotorEnable();
       this->sleep(500);
       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)){
         lightAllLeds(Color::RED);
         // Enable Motor again if docking was not successful
         global.motorcontrol.toggleMotorEnable();
         global.motorcontrol.setMotorEnable(true);
         success = 0;
       }else{
         lightAllLeds(Color::GREEN);
-...
        int whiteBuf = 0;
        int proxyBuf = 0;
        int releaseBuf = 0;
       //  int reverseBuf = 0;
        int correctionStep = 0;
        int dist_count = 0;
        bool needDistance = false;
       //  int dist_count = 0;
       //  bool needDistance = false;
        uint16_t rProx[8]; // buffer for ring proxy values
       int rpmSpeed[2] = {0};
-...
         // int FR = global.vcnl4020[constants::DiWheelDrive::PROX_FRONT_RIGHT].getProximityScaledWoOffset();
         switch(utState){
           case states::IDLE:
             if (!global.motorcontrol.getMotorEnable()){
                 global.motorcontrol.toggleMotorEnable();
+            }
             global.motorcontrol.setMotorEnable(true);
             setRpmSpeed(stop);
             if(/* checkPinVoltage() && */ checkPinEnabled()){
               global.robot.requestCharging(0);
+            }
             whiteBuf = 0;
             proxyBuf = 0;
             utCount.errorCount = 0;
             break;
           // ---------------------------------------
           case states::FOLLOW_LINE:
-...
             if((rProx[0] >= PROX_MAX_VAL) || (rProx[7] >= PROX_MAX_VAL)){
               // setRpmSpeed(stop);
               // checkForMotion();
               utCount.reverseCount = 0;
               newState = states::PUSH_BACK;
+            }
             break;
           // ---------------------------------------
           case states::PUSH_BACK:
             if(lf.getStrategy() != LineFollowStrategy::REVERSE){
               lf.setStrategy(LineFollowStrategy::REVERSE);
+            }
             lf.followLine(rpmSpeed);
             setRpmSpeed(rpmSpeed);
             utCount.reverseCount++;
             if (utCount.reverseCount > PUSH_BACK_COUNT){
               newState = states::CHECK_POSITIONING;
+            }
             break;
-...
             setRpmSpeed(stop);
             checkForMotion();
             // if((rProx[0] >= PROX_MAX_VAL) && (rProx[7] >= PROX_MAX_VAL)){
               if(checkDockingSuccess()){
                 newState = states::CHECK_VOLTAGE;
               }else{
                 newState = states::CORRECT_POSITIONING;
+              }
             if(checkDockingSuccess()){
               newState = states::CHECK_VOLTAGE;
             }else{
               utCount.errorCount++;
               newState = states::CORRECT_POSITIONING;
               if (utCount.errorCount > DOCKING_ERROR_THRESH){
                   newState = states::ERROR;
+                }
+            }
             // }
             // else{
             //   newState = CORRECT_POSITIONING;
-...
             } else {
               if(checkPinVoltage()){
                 // Pins are under voltage -> correctly docked
                 utCount.errorCount = 0;
                 newState = states::CHARGING;
               }else{
                 utCount.errorCount++;
                 // No voltage on pins -> falsely docked
                 // deactivate pins
                 if (!global.motorcontrol.getMotorEnable()){
                 global.motorcontrol.toggleMotorEnable();
+                }
                 global.motorcontrol.setMotorEnable(true);
                 global.robot.requestCharging(0);
                 // TODO: Soft release when docking falsely
                 if((rProx[0] >= PROX_MAX_VAL) && (rProx[7] >= PROX_MAX_VAL)){
                   newState = states::RELEASE_TO_CORRECT;
                 } else {
                   newState = states::CORRECT_POSITIONING;
                   newState = states::RELEASE_TO_CORRECT; //states::CORRECT_POSITIONING;
+                }
                 if (utCount.errorCount > DOCKING_ERROR_THRESH){
                   newState = states::ERROR;
+                }
+              }
+            }
             break;
           // ---------------------------------------
           case states::RELEASE_TO_CORRECT:
             global.distcontrol.setTargetPosition(0, ROTATION_20, ROTATION_DURATION);
             checkForMotion();
             // move 1cm forward
-...
             newState = states::CORRECT_POSITIONING;
             break;
           // ---------------------------------------
           case states::ERROR:
             utCount.errorCount = 0;
             lStrategy = LineFollowStrategy::EDGE_RIGHT;
             newState = states::FOLLOW_LINE;
             break;
           // ---------------------------------------
           case states::CHARGING:
             if (global.motorcontrol.getMotorEnable()){
               global.motorcontrol.toggleMotorEnable();
+            }
             global.motorcontrol.setMotorEnable(false);
             // Formulate Request to enable charging
             if(/* checkPinVoltage() && */ !checkPinEnabled()){
               global.robot.requestCharging(1);
-...
             if(/* checkPinVoltage() && */ checkPinEnabled()){
               global.robot.requestCharging(0);
             }else{
               if (!global.motorcontrol.getMotorEnable()){
                 global.motorcontrol.toggleMotorEnable();
+              }
               // if (releaseBuf > RELEASE_COUNT){
               //   releaseBuf = 0;
               //   setRpmSpeed(stop);
               //   needDistance = true;
               //   newState = states::FOLLOW_LINE;
               // }else{
               //   releaseBuf++;
               //   setRpmSpeed(turn);
               // }
               global.motorcontrol.setMotorEnable(true);
               //Rotate -20° to free from magnet
               global.distcontrol.setTargetPosition(0, ROTATION_20, ROTATION_DURATION);
               checkForMotion();
-...
           default:
             break;
+          }
           if (utState != newState){
             global.robot.transmitState(newState);
+          }
           utState = newState;
         this->sleep(CAN::UPDATE_PERIOD);
+      }

     #define PROXY_WHEEL_THRESH 18000
     // Thresh for detecting obsticles
     #define PROXY_RING_THRESH 15000
     #define PUSH_BACK_COUNT 50
     // Thresh for how long (update steps) the front sensors are allowed to detect white
     #define WHITE_COUNT_THRESH 150
     // Rotation around 180 degrees in microradian
-...
     #define RING_PROX_FRONT_THRESH 18000
     #define PROX_MAX_VAL 65430
     // Threshold for failing to dock
     #define DOCKING_ERROR_THRESH 4
     namespace amiro {
-...
         };
         // States of user thread state machine
         enum states{
           FOLLOW_LINE,
           RELEASE,
           RELEASE_TO_CORRECT,
           CHARGING,
           DETECT_STATION,
           CORRECT_POSITIONING,
           REVERSE,
           CHECK_POSITIONING,
           CHECK_VOLTAGE,
           IDLE
         enum states : uint8_t{
           IDLE                = 0,
           FOLLOW_LINE         = 1,
           DETECT_STATION      = 2,
           REVERSE             = 3,
           PUSH_BACK           = 4,
           CHECK_POSITIONING   = 5,
           CHECK_VOLTAGE       = 6,
           CHARGING            = 7,
           RELEASE             = 8,
           RELEASE_TO_CORRECT  = 9,
           CORRECT_POSITIONING = 10,
           ERROR               = 11
         };
       struct ut_counter{
           int whiteCount = 0;
           int proxyCount = 0;
           int reverseCount = 0;
           int correctionCount = 0;
           int errorCount = 0;
       };
       // static const struct ut_counter emptyUtCount;
       ut_counter utCount;
       explicit UserThread();
-...
       int getProxyRingSum();
       /**
        * Check if current position changes when the wheel are deactivated.
+       *

Also available in: Unified diff