commit 10bf9cc0dd224ec150da9a1b69fa70d860b3fdc4
Author: galberding <galberding@techfak.uni-bielefeld.de>
Date:   Tue Feb 18 17:08:39 2020 +0100

    Introducing error states (negative lable): DOCKING_ERROR, REVERSE_TIMEOUT_ERROR, CALIBRATION_ERROR, WHITE_DETECTION_ERROR, PROXY_DETECTION_ERROR, NO_CHARGING_POWER_ERROR (not in use), UNKNOWN_STATE_ERROR.
    Bootom sensors can be calibrated for specific colors with MSG_CALIBRATE_BLACK/WHITE,
    Add new object detection system that reduces the speed when frontal objects are detected and stops at defined distance.

diff --git a/devices/DiWheelDrive/userthread.hpp b/devices/DiWheelDrive/userthread.hpp
index 8ef209c..7e88b09 100644
--- a/devices/DiWheelDrive/userthread.hpp
+++ b/devices/DiWheelDrive/userthread.hpp
@@ -4,7 +4,9 @@
 #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
@@ -12,16 +14,24 @@
 #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
@@ -31,7 +41,9 @@
 #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 {
 
@@ -41,19 +53,9 @@ class UserThread : public chibios_rt::BaseStaticThread<USER_THREAD_STACK_SIZE>
 
   // 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,
@@ -65,22 +67,50 @@ class UserThread : public chibios_rt::BaseStaticThread<USER_THREAD_STACK_SIZE>
       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();
@@ -100,6 +130,19 @@ private:
   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
@@ -108,9 +151,6 @@ private:
    */
   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.
    * 
@@ -120,6 +160,18 @@ private:
    * 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
