commit 84b4c6326579b9f27a43d4bb38c1ae373a450bef
Author: galberding <galberding@techfak.uni-bielefeld.de>
Date:   Thu Apr 2 15:16:56 2020 +0200

    Add docking correction procedure

diff --git a/devices/DiWheelDrive/main.cpp b/devices/DiWheelDrive/main.cpp
index 741d30f..bf7cacc 100644
--- a/devices/DiWheelDrive/main.cpp
+++ b/devices/DiWheelDrive/main.cpp
@@ -812,11 +812,27 @@ void shellRequestCheckPower(BaseSequentialStream *chp, int argc, char *argv[]){
   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);
 }
 
+#define window  5
+int32_t counter = 0;
+int32_t proxbuf[window]= { 0 };
+int32_t meanDeviation(uint16_t a, uint16_t b){
+  int32_t diff = a-b;
+  int32_t res = 0; 
+  proxbuf[counter] = (diff*100)/((a+b)/2);
+  for (int i = 0; i< window; i++){
+    res += proxbuf[i];
+  }
+  counter++;
+  counter = counter % window;
+  return res / window;
+}
+
 
 void shellRequestProxyRingValues(BaseSequentialStream *chp, int argc, char *argv[]){
    int steps = 100;
    int i;
-
+   int16_t old3 = 0;
+   int16_t old4 = 0;
   uint16_t prox[8];
   uint32_t prox_sum = 0;
 
@@ -828,18 +844,69 @@ void shellRequestProxyRingValues(BaseSequentialStream *chp, int argc, char *argv
     }
   for (int j=0; j<steps; j++){
     prox_sum = 0;
+    old3 = prox[3];
+    old4 = prox[4];
     for(i=0; i<8;i++){
       prox[i] = global.robot.getProximityRingValue(i);
       prox_sum += prox[i];
     }
+    int32_t deviation = meanDeviation(prox[3] & 0xFFF0  , prox[4] & 0xFFF0);
+    // int32_t deviation = meanDeviation((prox[3]+old3) / 2  , (prox[4]+old4) / 2);
+    // 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 Deviation:%i \n", prox[0], prox[1], prox[2], prox[3], prox[4], prox[5], prox[6], prox[7], deviation);
+    BaseThread::sleep(CAN::UPDATE_PERIOD);
+  }
+}
+int buf[10][3] = { 0 };
+void shellRequestMagnetoMeter(BaseSequentialStream *chp, int argc, char *argv[]){
+   int steps = 10;
+  //  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{
+      chprintf(chp, "Usage: proxyRing <steps> \n");
+    }
+  chprintf((BaseSequentialStream*)&global.sercanmux1, "motor calibration starts in five seconds...\n");
+  BaseThread::sleep(MS2ST(5000));
+  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);
+
+    if (j < 10){
+      buf[j][0] = global.hmc5883l.getMagnetizationGauss(0x00u);
+      buf[j][1] = global.hmc5883l.getMagnetizationGauss(0x02u);
+      buf[j][2] = global.hmc5883l.getMagnetizationGauss(0x01u);
+    }
+    chprintf(chp, "X:%i Y:%i Z:%i\n", global.hmc5883l.getMagnetizationGauss(0x00u), global.hmc5883l.getMagnetizationGauss(0x02u), global.hmc5883l.getMagnetizationGauss(0x01u));
     BaseThread::sleep(CAN::UPDATE_PERIOD);
   }
 }
+void shellRequestMagnetoMeterPrint(BaseSequentialStream *chp, int argc, char *argv[]){
+  for (int j=0; j<10; j++){
+    chprintf(chp, "X:%i Y:%i Z:%i\n", buf[j][0], buf[j][1], buf[j][2]);
+    BaseThread::sleep(CAN::UPDATE_PERIOD);
+  }
+}
+
+void shellRequestPrintCoordinate(BaseSequentialStream *chp, int argc, char *argv[]){
+  types::position oldPos = global.odometry.getPosition();
+  chprintf(chp, "X:%i Y:%i\n",oldPos.x, oldPos.y);
+}
 
 static const ShellCommand commands[] = {
   {"shutdown", shellRequestShutdown},
@@ -870,6 +937,9 @@ static const ShellCommand commands[] = {
   {"calibrate_line_sensors", shellRequestCalibrateLineSensores},
   {"printProxyBottom", sellRequestgetBottomSensorData},
   {"printProxyRing", shellRequestProxyRingValues},
+  {"printMagnetometer", shellRequestMagnetoMeter},
+  {"printMagnetometerRes", shellRequestMagnetoMeterPrint},
+  {"printLocation", shellRequestPrintCoordinate},
   {"checkPowerPins", shellRequestCheckPower},
   {NULL, NULL}
 };
