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

Revision fbcb25cc

       // PID controller components ---------------
       int32_t K_p = 1400;
       float K_i = 0.06;
       float K_d = 0.2;
       float K_d = 0.1;
       int32_t accumHist = 0;
       float oldError = 0;
       // ----------------------------------------

+      }
+    }
     bool UserThread::checkFrontalObject(){
       uint32_t thresh = 1000;
       uint32_t prox;
       for(int i=0; i<8; i++){
         prox = global.robot.getProximityRingValue(i);
         if((i == 3) || (i == 4)){
           if(prox < thresh){
             return false;
+          }
         }else{
           if(prox > thresh){
             return false;
+          }
+        }
+      }
       return true;
+    }
     bool UserThread::checkPinVoltage(){
       return global.ltc4412.isPluggedIn();
+    }
-...
        * SETUP
        */
       // User thread state:
     states utState = states::IDLE;
     states newState;
        int whiteBuf = 0;
        int proxyBuf = 0;
       //  int whiteBuf = 0;
       //  int proxyBuf = 0;
       //  int reverseBuf = 0;
        int correctionStep = 0;
       //  int dist_count = 0;
-...
         if (accel_z < -900 /*-0.9g*/) {
           // Start line following when AMiRo is rotated
           if(utState == states::IDLE){
             utState = states::FOLLOW_LINE;
             newState = states::FOLLOW_LINE;
           }else{
             utState = states::IDLE;
             newState = states::IDLE;
+          }
           lightAllLeds(Color::GREEN);
           this->sleep(1000);
-...
           // running = true;
           switch(global.lfStrategy){
             case msg_content::START:
               utState = states::FOLLOW_LINE;
               newState = states::FOLLOW_LINE;
               break;
             case msg_content::STOP:
               utState = states::IDLE;
               newState = states::IDLE;
               break;
             case msg_content::EDGE_RIGHT:
               // utState = states::FOLLOW_LINE;
               // newState = states::FOLLOW_LINE;
               lStrategy = LineFollowStrategy::EDGE_RIGHT;
               break;
             case  msg_content::EDGE_LEFT:
               // utState = states::FOLLOW_LINE;
               // newState = states::FOLLOW_LINE;
               lStrategy = LineFollowStrategy::EDGE_LEFT;
               break;
             case msg_content::FUZZY:
               // utState = states::FOLLOW_LINE;
               // newState = states::FOLLOW_LINE;
               lStrategy = LineFollowStrategy::FUZZY;
               break;
             case msg_content::DOCK:
               utState = states::DETECT_STATION;
               newState = states::DETECT_STATION;
               break;
             case msg_content::UNDOCK:
               utState = states::RELEASE;
               newState = states::RELEASE;
               break;
             case msg_content::CHARGE:
               utState = states::CHARGING;
               newState = states::CHARGING;
               break;
             default:
               utState = states::IDLE;
               newState = states::IDLE;
               break;
+          }
+        }
         newState = utState;
         // newState = utState;
         // Get sensor data
         uint16_t WL = global.vcnl4020[constants::DiWheelDrive::PROX_WHEEL_LEFT].getProximityScaledWoOffset();
         uint16_t WR = global.vcnl4020[constants::DiWheelDrive::PROX_WHEEL_RIGHT].getProximityScaledWoOffset();
         // uint16_t WL = global.vcnl4020[constants::DiWheelDrive::PROX_WHEEL_LEFT].getProximityScaledWoOffset();
         // uint16_t WR = global.vcnl4020[constants::DiWheelDrive::PROX_WHEEL_RIGHT].getProximityScaledWoOffset();
         for(int i=0; i<8;i++){
           rProx[i] = global.robot.getProximityRingValue(i);
+        }
-...
             if(/* checkPinVoltage() && */ checkPinEnabled()){
               global.robot.requestCharging(0);
+            }
             whiteBuf = 0;
             proxyBuf = 0;
             utCount.whiteCount = 0;
             utCount.proxyCount = 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;
           // ---------------------------------------
           case states::FOLLOW_LINE:
-...
               lf.setStrategy(lStrategy);
+            }
             //TODO: Check if white is detected and stop threshold is reached
             if(lf.followLine(rpmSpeed)){
               whiteBuf++;
               if(whiteBuf >= WHITE_COUNT_THRESH){
               utCount.whiteCount++;
               if(utCount.whiteCount >= WHITE_COUNT_THRESH){
                 rpmSpeed[0] = stop[0];
                 rpmSpeed[1] = stop[1];
                 newState = states::IDLE;
+              }
             }else{
               whiteBuf = 0;
               utCount.whiteCount = 0;
               // setRpmSpeed(rpmSpeed);
+            }
             if(getProxyRingSum() > PROXY_RING_THRESH){
               proxyBuf++;
               if(proxyBuf > WHITE_COUNT_THRESH){
                 newState = states::IDLE;
               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{
                 proxyBuf = 0;
                 utCount.proxyCount = 0;
+            }
             if ((rProx[3] + rProx[4]) > RING_PROX_FRONT_THRESH){
-...
               rpmSpeed[1] = stop[1];
+            }
             // if (needDistance){
             //   whiteBuf = 0;
             //   proxyBuf = 0;
             //   dist_count++;
             //   if(dist_count > DIST_THRESH){
             //     dist_count = 0;
             //     needDistance = false;
             //   }
             // }
             // whiteBuf = 0;
             // proxyBuf = 0;
             // lf.followLine(rpmSpeed);
             if (lf.getStrategy() == LineFollowStrategy::FUZZY){
               setRpmSpeedFuzzy(rpmSpeed);
             }else{
-...
             break;
           // ---------------------------------------
           case states::TURN:
             checkForMotion();
             // Check if only front sensors are active
             if(checkFrontalObject()){
               global.distcontrol.setTargetPosition(0, ROTATION_180, ROTATION_DURATION);
               // BaseThread::sleep(8000);
               checkForMotion();
               newState = states::FOLLOW_LINE;
               lf.promptStrategyChange(LineFollowStrategy::EDGE_LEFT);
             }else{
               newState = states::IDLE;
+            }
             break;
           // ---------------------------------------
           case states::DETECT_STATION:
             if (global.forwardSpeed != DETECTION_SPEED){
               global.forwardSpeed = DETECTION_SPEED;
-...
             lf.followLine(rpmSpeed);
             setRpmSpeed(rpmSpeed);
             correctionStep++;
             if (correctionStep >= MAX_CORRECTION_STEPS){
               correctionStep = 0;
             utCount.stepCount++;
             if (utCount.stepCount >= MAX_CORRECTION_STEPS){
               utCount.stepCount = 0;
               newState = states::REVERSE;
               setRpmSpeed(stop);
               checkForMotion();
-...
             if((rProx[0] >= PROX_MAX_VAL) || (rProx[7] >= PROX_MAX_VAL)){
               // setRpmSpeed(stop);
               // checkForMotion();
               utCount.reverseCount = 0;
               utCount.stepCount = 0;
               newState = states::PUSH_BACK;
+            }
             break;
-...
             lf.followLine(rpmSpeed);
             setRpmSpeed(rpmSpeed);
             utCount.reverseCount++;
             if (utCount.reverseCount > PUSH_BACK_COUNT){
             utCount.stepCount++;
             if (utCount.stepCount > PUSH_BACK_COUNT){
               newState = states::CHECK_POSITIONING;
+            }
             break;
-...
           // ---------------------------------------
           case states::ERROR:
             utCount.errorCount = 0;
             lStrategy = LineFollowStrategy::EDGE_RIGHT;
             newState = states::FOLLOW_LINE;
             // lStrategy = LineFollowStrategy::EDGE_RIGHT;
             newState = states::RELEASE;
             break;
           // ---------------------------------------
           case states::CHARGING:
-...
             break;
+          }
           if (utState != newState){
             chprintf((BaseSequentialStream*)&global.sercanmux1, "Transmit state %d\n", newState);
             global.robot.transmitState(newState);
+          }
           utState = newState;

     // Thresh for detecting obsticles
     #define PROXY_RING_THRESH 15000
     #define PUSH_BACK_COUNT 50
     #define PUSH_BACK_COUNT 5
     // 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
-...
           RELEASE             = 8,
           RELEASE_TO_CORRECT  = 9,
           CORRECT_POSITIONING = 10,
           ERROR               = 11
           ERROR               = 11,
           TURN                =12
         };
       struct ut_counter{
           int whiteCount = 0;
           int proxyCount = 0;
           int reverseCount = 0;
           int correctionCount = 0;
           // int correctionCount = 0;
           int errorCount = 0;
           int idleCount = 0;
           int stepCount = 0;
       };
       // static const struct ut_counter emptyUtCount;
-...
       bool checkPinVoltage();
       bool checkPinEnabled();
       int getProxyRingSum();
       /**
        * Returns true when front sensors reaching high values
        * and all others are low. This indicates that the loading station is ahead.
        * If other sensores are blocked too, indicates that someone grabs the robot.
        */
       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.
+       *

Also available in: Unified diff