commit 98e7c69b428a0b1e222d2bb0614fea029782c025
Author: galberding <galberding@techfak.uni-bielefeld.de>
Date:   Thu Dec 12 09:17:49 2019 +0100

    Refactor

diff --git a/devices/DiWheelDrive/main.cpp b/devices/DiWheelDrive/main.cpp
index 180436e..4eea0d4 100644
--- a/devices/DiWheelDrive/main.cpp
+++ b/devices/DiWheelDrive/main.cpp
@@ -701,7 +701,7 @@ void shellRequestMotorResetGains(BaseSequentialStream *chp, int argc, char *argv
  * Note: invert the threshs to drive on the other edge.
  * 
  * */
-void calibrateLineSensores(BaseSequentialStream *chp, int argc, char *argv[]) {
+void shellRequestCalibrateLineSensores(BaseSequentialStream *chp, int argc, char *argv[]) {
     // int vcnl4020AmbientLight[4];
     int vcnl4020Proximity[4];
     int rounds = 1;
@@ -768,7 +768,7 @@ void calibrateLineSensores(BaseSequentialStream *chp, int argc, char *argv[]) {
 
 
 
-void getProxySensorData(BaseSequentialStream *chp, int argc, char *argv[]) {
+void sellRequestgetBottomSensorData(BaseSequentialStream *chp, int argc, char *argv[]) {
   // uint16_t vcnl4020AmbientLight[4];
   uint16_t vcnl4020Proximity[4];
   uint16_t rounds = 1;
@@ -780,8 +780,7 @@ void getProxySensorData(BaseSequentialStream *chp, int argc, char *argv[]) {
   } else {
     chprintf(chp, "Usage: dev_proxi_sensor_data <rounds> \n");
   }
-  global.motorcontrol.getGains(&global.motorConfigGains);
-  global.motorcontrol.setGains(&global.stopGains);
+  global.motorcontrol.setMotorEnable(false);
 
   for (int j = 0; j < rounds; j++) {
     for (int i = 0; i < 4; i++) {
@@ -801,250 +800,20 @@ void getProxySensorData(BaseSequentialStream *chp, int argc, char *argv[]) {
     // sleep(CAN::UPDATE_PERIOD);
     BaseThread::sleep(CAN::UPDATE_PERIOD);
   }
-  global.motorcontrol.setGains(&global.motorConfigGains);
+  global.motorcontrol.setMotorEnable(true);
   // chprintf(chp,"Summary: MaxDelta: %d, FL: %x, FR: %d\n", maxDelta, proxyL, proxyR);
   return;
 }
 
-// Either 0 to disable record or > 0 to enable it
-void setRecord(BaseSequentialStream *chp, int argc, char *argv[]){
-  if (argc == 1){
-    chprintf(chp, "Set recording to %d\n", atoi(argv[0]));
-    global.enableRecord = atoi(argv[0]);
-  }
-}
-
-
-void zieglerMeth2(BaseSequentialStream *chp, int argc, char *argv[]) {
-  // int vcnl4020AmbientLight[4];
-  // int vcnl4020Proximity[4];
-  int rpmSpeed[2] = {0};
-  int steps = 0;
-  // int proxyL = global.threshProxyL;
-  // int proxyR = global.threshProxyR;
-  int maxDelta = 0;
-  float KCrit = 0.0f;
-  
-  global.sensSamples = 0;
-  global.maxDist.error = 0;
-  LineFollow lf(&global);
-  int led = 0;
- 
-  if (argc == 2){
-    chprintf(chp, "KCrti %f\n", atof(argv[0]));
-    chprintf(chp, "Steps %i\n", atoi(argv[1]));
-    KCrit = atof(argv[0]);
-    steps = atoi(argv[1]);
-  } else if (argc == 3){
-    chprintf(chp, "KCrti %f\n", atof(argv[0]));
-    chprintf(chp, "Steps %i\n", atoi(argv[1]));
-    global.K_p = atof(argv[0]);
-    global.K_d = atof(argv[1]);
-    steps = atoi(argv[2]);
-    global.forwardSpeed = 15;
-    
-  }else{
-    chprintf(chp, "Usage: dev_ziegler2 <K_crit> <kd> <steps> (<speed>)");
-    return;
-  }
-  for(led=0; led<8; led++){
-        global.robot.setLightColor(led, Color(Color::BLACK));
-  }
-  chprintf((BaseSequentialStream*)&global.sercanmux1, "Recodring starts in five seconds...\n");
-  BaseThread::sleep(MS2ST(5000));
-  // global.motorcontrol.setTargetRPM(rpmSpeed[constants::DiWheelDrive::LEFT_WHEEL] * 1000000, rpmSpeed[constants::DiWheelDrive::RIGHT_WHEEL] * 1000000);
-  int checkWhite = 0;
-  int it_switch = steps / 2;
-  // lf.setStrategie(LineFollowStrategie::MIDDLE);
-  for(int s=0; s < steps; s++){
-    
-    checkWhite = lf.followLine(rpmSpeed);
-    checkWhite = 0;
-    // chprintf(chp,"S:%d,",s);
-    // if(global.threshWhite)
-    // if(s < it_switch){
-    //   lf.setStrategie(LineFollowStrategie::EDGE_LEFT);
-    //   checkWhite = lf.followLine(rpmSpeed);
-    // }else{
-    //   lf.setStrategie(LineFollowStrategie::EDGE_RIGHT);
-    //   checkWhite = lf.followLine(rpmSpeed);
-    // }
-    if(checkWhite){
-      global.motorcontrol.setTargetRPM(0,0);
-      for(led=0; led<8; led++){
-        global.robot.setLightColor(led, Color(Color::RED));
-      }
-    }else{
-      global.motorcontrol.setTargetRPM(rpmSpeed[constants::DiWheelDrive::LEFT_WHEEL] * 1000000, rpmSpeed[constants::DiWheelDrive::RIGHT_WHEEL] * 1000000);
-    }
-    
-    
-    BaseThread::sleep(CAN::UPDATE_PERIOD);
-  }
-
-  global.motorcontrol.setTargetRPM(0,0);
-}
-
-
-void followLine(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]));
-      steps = atoi(argv[0]);
-    }else if (argc == 2){
-      steps = atoi(argv[0]);
-      speed = atoi(argv[1]);
-    }else if (argc == 3){
-      steps = atoi(argv[0]);
-      speed = atoi(argv[1]);
-      strategy = atoi(argv[2]);
-    }else{
-      chprintf(chp, "Use: followLine <steps> <speed> <strategy>\n");
-      return;
-    }
-    global.forwardSpeed = speed;
-    switch (strategy)
-    {
-    case 0:
-      lf.setStrategy(amiro::LineFollowStrategy::EDGE_RIGHT);
-      break;
-    case 1:
-      lf.setStrategy(amiro::LineFollowStrategy::EDGE_LEFT);
-      break;
-    case 2:
-      lf.setStrategy(amiro::LineFollowStrategy::FUZZY);
-      break;
-    default:
-      break;
-    }
-
-
-    for(int s=0; s < steps; s++){
-    
-      checkWhite = lf.followLine(rpmSpeed);
-      if(checkWhite){
-        global.motorcontrol.setTargetRPM(0,0);
-        for(led=0; led<8; led++){
-          global.robot.setLightColor(led, Color(Color::RED));
-        }
-      }else{
-        global.motorcontrol.setTargetRPM(rpmSpeed[constants::DiWheelDrive::LEFT_WHEEL] * 1000000, rpmSpeed[constants::DiWheelDrive::RIGHT_WHEEL] * 1000000);
-      }
-      
-      BaseThread::sleep(CAN::UPDATE_PERIOD);
-  }
-
-  global.motorcontrol.setTargetRPM(0,0);
-}
-
-
-void printMove(BaseSequentialStream *chp, int argc, char *argv[]){
-
-  for (int j=0; j<global.sensSamples;j++){
-    chprintf(chp,"FL:%d,FR:%d,Delta:%d,Error:%d\n",global.senseRec[j].FL, global.senseRec[j].FR, global.senseRec[j].delta, global.senseRec[j].error);
-  }
-  chprintf(chp,"MaxDist: FL:%d,FR:%d,Delta:%d,Error:%d\n",global.maxDist.FL, global.maxDist.FR, global.maxDist.delta, global.maxDist.error);
-
-
-}
-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 setSpeed(int (&rpmSpeed)[2]){
-  global.motorcontrol.setTargetRPM(rpmSpeed[constants::DiWheelDrive::LEFT_WHEEL] * 1000000, rpmSpeed[constants::DiWheelDrive::RIGHT_WHEEL] * 1000000);
-}
-
-
-void enableCharging(){
-  global.ltc4412.enable(true);
-}
-
-void disableCharging(){
-  global.ltc4412.enable(false);
-}
-
-
-void setGlobalStrategy(BaseSequentialStream *chp, int argc, char *argv[]){
-  uint8_t strategy = 0;
-  if(argc == 1){
-    strategy = atoi(argv[0]); 
-  }
-  // send over can
-  global.strategyTest = strategy;
-  global.triggerCan = true;
-}
-
-
-
-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);
-
-  if(argc == 1){
-    for (int i=0; i<atoi(argv[0]);i++){
-      types::position pos = global.odometry.getPosition();
-      chprintf(chp, "End: Position X: %d, Y: %d,  Rotation X: %d, Y: %d, Z: %d\n", pos.x, pos.y, pos.f_x, pos.f_y, pos.f_z);
-      BaseThread::sleep(CAN::UPDATE_PERIOD);
-    }
-  }
-}
-
-// TODO: Not wokring, either loading station has no power or logic not working
-void checkPower(BaseSequentialStream *chp, int argc, char *argv[]){
+void shellRequestCheckPower(BaseSequentialStream *chp, int argc, char *argv[]){
   int steps = 2000;
   int led = 0;
-  // enableCharging();
-  // for (int i=0; i<steps;i++){
-  //   chprintf(chp, "%s Enable: %s\n", global.ltc4412.isPluggedIn() ? "y" : "n", global.ltc4412.isEnabled() ? "y2" : "n2"); 
-  //   if(global.ltc4412.isPluggedIn()){
-  //     // enableCharging();
-  //     for(led=0; led<8; led++){
-  //       global.robot.setLightColor(led, Color(Color::GREEN));
-  //     }
-  //   }else{
-  //     // disableCharging();
-  //     for(led=0; led<8; led++){
-  //       global.robot.setLightColor(led, Color(Color::RED));
-  //     }
-  //   }
-  //   BaseThread::sleep(CAN::UPDATE_PERIOD);
-  // }
-  // disableCharging();
-  // for(led=0; led<8; led++){
-  //   global.robot.setLightColor(led, Color(Color::BLACK));
-  // }
-  // chprintf((BaseSequentialStream*) &SD1, "Charging Flags: %d, State of charge: %d, Minutes: %d, Consumption: %d\n", global.powerFrame.data8[0], global.powerFrame.data8[1], global.powerFrame.data16[1], global.powerFrame.data16[2]);
+
   chprintf(chp, "Power over Pins: %s Pins Enabled: %s, Power: %d\n", global.ltc4412.isPluggedIn() ? "y" : "n", global.ltc4412.isEnabled() ? "y" : "n", global.robot.getPowerStatus().state_of_charge);
 }
 
 
-void proxyRing(BaseSequentialStream *chp, int argc, char *argv[]){
+void shellRequestProxyRingValues(BaseSequentialStream *chp, int argc, char *argv[]){
    int steps = 10000;
    int i;
 
@@ -1098,22 +867,11 @@ 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", getProxySensorData},
-  {"dev_ziegler2", zieglerMeth2},
-  // TODO: Stop user process from execution to finish/force calibration before anything starts
-  {"calibrate_line", calibrateLineSensores}, 
-  // {"record_move", recordMove},
-  {"print_record", printMove},
-  {"setRecord", setRecord},
-  {"followLine", followLine},
-  {"getPos", getPosition},
-  {"checkPower", checkPower},
-  {"setStrategy", setGlobalStrategy},
-  {"proxyRing", proxyRing},
+  {"calibrate_line_sensors", shellRequestCalibrateLineSensores},
+  {"printProxyBottom", sellRequestgetBottomSensorData},
+  {"printProxyRing", shellRequestProxyRingValues},
+  {"checkPowerPins", shellRequestCheckPower},
   {NULL, NULL}
 };
 
