commit bfffb0bdf024908aeb5d393a44271817260d8adf
Author: galberding <galberding@techfak.uni-bielefeld.de>
Date:   Tue Oct 15 16:24:39 2019 +0200

    Implement driving towards loading station

diff --git a/devices/DiWheelDrive/global.hpp b/devices/DiWheelDrive/global.hpp
index e9b76a9..d47ae96 100644
--- a/devices/DiWheelDrive/global.hpp
+++ b/devices/DiWheelDrive/global.hpp
@@ -187,9 +187,20 @@ public:
   int accumHist = 0;
   int oldError = 0;
 
+// Struct for saving motor gains
+
+int resetProtect = 1;
+motorGains motorConfigGains;
+motorGains stopGains;
+
+types::position startPos;
+types::position endPos;
+
 // Debugging stuff --------------
   int forwardSpeed = 10;
-  int enableRecord = 1;
+  int enableRecord = 0;
+
+  
 
   // Buffer for sensor values
   struct sensorRecord
diff --git a/devices/DiWheelDrive/linefollow2.cpp b/devices/DiWheelDrive/linefollow2.cpp
index 44b4682..50e0ae4 100644
--- a/devices/DiWheelDrive/linefollow2.cpp
+++ b/devices/DiWheelDrive/linefollow2.cpp
@@ -24,7 +24,7 @@ int LineFollow::getError(){
     int error = 0;
     switch (this->strategy)
     {
-    case LineFollowStrategy::EDGE_RIGHT:
+    case LineFollowStrategy::EDGE_RIGHT: case LineFollowStrategy::DOCKING:
         error = -(FL -targetL + FR - targetR);
         break;
     case LineFollowStrategy::EDGE_LEFT:
@@ -57,28 +57,35 @@ int LineFollow::getError(){
 }
 
 int LineFollow::followLine(int (&rpmSpeed)[2]){
-
+    int correctionSpeed = 0;
     switch (this->strategy)
     {
     case LineFollowStrategy::FUZZY:
-        for (int i = 0; i < 4; i++) {
-                vcnl4020AmbientLight[i] = global->vcnl4020[i].getAmbientLight();
-                vcnl4020Proximity[i] = global->vcnl4020[i].getProximityScaledWoOffset();
-            }
+      for (int i = 0; i < 4; i++) {
+          vcnl4020AmbientLight[i] = global->vcnl4020[i].getAmbientLight();
+          vcnl4020Proximity[i] = global->vcnl4020[i].getProximityScaledWoOffset();
+      }
+      lineFollowing(vcnl4020Proximity, rpmSpeed);
+      break;
+
+    case LineFollowStrategy::DOCKING:
+      correctionSpeed = -getPidCorrectionSpeed();
+      rpmSpeed[constants::DiWheelDrive::LEFT_WHEEL] = -global->forwardSpeed;
+
+      rpmSpeed[constants::DiWheelDrive::RIGHT_WHEEL] = -global->forwardSpeed;
+      
+      break;
 
-        lineFollowing(vcnl4020Proximity, rpmSpeed);
-        break;
-    
     default:
-        int correctionSpeed = getPidCorrectionSpeed();
-        // chprintf((BaseSequentialStream*) &SD1, "Correction: %d, thresh: %d\n",correctionSpeed,  global->threshWhite);
+      correctionSpeed = getPidCorrectionSpeed();
+      // chprintf((BaseSequentialStream*) &SD1, "Correction: %d, thresh: %d\n",correctionSpeed,  global->threshWhite);
 
-        rpmSpeed[constants::DiWheelDrive::LEFT_WHEEL] =   global->forwardSpeed + correctionSpeed;
+      rpmSpeed[constants::DiWheelDrive::LEFT_WHEEL] =   global->forwardSpeed + correctionSpeed;
 
-        rpmSpeed[constants::DiWheelDrive::RIGHT_WHEEL] = global->forwardSpeed - correctionSpeed;
-        return whiteFlag;
-        break;
+      rpmSpeed[constants::DiWheelDrive::RIGHT_WHEEL] = global->forwardSpeed - correctionSpeed;
+      break;
     }
+    return whiteFlag;
 }
 
 
diff --git a/devices/DiWheelDrive/linefollow2.hpp b/devices/DiWheelDrive/linefollow2.hpp
index 5bf878c..d9c5493 100644
--- a/devices/DiWheelDrive/linefollow2.hpp
+++ b/devices/DiWheelDrive/linefollow2.hpp
@@ -9,6 +9,7 @@ namespace amiro {
   enum class LineFollowStrategy{
   EDGE_LEFT,
   EDGE_RIGHT,
+  DOCKING,
   MIDDLE,
   FUZZY
 };
@@ -73,7 +74,7 @@ public:
 
     char whiteFlag = 0;
     LineFollowStrategy strategy = LineFollowStrategy::EDGE_RIGHT;
-    float Kp = 0.003;
+    float Kp = 0.002;
     float Ki = 0;
     float Kd = 0;
     int accumHist = 0;
diff --git a/devices/DiWheelDrive/main.cpp b/devices/DiWheelDrive/main.cpp
index a4f54cf..2f3c15a 100644
--- a/devices/DiWheelDrive/main.cpp
+++ b/devices/DiWheelDrive/main.cpp
@@ -783,7 +783,8 @@ void proxySensorData(BaseSequentialStream *chp, int argc, char *argv[]) {
     rounds = atoi(argv[0]);
     
   }
- 
+  global.motorcontrol.getGains(&global.motorConfigGains);
+  global.motorcontrol.setGains(&global.stopGains);
 
   for (int j = 0; j < rounds; j++) {
     for (int i = 0; i < 4; i++) {
@@ -813,6 +814,7 @@ void proxySensorData(BaseSequentialStream *chp, int argc, char *argv[]) {
     // sleep(CAN::UPDATE_PERIOD);
     BaseThread::sleep(CAN::UPDATE_PERIOD);
   }
+  global.motorcontrol.setGains(&global.motorConfigGains);
   // chprintf(chp,"Summary: MaxDelta: %d, FL: %x, FR: %d\n", maxDelta, proxyL, proxyR);
   return;
 }
@@ -962,7 +964,173 @@ void printMove(BaseSequentialStream *chp, int argc, char *argv[]){
 
 
 }
+void freeGains(BaseSequentialStream *chp, int argc, char *argv[]){
+  if (global.resetProtect){
+    global.motorcontrol.getGains(&global.motorConfigGains);
+    global.resetProtect = 0;
+  }
+  
+  global.motorcontrol.setGains(&global.stopGains);
+}
+
+
+void setGains(BaseSequentialStream *chp, int argc, char *argv[]){
+  if(!global.resetProtect){
+    global.motorcontrol.setGains(&global.motorConfigGains);
+    global.resetProtect = 1;
+  }
+}
+void motorToggle(BaseSequentialStream *chp, int argc, char *argv[]){
+  global.odometry.resetPosition();
+  global.motorcontrol.toggleMotorEnable();
+}
+
+
+void rotate(BaseSequentialStream *chp, int argc, char *argv[]){
+  int steps = 1000;
+  int speed = 0;
+  int strategy = 0;
+  int led = 0;
+  int checkWhite = 0;
+  int rpmSpeed[2] = {0};
+  LineFollow lf(&global);
+  if (argc == 1){
+      chprintf(chp, "%i steps \n", atoi(argv[0]));
+      speed = atoi(argv[0]);
+    }else if (argc == 2){
+      speed = atoi(argv[0]);
+      steps = atoi(argv[1]);
+    }else{
+      chprintf(chp, "Use: rotate <speed> <steps>\n");
+      return;
+    }
+    chprintf((BaseSequentialStream*)&global.sercanmux1, "Recodring starts in five seconds...\n");
+    BaseThread::sleep(MS2ST(5000));
+    // global.forwardSpeed = speed;
+  //   rpmSpeed[0] = -speed;
+  //   rpmSpeed[1] = speed;
+  //   global.motorcontrol.setTargetRPM(rpmSpeed[constants::DiWheelDrive::LEFT_WHEEL] * 1000000, rpmSpeed[constants::DiWheelDrive::RIGHT_WHEEL] * 1000000);
+  //   for(int s=0; s < steps; s++){
+  //     BaseThread::sleep(CAN::UPDATE_PERIOD);
+  // }
+  global.odometry.resetPosition();
+  global.startPos = global.odometry.getPosition();
+  global.distcontrol.setTargetPosition(0, 3141592, 5000);
+  BaseThread::sleep(MS2ST(11000));
+  global.endPos = global.odometry.getPosition();
+  // global.motorcontrol.setTargetRPM(0,0);
+}
+
+void setSpeed(int (&rpmSpeed)[2]){
+  global.motorcontrol.setTargetRPM(rpmSpeed[constants::DiWheelDrive::LEFT_WHEEL] * 1000000, rpmSpeed[constants::DiWheelDrive::RIGHT_WHEEL] * 1000000);
+}
+
+void followAndRotate(BaseSequentialStream *chp, int argc, char *argv[]){
+  int steps = 10000;
+  int speed = 0;
+  int strategy = 0;
+  int led = 0;
+  int checkWhite = 0;
+  int rpmSpeed[2] = {0};
+  int proxyWheelThresh = 13000;
+  LineFollow lf(&global);
+  if (argc == 1){
+      chprintf(chp, "%i steps \n", atoi(argv[0]));
+      speed = atoi(argv[0]);
+    }else if (argc == 2){
+      speed = atoi(argv[0]);
+      steps = atoi(argv[1]);
+    }else{
+      chprintf(chp, "Use: rotate <speed> <steps>\n");
+      return;
+    }
+    chprintf((BaseSequentialStream*)&global.sercanmux1, "Recodring starts in five seconds...\n");
+    BaseThread::sleep(MS2ST(5000));
+    global.forwardSpeed = speed;
+    
+    // rpmSpeed[0] = -speed;
+    // rpmSpeed[1] = speed;
+    setSpeed(rpmSpeed);
+    // lf.setStrategy(LineFollowStrategy::DOCKING);
+    for(int s=0; s < steps; s++){
+
+      int WL = global.vcnl4020[constants::DiWheelDrive::PROX_WHEEL_LEFT].getProximityScaledWoOffset();
+      int WR = global.vcnl4020[constants::DiWheelDrive::PROX_WHEEL_LEFT].getProximityScaledWoOffset();
+      if ((WL+WR) < proxyWheelThresh){
+        chprintf((BaseSequentialStream*)&global.sercanmux1, "Hit Break!\n");
+        if(lf.getStrategy() == LineFollowStrategy::DOCKING){
+          break;
+        }else{
+          global.motorcontrol.setTargetRPM(0,0);
+          BaseThread::sleep(1000);
+          global.distcontrol.setTargetPosition(0, 3141592, 10000);
+          BaseThread::sleep(10000);
+          lf.setStrategy(LineFollowStrategy::EDGE_LEFT);
+          for (int correction=0; correction<300; correction++){
+            checkWhite = lf.followLine(rpmSpeed);
+            setSpeed(rpmSpeed);
+            BaseThread::sleep(CAN::UPDATE_PERIOD);
+          }
+          // break;
+          lf.setStrategy(LineFollowStrategy::DOCKING);
+          
+        }
+      }
+      checkWhite = lf.followLine(rpmSpeed);
+      setSpeed(rpmSpeed);
+      BaseThread::sleep(CAN::UPDATE_PERIOD);
+  }
+
+  global.motorcontrol.setTargetRPM(0,0);
+  global.odometry.resetPosition();
+  global.startPos = global.odometry.getPosition();
+  global.motorcontrol.toggleMotorEnable();
+  BaseThread::sleep(3000);
+  global.endPos = global.odometry.getPosition();
+  global.motorcontrol.toggleMotorEnable();
+}
+
+
+void getAccel(BaseSequentialStream *chp, int argc, char *argv[]){
+  int steps = 0;
+  // global.motorcontrol.getGains(&global.motorConfigGains);
+  // global.motorcontrol.setGains(&global.stopGains);
+  int16_t time = 10000;
+  int32_t angle = 3141592;
+  int32_t distance = 0;
+  if (argc == 1){
+    chprintf(chp, "%i steps \n", atoi(argv[0]));
+    steps = atoi(argv[0]);
+  }
+  else if(argc == 3){
+    distance = atoi(argv[0]);
+    angle = atoi(argv[0]);
+    time = atoi(argv[0]);
+  }else{
+    chprintf(chp, "Use: accel <steps>\n");
+    return;
+  }
+  global.distcontrol.setTargetPosition(distance, angle, time);
+
+  for(int i=0; i<steps; i++){
+    int16_t Z = global.lis331dlh.getAccelerationForce(LIS331DLH::AXIS_Z);
+    int16_t X = global.lis331dlh.getAccelerationForce(LIS331DLH::AXIS_X);
+    int16_t Y = global.lis331dlh.getAccelerationForce(LIS331DLH::AXIS_Y);
+    types::position pos = global.odometry.getPosition();
+    // chprintf((BaseSequentialStream*)&global.sercanmux1, "Axis X: %d, Y: %d, Z: %d\n", X, Y, Z);
+    chprintf((BaseSequentialStream*)&global.sercanmux1, "Position X: %d, Y: %d,  Rotation X: %d, Y: %d, Z: %d, Angle: %d\n", pos.x, pos.y, pos.f_x, pos.f_y, pos.f_z, global.distcontrol.getCurrentTargetAngle());
+    // chprintf((BaseSequentialStream*)&global.sercanmux1, "Position X: %d, Y: %d, Z: %d\n", pos.f_x, pos.f_y, pos.f_z);
+    BaseThread::sleep(CAN::UPDATE_PERIOD);
+  }
+
+  // global.motorcontrol.setGains(&global.motorConfigGains);
+}
 
+void getPosition(BaseSequentialStream *chp, int argc, char *argv[]){
+  types::position pos = global.odometry.getPosition();
+  chprintf(chp, "Start: Position X: %d, Y: %d,  Rotation X: %d, Y: %d, Z: %d\n", global.startPos.x, global.startPos.y, global.startPos.f_x, global.startPos.f_y, global.startPos.f_z);
+  chprintf(chp, "End: Position X: %d, Y: %d,  Rotation X: %d, Y: %d, Z: %d\n", global.endPos.x, global.endPos.y, global.endPos.f_x, global.endPos.f_y, global.endPos.f_z);
+}
 
 static const ShellCommand commands[] = {
   {"shutdown", shellRequestShutdown},
@@ -989,7 +1157,10 @@ static const ShellCommand commands[] = {
   {"motor_stop", shellRequestMotorStop},
   {"motor_calibrate", shellRequestMotorCalibrate},
   {"motor_getGains", shellRequestMotorGetGains},
+  {"motor_deactivate", freeGains},
+  {"motor_activate", setGains},
   {"motor_resetGains", shellRequestMotorResetGains},
+  {"motorToggle", motorToggle},
   {"dev_proxi_sensor_data", proxySensorData},
   {"dev_ziegler2", zieglerMeth2},
   // TODO: Stop user process from execution to finish/force calibration before anything starts
@@ -998,6 +1169,10 @@ static const ShellCommand commands[] = {
   {"print_record", printMove},
   {"setRecord", setRecord},
   {"followLine", followLine},
+  {"rotate", rotate},
+  {"followRotate", followAndRotate},
+  {"accel", getAccel},
+  {"getPos", getPosition},
   {NULL, NULL}
 };
 
diff --git a/devices/DiWheelDrive/userthread.cpp b/devices/DiWheelDrive/userthread.cpp
index 5be3fb8..682b9f9 100644
--- a/devices/DiWheelDrive/userthread.cpp
+++ b/devices/DiWheelDrive/userthread.cpp
@@ -56,39 +56,7 @@ bool running;
 const int sizeOfPolicy = sizeof(policy) / sizeof(states);
 int policyCounter = 0; // Do not change this, it points to the beginning of the policy
 
-// Different speed settings (all values in "rounds per minute")
-// const int rpmTurnLeft[2] = {-10, 10};
-// const int rpmTurnRight[2] = {rpmTurnLeft[1],rpmTurnLeft[0]};
-// const int rpmHalt[2] = {0, 0};
-
-// Definition of the fuzzyfication function
-//  | Membership
-// 1|_B__   G    __W__
-//  |    \  /\  /
-//  |     \/  \/
-//  |_____/\__/\______ Sensor values
-// SEE MATLAB SCRIPT "fuzzyRule.m" for adjusting the values
-// All values are "raw sensor values"
-/* Use these values for white ground surface (e.g. paper) */
-
-// const int blackStartFalling = 0x1000; // Where the black curve starts falling
-// const int blackOff = 0x1800; // Where no more black is detected
-// const int whiteStartRising = 0x2800; // Where the white curve starts rising
-// const int whiteOn = 0x6000; // Where the white curve has reached the maximum value
-// const int greyMax = (whiteOn + blackStartFalling) / 2; // Where grey has its maximum
-// const int greyStartRising = blackStartFalling; // Where grey starts rising
-// const int greyOff = whiteOn; // Where grey is completely off again
-
-/* Use these values for gray ground surfaces */
-/*
-const int blackStartFalling = 0x1000; // Where the black curve starts falling
-const int blackOff = 0x2800; // Where no more black is detected
-const int whiteStartRising = 0x4000; // Where the white curve starts rising
-const int whiteOn = 0x5000; // Where the white curve starts rising
-const int greyMax = (whiteOn + blackStartFalling) / 2; // Where grey has its maximum
-const int greyStartRising = blackStartFalling; // Where grey starts rising
-const int greyOff = whiteOn; // Where grey is completely off again
-*/
+
 
 int vcnl4020AmbientLight[4] = {0};
 int vcnl4020Proximity[4] = {0};
@@ -206,6 +174,8 @@ UserThread::main()
                 vcnl4020AmbientLight[i] = global.vcnl4020[i].getAmbientLight();
                 vcnl4020Proximity[i] = global.vcnl4020[i].getProximityScaledWoOffset();
             }
+
+
 			// lf.stableFollow(vcnl4020Proximity, rpmFuzzyCtrl, &global);
             // chprintf((BaseSequentialStream*) &SD1, "0x%04X 0x%04X 0x%04X 0x%04X\n",
             //         vcnl4020Proximity[constants::DiWheelDrive::PROX_WHEEL_LEFT],
@@ -216,6 +186,7 @@ UserThread::main()
 	//   lineFollownew
 	//else
             // lineFollowing(vcnl4020Proximity, rpmFuzzyCtrl, &global);
+			lf.followLine(rpmFuzzyCtrl);
             setRpmSpeed(rpmFuzzyCtrl);
         }
 
