commit 7f4e10f7a55c8a04657e88a47f1df0b302a288a2
Author: galberding <galberding@techfak.uni-bielefeld.de>
Date:   Tue Nov 26 08:30:07 2019 +0100

    Add debugging methods

diff --git a/devices/DiWheelDrive/main.cpp b/devices/DiWheelDrive/main.cpp
index 600b91e..180436e 100644
--- a/devices/DiWheelDrive/main.cpp
+++ b/devices/DiWheelDrive/main.cpp
@@ -17,7 +17,7 @@
 #include <chprintf.h>
 #include <shell.h>
 
-#include "linefollow2.hpp" 
+#include "linefollow.hpp" 
 
 using namespace chibios_rt;
 
@@ -768,20 +768,17 @@ void calibrateLineSensores(BaseSequentialStream *chp, int argc, char *argv[]) {
 
 
 
-void proxySensorData(BaseSequentialStream *chp, int argc, char *argv[]) {
+void getProxySensorData(BaseSequentialStream *chp, int argc, char *argv[]) {
   // uint16_t vcnl4020AmbientLight[4];
   uint16_t vcnl4020Proximity[4];
   uint16_t rounds = 1;
-  // uint16_t proxyL = global.threshProxyL;
-  // uint16_t proxyR = global.threshProxyR;
-  // uint16_t maxDelta = 0;
-  
-  // int sensorL = 0;
   // int sensorR = 0;
   if (argc == 1){
     chprintf(chp, "Test %i rounds \n", atoi(argv[0]));
     rounds = atoi(argv[0]);
     
+  } else {
+    chprintf(chp, "Usage: dev_proxi_sensor_data <rounds> \n");
   }
   global.motorcontrol.getGains(&global.motorConfigGains);
   global.motorcontrol.setGains(&global.stopGains);
@@ -794,16 +791,6 @@ void proxySensorData(BaseSequentialStream *chp, int argc, char *argv[]) {
     
     uint16_t delta = (vcnl4020Proximity[constants::DiWheelDrive::PROX_FRONT_LEFT]
                   - vcnl4020Proximity[constants::DiWheelDrive::PROX_FRONT_RIGHT]);
-    // // Update proximity thresh
-    // if (delta > maxDelta) {
-    //   maxDelta = delta;
-    //   proxyL = vcnl4020Proximity[constants::DiWheelDrive::PROX_FRONT_LEFT];
-    //   proxyR = vcnl4020Proximity[constants::DiWheelDrive::PROX_FRONT_RIGHT];
-    // }
-
-    // if (vcnl4020Proximity[constants::DiWheelDrive::PROX_FRONT_RIGHT] > proxyR && vcnl4020Proximity[constants::DiWheelDrive::PROX_FRONT_LEFT] > proxyL ){
-    //   delta *= -1;
-    // }
 
     chprintf(chp,"WL:%d,FL:%d,FR:%d,WR:%d,Delta:%d\n", 
                   vcnl4020Proximity[constants::DiWheelDrive::PROX_WHEEL_LEFT],
@@ -837,6 +824,7 @@ void zieglerMeth2(BaseSequentialStream *chp, int argc, char *argv[]) {
   // int proxyR = global.threshProxyR;
   int maxDelta = 0;
   float KCrit = 0.0f;
+  
   global.sensSamples = 0;
   global.maxDist.error = 0;
   LineFollow lf(&global);
@@ -850,15 +838,15 @@ void zieglerMeth2(BaseSequentialStream *chp, int argc, char *argv[]) {
   } else if (argc == 3){
     chprintf(chp, "KCrti %f\n", atof(argv[0]));
     chprintf(chp, "Steps %i\n", atoi(argv[1]));
-    KCrit = atof(argv[0]);
-    steps = atoi(argv[1]);
-    global.forwardSpeed = atoi(argv[2]);
+    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> <steps> (<speed>)");
+    chprintf(chp, "Usage: dev_ziegler2 <K_crit> <kd> <steps> (<speed>)");
     return;
   }
-  global.K_p = KCrit;
   for(led=0; led<8; led++){
         global.robot.setLightColor(led, Color(Color::BLACK));
   }
@@ -871,6 +859,7 @@ void zieglerMeth2(BaseSequentialStream *chp, int argc, char *argv[]) {
   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){
@@ -986,179 +975,10 @@ void motorToggle(BaseSequentialStream *chp, int argc, char *argv[]){
 }
 
 
-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);
 }
 
-/**
- * Blocks as long as the position changes.
- */
-void checkForMotion(){
-  int motion = 1;
-  int led = 0;
-  types::position oldPos = global.odometry.getPosition();
-  while(motion){
-    BaseThread::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;
-      global.robot.setLightColor((led + 1) % 8, Color(Color::YELLOW));
-      global.robot.setLightColor(led % 8, Color(Color::BLACK));
-      led++;
-  }
-}
-
-/**
- * Turns MotorControl off and checks if position remains stable.
- */
-int checkDockingSuccess(){
-  // global.motorcontrol.setTargetRPM(0,0);
-  checkForMotion();
-  int led = 0;
-  int success = 0;
-  global.odometry.resetPosition();
-  types::position start = global.startPos = global.odometry.getPosition();
-  global.motorcontrol.toggleMotorEnable();
-  BaseThread::sleep(1000);
-  types::position stop = global.endPos = global.odometry.getPosition();
-  global.motorcontrol.toggleMotorEnable();
-  // 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));
-    }
-    success = 0;
-  }else{
-    for(led=0; led<8; led++){
-      global.robot.setLightColor(led, Color(Color::GREEN));
-    }
-    success = 1;
-  }
-  
-  BaseThread::sleep(500);
-  for(led=0; led<8; led++){
-          global.robot.setLightColor(led, Color(Color::BLACK));
-  }
-  return success;
-}
-
-/**
- * Switch to EDGE_LEFT line following and orientate for n steps along the line.
- * 
- * @param lf  current line following
- * @param steps steps to take for the correction (measured in CAN-update durations)
- */
-void correctPosition(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);
-    setSpeed(rpmSpeed);
-    BaseThread::sleep(CAN::UPDATE_PERIOD);
-  }
-}
-
-
-
-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 = 20000;
-  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;
-    
-    setSpeed(rpmSpeed);
-
-
-    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){
-        global.motorcontrol.setTargetRPM(0,0);
-        chprintf((BaseSequentialStream*)&global.sercanmux1, "Hit Break!\n");
-        if(lf.getStrategy() == LineFollowStrategy::DOCKING){
-          // Check if Docking was successful
-          if(checkDockingSuccess()){
-            break;
-          }else{
-            correctPosition(lf, 250);
-            lf.setStrategy(LineFollowStrategy::DOCKING);
-            // break;
-          }
-          
-        }else{
-          // global.motorcontrol.setTargetRPM(0,0);
-          BaseThread::sleep(1000);
-          // 180° Rotation 
-          global.distcontrol.setTargetPosition(0, 3141592, 10000);
-          // BaseThread::sleep(8000);
-          checkForMotion();
-          correctPosition(lf, 250);
-          // break;
-          lf.setStrategy(LineFollowStrategy::DOCKING);
-          
-        }
-      }
-      checkWhite = lf.followLine(rpmSpeed);
-      setSpeed(rpmSpeed);
-      BaseThread::sleep(CAN::UPDATE_PERIOD);
-  }
-
-  
-}
 
 void enableCharging(){
   global.ltc4412.enable(true);
@@ -1199,26 +1019,57 @@ void getPosition(BaseSequentialStream *chp, int argc, char *argv[]){
 void checkPower(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));
-      }
+  // 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[]){
+   int steps = 10000;
+   int i;
+
+  uint16_t prox[8];
+  uint32_t prox_sum = 0;
+
+  if (argc == 1){
+      chprintf(chp, "%i steps \n", atoi(argv[0]));
+      steps = atoi(argv[0]);
     }else{
-      // disableCharging();
-      for(led=0; led<8; led++){
-        global.robot.setLightColor(led, Color(Color::RED));
-      }
+      chprintf(chp, "Usage: proxyRing <steps> \n");
+    }
+  for (int j=0; j<steps; j++){
+    prox_sum = 0;
+    for(i=0; i<8;i++){
+      prox[i] = global.robot.getProximityRingValue(i);
+      prox_sum += prox[i];
     }
+    // uint16_t notouch = 100;
+    // uint16_t toucht = 20031;
+    // sign = 
+    // i = 0;
+    chprintf(chp, "0:%i 1:%i 2:%i 3:%i 4:%i 5:%i 6:%i 7:%i prox_sum:%i \n", prox[0], prox[1], prox[2], prox[3], prox[4], prox[5], prox[6], prox[7], prox_sum);
     BaseThread::sleep(CAN::UPDATE_PERIOD);
   }
-  disableCharging();
-  for(led=0; led<8; led++){
-    global.robot.setLightColor(led, Color(Color::BLACK));
-  }
 }
 
 
@@ -1251,7 +1102,7 @@ static const ShellCommand commands[] = {
   {"motor_activate", setGains},
   {"motor_resetGains", shellRequestMotorResetGains},
   {"motorToggle", motorToggle},
-  {"dev_proxi_sensor_data", proxySensorData},
+  {"dev_proxi_sensor_data", getProxySensorData},
   {"dev_ziegler2", zieglerMeth2},
   // TODO: Stop user process from execution to finish/force calibration before anything starts
   {"calibrate_line", calibrateLineSensores}, 
@@ -1259,11 +1110,10 @@ static const ShellCommand commands[] = {
   {"print_record", printMove},
   {"setRecord", setRecord},
   {"followLine", followLine},
-  {"rotate", rotate},
-  {"followRotate", followAndRotate},
   {"getPos", getPosition},
   {"checkPower", checkPower},
   {"setStrategy", setGlobalStrategy},
+  {"proxyRing", proxyRing},
   {NULL, NULL}
 };
 
