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/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}
 };
 
