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

Revision 181f2892

     #include "userthread.hpp"
     #include "global.hpp"
     #include "linefollow2.hpp"
     using namespace amiro;
     extern Global global;
     enum class states{
       FOLLOW_LINE,
       OCCUPY,
       RELEASE,
       CHARGING
     };
     enum msg_content{
       STOP,
       START,
       EDGE_LEFT,
       EDGE_RIGHT,
       FUZZY,
       DOCKING
     };
     // User thread state:
     states utState = states::FOLLOW_LINE;
     // a buffer for the z-value of the accelerometer
     int16_t accel_z;
     bool running = false;
     int vcnl4020AmbientLight[4] = {0};
     int vcnl4020Proximity[4] = {0};
     // Set the speed by the array
     void setRpmSpeed(const int (&rpmSpeed)[2]) {
     /**
      * Set speed.
+     *
      * @param rpmSpeed speed for left and right wheel in rounds/min
      */
     void UserThread::setRpmSpeed(const int (&rpmSpeed)[2]) {
       global.motorcontrol.setTargetRPM(rpmSpeed[constants::DiWheelDrive::LEFT_WHEEL] * 1000000, rpmSpeed[constants::DiWheelDrive::RIGHT_WHEEL] * 1000000);
+    }
     void lightOneLed(Color color, int idx){
     void UserThread::lightOneLed(Color color, int idx){
       global.robot.setLightColor(idx, Color(color));
+    }
     void lightAllLeds(Color color){
     void UserThread::lightAllLeds(Color color){
       int led = 0;
       for(led=0; led<8; led++){
             lightOneLed(color, led);
+          }
+    }
     /**
      * Get the wanted strategy from the global object.
      */
     LineFollowStrategy determineStrategy(){
       switch(global.lfStrategy){
         case 0:
           return LineFollowStrategy::EDGE_RIGHT;
           break;
         case 1:
           return LineFollowStrategy::EDGE_LEFT;
           break;
         case 2:
           return LineFollowStrategy::FUZZY;
           break;
         case 3:
           return LineFollowStrategy::DOCKING;
         default:
           break;
     void UserThread::showChargingState(){
       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);
+      }
       return LineFollowStrategy::NONE;
       this->sleep(1000);
       lightAllLeds(Color::BLACK);
+    }
     /**
      * Blocks as long as the position changes.
      */
     void checkForMotion(UserThread *ut){
     void UserThread::checkForMotion(){
       int motion = 1;
       int led = 0;
       types::position oldPos = global.odometry.getPosition();
       while(motion){
         ut->sleep(500);
         this->sleep(500);
         types::position tmp = global.odometry.getPosition();
         motion = abs(oldPos.x - tmp.x)+ abs(oldPos.y - tmp.y)+abs(oldPos.z - tmp.z);
         oldPos = tmp;
-...
+      }
+    }
     bool UserThread::checkPinVoltage(){
       return global.ltc4412.isPluggedIn();
+    }
     /**
      * Turns MotorControl off and checks if position remains stable.
      */
     int checkDockingSuccess(UserThread *ut){
       // global.motorcontrol.setTargetRPM(0,0);
       checkForMotion(ut);
       int led = 0;
     bool UserThread::checkPinEnabled(){
       return global.ltc4412.isEnabled();
+    }
     int UserThread::checkDockingSuccess(){
       // setRpmSpeed(stop);
       checkForMotion();
       int success = 0;
       global.odometry.resetPosition();
       types::position start = global.startPos = global.odometry.getPosition();
       global.motorcontrol.toggleMotorEnable();
       ut->sleep(1000);
       types::position stop = global.endPos = global.odometry.getPosition();
       global.motorcontrol.toggleMotorEnable();
       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)){
         for(led=0; led<8; led++){
           global.robot.setLightColor(led, Color(Color::RED));
+        }
       if ((start.x + stop_.x) || (start.y + stop_.y)){
         lightAllLeds(Color::RED);
         // Enable Motor again if docking was not successful
         global.motorcontrol.toggleMotorEnable();
         success = 0;
       }else{
         for(led=0; led<8; led++){
           global.robot.setLightColor(led, Color(Color::GREEN));
+        }
         lightAllLeds(Color::GREEN);
         success = 1;
+      }
       ut->sleep(500);
       for(led=0; led<8; led++){
               global.robot.setLightColor(led, Color(Color::BLACK));
+      }
       this->sleep(500);
       lightAllLeds(Color::BLACK);
       return success;
+    }
     void correctPosition(UserThread *ut, LineFollow &lf, int steps){
       int checkWhite = 0;
       int rpmSpeed[2] ={0};
       lf.setStrategy(LineFollowStrategy::EDGE_LEFT);
       for (int correction=0; correction<steps; correction++){
         checkWhite = lf.followLine(rpmSpeed);
         setRpmSpeed(rpmSpeed);
         ut->sleep(CAN::UPDATE_PERIOD);
+      }
+    }
     UserThread::UserThread() :
       chibios_rt::BaseStaticThread<USER_THREAD_STACK_SIZE>()
+    {
-...
       /*
        * SETUP
        */
       // User thread state:
     states utState = states::IDLE;
     states newState;
        int whiteBuf = 0;
        int correctionStep = 0;
       int rpmSpeed[2] = {0};
       int stop[2] = {0};
       int proxyWheelThresh = 18000;
       LineFollowStrategy lStrategy;
       LineFollowStrategy lStrategy = LineFollowStrategy::FUZZY;
       for (uint8_t led = 0; led < 8; ++led) {
         global.robot.setLightColor(led, Color(Color::BLACK));
+      }
-...
        */
       while (!this->shouldTerminate())
+      {
             /*
              * read accelerometer z-value
              */
             accel_z = global.lis331dlh.getAccelerationForce(LIS331DLH::AXIS_Z);
             /*
              * evaluate the accelerometer
              */
         /*
         * read accelerometer z-value
         */
         accel_z = global.lis331dlh.getAccelerationForce(LIS331DLH::AXIS_Z);
         if (accel_z < -900 /*-0.9g*/) {
           // Start line following when AMiRo is rotated
           if(utState == states::IDLE){
             utState = states::FOLLOW_LINE;
           }else{
             utState = states::IDLE;
+          }
           lightAllLeds(Color::GREEN);
           this->sleep(1000);
           lightAllLeds(Color::BLACK);
       //Features over can: (Line Following)
       //SetKP
       //SetKI
       //SetKd ?
       //SetSleepTime
       //SeForwardSpeed? or SetMaxSpeed
       //DriveOnLeftLine
       //DriveOnRightLine
       //if accel_z<-900
         //send can event (one time)
         // Line following is started if amiro roteted
         if (accel_z < -900 /*-0.9g*/) { //new: (can.allowLineFollowOnTurn && accel_z < 900) || can.startLineFollow optional feature
             // set the front LEDs to blue for one second
             lightAllLeds(Color::GREEN);
             this->sleep(1000);
             lightAllLeds(Color::BLACK);
             running = !running;
         // If message was received handle it here:
         } else if(global.msgReceived){
           global.msgReceived = false;
           running = true;
           // running = true;
           switch(global.lfStrategy){
             case msg_content::START:
               utState = states::FOLLOW_LINE;
               break;
             case msg_content::STOP:
               running = false;
               utState = states::IDLE;
               break;
             case msg_content::EDGE_RIGHT:
               // utState = states::FOLLOW_LINE;
               lStrategy = LineFollowStrategy::EDGE_RIGHT;
               break;
             case  msg_content::EDGE_LEFT:
               // utState = states::FOLLOW_LINE;
               lStrategy = LineFollowStrategy::EDGE_LEFT;
               break;
             case msg_content::FUZZY:
               // utState = states::FOLLOW_LINE;
               lStrategy = LineFollowStrategy::FUZZY;
               break;
             case msg_content::DOCKING:
               utState = states::OCCUPY;
               lStrategy = LineFollowStrategy::EDGE_RIGHT;
             case msg_content::DOCK:
               utState = states::DETECT_STATION;
               break;
             case msg_content::UNDOCK:
               utState = states::RELEASE;
               break;
             case msg_content::CHARGE:
               utState = states::CHARGING;
               break;
             default:
               utState = states::IDLE;
               break;
+          }
+        }
         newState = utState;
         if(running){
           switch(utState){
             case states::FOLLOW_LINE:{
               // LineFollowStrategy lStrategy = determineStrategy();
               if(lStrategy == LineFollowStrategy::DOCKING){
                 utState = states::OCCUPY;
                 break;
+              }
         // Get sensor data
         int WL = global.vcnl4020[constants::DiWheelDrive::PROX_WHEEL_LEFT].getProximityScaledWoOffset();
         int WR = global.vcnl4020[constants::DiWheelDrive::PROX_WHEEL_RIGHT].getProximityScaledWoOffset();
         // int FL = global.vcnl4020[constants::DiWheelDrive::PROX_FRONT_LEFT].getProximityScaledWoOffset();
         // int FR = global.vcnl4020[constants::DiWheelDrive::PROX_FRONT_RIGHT].getProximityScaledWoOffset();
         switch(utState){
           case states::IDLE:
             if (!global.motorcontrol.getMotorEnable()){
                 global.motorcontrol.toggleMotorEnable();
+            }
             setRpmSpeed(stop);
             if(/* checkPinVoltage() && */ checkPinEnabled()){
               global.robot.requestCharging(0);
+            }
             break;
           // ---------------------------------------
           case states::FOLLOW_LINE:
           // Set correct forward speed to every strategy
             if (global.forwardSpeed != global.rpmForward[0]){
               global.forwardSpeed = global.rpmForward[0];
+            }
             if(lf.getStrategy() != lStrategy){
               lf.setStrategy(lStrategy);
               lf.followLine(rpmSpeed);
               setRpmSpeed(rpmSpeed);
               break;
+            }
             case states::OCCUPY:{
               // Get Wheel proxy sensors
               int WL = global.vcnl4020[constants::DiWheelDrive::PROX_WHEEL_LEFT].getProximityScaledWoOffset();
               int WR = global.vcnl4020[constants::DiWheelDrive::PROX_WHEEL_LEFT].getProximityScaledWoOffset();
               // Check for black
               if ((WL+WR) < proxyWheelThresh){ /*TODO: Only check for one sensor */
                 global.motorcontrol.setTargetRPM(0,0);
                 // Stop when in docking state and take further actions
                 if(lf.getStrategy() == LineFollowStrategy::DOCKING){
                   if(checkDockingSuccess(this)){
                     utState = states::CHARGING;
                     break;
                   }else{
                     correctPosition(this, lf, 250);
                     lf.setStrategy(LineFollowStrategy::DOCKING);
                     // break;
+                  }
                 }else{
                   checkForMotion(this);
                   // 180° Rotation
                   global.distcontrol.setTargetPosition(0, 3141592, 10000);
                   // BaseThread::sleep(8000);
                   checkForMotion(this);
                   correctPosition(this, lf, 250);
                   // break;
                   lf.setStrategy(LineFollowStrategy::DOCKING);
+                }
             //TODO: Check if white is detected and stop threshold is reached
             if(lf.followLine(rpmSpeed)){
               if(whiteBuf >= WHITE_COUNT_THRESH){
                 setRpmSpeed(stop);
                 newState = states::IDLE;
               }else{
                 whiteBuf++;
+              }
               // Set correct following
               // lf.setStrategy(LineFollowStrategy::EDGE_RIGHT);
               lf.followLine(rpmSpeed);
             }else{
               whiteBuf = 0;
               setRpmSpeed(rpmSpeed);
               break;
+            }
             case states::RELEASE:{
               break;
             // lf.followLine(rpmSpeed);
             // setRpmSpeed(rpmSpeed);
             break;
           // ---------------------------------------
           case states::DETECT_STATION:
             if (global.forwardSpeed != CHARGING_SPEED){
               global.forwardSpeed = CHARGING_SPEED;
+            }
             case states::CHARGING:{
             if(lf.getStrategy() != LineFollowStrategy::EDGE_RIGHT){
               lf.setStrategy(LineFollowStrategy::EDGE_RIGHT);
+            }
             lf.followLine(rpmSpeed);
             setRpmSpeed(rpmSpeed);
             // // Detect marker before docking station
             if ((WL+WR) < PROXY_WHEEL_THRESH){
               setRpmSpeed(stop);
               break;
               checkForMotion();
               // 180° Rotation
               global.distcontrol.setTargetPosition(0, ROTATION_180, ROTATION_DURATION);
               // BaseThread::sleep(8000);
               checkForMotion();
               newState = states::CORRECT_POSITIONING;
+            }
             default:{
               break;
             break;
           // ---------------------------------------
           case states::CORRECT_POSITIONING:
             if(lf.getStrategy() != LineFollowStrategy::EDGE_LEFT){
               lf.promptStrategyChange(LineFollowStrategy::EDGE_LEFT);
+            }
+          }
             lf.followLine(rpmSpeed);
             setRpmSpeed(rpmSpeed);
         }else{
           // Stop
             setRpmSpeed(stop);
+          }
             correctionStep++;
             if (correctionStep >= MAX_CORRECTION_STEPS){
               correctionStep = 0;
               newState = states::REVERSE;
               setRpmSpeed(stop);
               checkForMotion();
+            }
             break;
           // ---------------------------------------
           case states::REVERSE:
             if(lf.getStrategy() != LineFollowStrategy::REVERSE){
               lf.setStrategy(LineFollowStrategy::REVERSE);
+            }
             lf.followLine(rpmSpeed);
             setRpmSpeed(rpmSpeed);
             if ((WL+WR) < PROXY_WHEEL_THRESH){
               setRpmSpeed(stop);
               checkForMotion();
               newState = states::CHECK_POSITIONING;
+            }
             break;
           // ---------------------------------------
           case states::CHECK_POSITIONING:
             if(checkDockingSuccess()/* && checkVoltage() */){
               newState = states::CHARGING;
             }else{
               newState = states::CORRECT_POSITIONING;
+            }
             break;
           // ---------------------------------------
           case states::CHARGING:
             if (global.motorcontrol.getMotorEnable()){
               global.motorcontrol.toggleMotorEnable();
+            }
             // Formulate Request to enable charging
             if(/* checkPinVoltage() && */ !checkPinEnabled()){
               global.robot.requestCharging(1);
+            }
             if(checkPinEnabled()){
               showChargingState();
+            }
             break;
           // ---------------------------------------
           case states::RELEASE:
             if(/* checkPinVoltage() && */ checkPinEnabled()){
               global.robot.requestCharging(0);
+            }
             if(checkPinEnabled()){
               showChargingState();
             }else{
               if (!global.motorcontrol.getMotorEnable()){
                 global.motorcontrol.toggleMotorEnable();
+              }
               //Rotate -20° to free from magnet
               global.distcontrol.setTargetPosition(0, ROTATION_20, ROTATION_DURATION);
               checkForMotion();
               lStrategy = LineFollowStrategy::EDGE_RIGHT;
               newState = states::FOLLOW_LINE;
+            }
             lightAllLeds(Color::BLACK);
             break;
           default:
             break;
+          }
           utState = newState;
         this->sleep(CAN::UPDATE_PERIOD);
+      }

     #include <ch.hpp>
     #include <amiroosconf.h>
     #include <amiro/Color.h>
     // #include "global.hpp"
     // Speed when driving towards the docking station
     #define CHARGING_SPEED 5
     // Thresh to determain how much update steps should pass while alining
     #define MAX_CORRECTION_STEPS 250
     // Thresh for wheel proxy sensors, when summed values fall below the state changes
     #define PROXY_WHEEL_THRESH 18000
     // Thresh for how long (update steps) the front sensors are allowed to detect white
     #define WHITE_COUNT_THRESH 120
     // Rotation around 180 degrees in microradian
     #define ROTATION_180 3141592
     // Rotation around -20 degrees in microradian
     #define ROTATION_20 -349065
     #define ROTATION_DURATION 10000
     namespace amiro {
     class UserThread : public chibios_rt::BaseStaticThread<USER_THREAD_STACK_SIZE>
+    {
     public:
       // 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{
           FOLLOW_LINE,
           RELEASE,
           CHARGING,
           DETECT_STATION,
           CORRECT_POSITIONING,
           REVERSE,
           CHECK_POSITIONING,
           IDLE
         };
       explicit UserThread();
       virtual ~UserThread();
       virtual msg_t main();
     private:
       void setRpmSpeed(const int (&rpmSpeed)[2]);
       void lightOneLed(Color color, int idx);
       void lightAllLeds(Color color);
       /**
        * Uses light ring indicate the state of charge.
        */
       void showChargingState();
       void checkForMotion();
       bool checkPinVoltage();
       bool checkPinEnabled();
       /**
        * Check if current position changes when the wheel are deactivated.
+       *
        * When AMiRo drives towards the loading station, it stops when a specific marker is reached.
        * In order to validate that the AMiRo is correctly positioned in the loading station
        * the wheels are turned off. When the position remains the same the docking procedure
        * was successful (return 1) otherwise a correction is needed (return 0).
        */
       int checkDockingSuccess();
     };
     } // end of namespace amiro

Also available in: Unified diff