commit b8b3a9c95cd419ebf1c47826cab94f6804cccf88
Author: galberding <galberding@techfak.uni-bielefeld.de>
Date:   Sun Jun 7 11:11:42 2020 +0200

    Add basic test framework for amiro map behavior

diff --git a/devices/DiWheelDrive/amiro_map.cpp b/devices/DiWheelDrive/amiro_map.cpp
index b017f98..fa09ce5 100644
--- a/devices/DiWheelDrive/amiro_map.cpp
+++ b/devices/DiWheelDrive/amiro_map.cpp
@@ -1,37 +1,40 @@
 #include "amiro_map.hpp"
 
 
-void AmiroMap::initialize(int8_t (&config)[MAX_NODES][NODE_ATTRIBUTES]){
+void AmiroMap::initialize(uint8_t (&config)[MAX_NODES][NODE_ATTRIBUTES]){
 
-  uint8_t nodeCount = 0;
-  bool end = false;
+  // Clear old values in case the map is initialized again
+  this->current = 0;
+  this->next = 0;
+  this->valid = false;
 
+  // convert proto map to internal representation
   for (int i=0; i<MAX_NODES; i++){
-    if(config[i][NODE_ATTRIBUTES - 1] == 255){
-      end = true;
+    if(config[i][2] == 0xff && i != 0){
+      this->valid = true;
       break;
+    } else if (config[i][2] == 0xff && i == 0) {
+      this->valid = false;
+      return;
     }
-    nodeCount++;
-  }
-  if (!end || nodeCount == 0 ){
-
-    // Failed to determine the amount of nodes
-    return;
-  }
 
-  this->nodeList = (struct node*) malloc(nodeCount * sizeof(struct node)); 
+    //look for start node (default is Node 0)
+    if (config[i][2] == 1 ) {
+      this->current= i;
+    }
 
-  
+    this->nodeList[i].id = i;
+    this->nodeList[i].left = config[i][0];
+    this->nodeList[i].right = config[i][1];
+    this->nodeList[i].flag = config[i][2];
+    this->nodeCount++;
+  }
 
-}
+  // TODO make validity check
 
 
-void AmiroMap::update(){
-  
 }
 
+void AmiroMap::update(bool left, bool right, LineFollowStrategy strategy) {
 
-void AmiroMap::uninitialize(){
-  
 }
-
diff --git a/devices/DiWheelDrive/amiro_map.hpp b/devices/DiWheelDrive/amiro_map.hpp
index 5f8ce93..00441e7 100644
--- a/devices/DiWheelDrive/amiro_map.hpp
+++ b/devices/DiWheelDrive/amiro_map.hpp
@@ -1,40 +1,79 @@
 #ifndef AMIRO_MAP
 #define AMIRO_MAP
 
-#include <ch.hpp>
 #include "global.hpp"
+#include "linefollow.hpp"
 #include <amiroosconf.h>
+#include <ch.hpp>
+#include <cstdint>
 
+#define MAX_NODES 10
+#define NODE_ATTRIBUTES 3
 
-#define MAX_NODES 20
-#define NODE_ATTRIBUTES 4
+namespace amiro {
 
+struct node {
+  uint8_t id;
+  uint8_t flag;
+  uint8_t left;
+  uint8_t right;
+  types::position pL; // Left
+  types::position pR; // Right
+  types::position pB; // Back
+};
 
-namespace amiro {
+class AmiroMap {
+public:
+  uint8_t get_next()  { return next; }
 
+  void set_next( uint8_t next) { this->next = next; }
 
-  struct node{
-    uint8_t id;
-    uint8_t flag;
-    types::position pL; //Left
-    types::position pR; //Right
-    types::position pB; //Back
-  };
-
-  class AmiroMap{
-  public:
-    AmiroMap(Global *global):global{global}{}
-    void initialize(int8_t (&config)[MAX_NODES][NODE_ATTRIBUTES]);
-    void update();
-  private:
-    Global *global;
-    bool valid = false;
-    node *nodeList = NULL;
-    node *current = NULL;
-    node *next = NULL;
-    void uninitialize();
-  };
-};
+  uint8_t get_current()  { return current; }
+
+  void set_current( uint8_t current) { this->current = current; }
+
+  node* get_nodeList()  { return nodeList; }
+
+  uint8_t get_nodeCount()  { return nodeCount; }
+
+  void set_nodeCount( uint8_t nodeCount) { this->nodeCount = nodeCount; }
 
+  bool get_valid()  { return valid; }
+
+  void set_valid( bool valid) { this->valid = valid; }
+
+  AmiroMap(Global *global) : global{global} {}
+
+  /**
+   * Initialize a new map from configuration.
+   * @param config map configuration array
+   *
+   */
+  void initialize(uint8_t (&config)[MAX_NODES][NODE_ATTRIBUTES]);
+
+
+  /**
+     Update the internal map state according to the detected fixpoint
+     This function should be called for a generic update, each can cycle and in
+     case a fixpoint on one side is detected.
+     Internal state maschine will go into an error state in case both sensors are black.
+
+     @param left fixpoint on left side detected
+     @param right fixpoint on right side detected
+     @param strategy current line follow strategy
+
+
+   */
+  void update(bool left, bool right, LineFollowStrategy strategy);
+
+private:
+  Global *global;
+  bool valid = false;
+  uint8_t nodeCount = 0;
+  node nodeList[MAX_NODES];
+  uint8_t current = 0;
+  uint8_t next = 0;
+};
+}; // namespace amiro
 
 #endif /* AMIRO_MAP */
diff --git a/devices/DiWheelDrive/main.cpp b/devices/DiWheelDrive/main.cpp
index 4ff0fd1..9044d29 100644
--- a/devices/DiWheelDrive/main.cpp
+++ b/devices/DiWheelDrive/main.cpp
@@ -1,3 +1,4 @@
+#include <cstdint>
 #define BL_CALLBACK_TABLE_ADDR  (0x08000000 + 0x01C0)
 #define BL_MAGIC_NUMBER         ((uint32_t)0xFF669900u)
 
@@ -17,8 +18,8 @@
 #include <chprintf.h>
 #include <shell.h>
 
-#include "linefollow.hpp" 
-
+#include "linefollow.hpp"
+#include "amiro_map.hpp"
 using namespace chibios_rt;
 
 Global global;
@@ -697,9 +698,9 @@ void shellRequestMotorResetGains(BaseSequentialStream *chp, int argc, char *argv
  * Calibrate the thresholds for left and right sensor to get the maximum threshold and to
  * be able to detect the correction direction.
  * In this case it is expected that the FL-Sensor sould be in the white part of the edge and the FR-Sensor in the black one.
- * 
+ *
  * Note: invert the threshs to drive on the other edge.
- * 
+ *
  * */
 void shellRequestCalibrateLineSensores(BaseSequentialStream *chp, int argc, char *argv[]) {
     // int vcnl4020AmbientLight[4];
@@ -710,11 +711,11 @@ void shellRequestCalibrateLineSensores(BaseSequentialStream *chp, int argc, char
     int 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: calbrate_line_sensors [1,n]\nThis will calibrate the thresholds for the left and right sensor\naccording to the maximum delta value recorded.\n");
     return;
@@ -748,7 +749,7 @@ void shellRequestCalibrateLineSensores(BaseSequentialStream *chp, int argc, char
     //   delta *= -1;
     // }
 
-    chprintf(chp,"FL: 0x%x, FR: 0x%x, Delta: %d, ProxyL: %x, ProxyR: %x, MaxDelta: %d\n", 
+    chprintf(chp,"FL: 0x%x, FR: 0x%x, Delta: %d, ProxyL: %x, ProxyR: %x, MaxDelta: %d\n",
                   vcnl4020Proximity[constants::DiWheelDrive::PROX_FRONT_LEFT],
                   vcnl4020Proximity[constants::DiWheelDrive::PROX_FRONT_RIGHT],
                   delta,
@@ -758,7 +759,7 @@ void shellRequestCalibrateLineSensores(BaseSequentialStream *chp, int argc, char
     // sleep(CAN::UPDATE_PERIOD);
     BaseThread::sleep(CAN::UPDATE_PERIOD);
   }
-  
+
 
   global.linePID.threshProxyL = sensorL / rounds;
   global.linePID.threshProxyR = sensorR / rounds;
@@ -776,7 +777,7 @@ void sellRequestgetBottomSensorData(BaseSequentialStream *chp, int argc, char *a
   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");
   }
@@ -787,11 +788,11 @@ void sellRequestgetBottomSensorData(BaseSequentialStream *chp, int argc, char *a
         // vcnl4020AmbientLight[i] = global.vcnl4020[i].getAmbientLight();
         vcnl4020Proximity[i] = global.vcnl4020[i].getProximityScaledWoOffset();
     }
-    
+
     int32_t delta = (vcnl4020Proximity[constants::DiWheelDrive::PROX_FRONT_LEFT]
                   - vcnl4020Proximity[constants::DiWheelDrive::PROX_FRONT_RIGHT]);
 
-    chprintf(chp,"WL:%d,FL:%d,FR:%d,WR:%d,Delta:%d\n", 
+    chprintf(chp,"WL:%d,FL:%d,FR:%d,WR:%d,Delta:%d\n",
                   vcnl4020Proximity[constants::DiWheelDrive::PROX_WHEEL_LEFT],
                   vcnl4020Proximity[constants::DiWheelDrive::PROX_FRONT_LEFT],
                   vcnl4020Proximity[constants::DiWheelDrive::PROX_FRONT_RIGHT],
@@ -817,7 +818,7 @@ 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; 
+  int32_t res = 0;
   proxbuf[counter] = (diff*100)/((a+b)/2);
   for (int i = 0; i< window; i++){
     res += proxbuf[i];
@@ -854,7 +855,7 @@ void shellRequestProxyRingValues(BaseSequentialStream *chp, int argc, char *argv
     // int32_t deviation = meanDeviation((prox[3]+old3) / 2  , (prox[4]+old4) / 2);
     // uint16_t notouch = 100;
     // uint16_t toucht = 20031;
-    // sign = 
+    // 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);
@@ -884,7 +885,7 @@ void shellRequestMagnetoMeter(BaseSequentialStream *chp, int argc, char *argv[])
     // }
     // uint16_t notouch = 100;
     // uint16_t toucht = 20031;
-    // sign = 
+    // sign =
     // i = 0;
 
     if (j < 10){
@@ -964,45 +965,122 @@ void shellRequestErrorInfo(BaseSequentialStream *chp, int argc, char *argv[]){
   for (int i=0; i<24;i++){
     global.stateTracker[i] = 0;
   }
+}
 
+inline void setAttributes(uint8_t (&map)[MAX_NODES][NODE_ATTRIBUTES], uint8_t id,
+                   uint8_t l, uint8_t r, uint8_t att) {
+  map[id][0] = l;
+  map[id][1] = r;
+  map[id][2] = att;
+}
+
+#define TEST_CASES 10
+void shellRequestTestSuite(BaseSequentialStream *chp, int argc, char *argv[]){
+  uint8_t testmap[MAX_NODES][NODE_ATTRIBUTES] = {0};
+  bool testres[TEST_CASES];
+  setAttributes(testmap, 0, 1, 2, 1);
+  setAttributes(testmap, 1, 2, 2, 0);
+  setAttributes(testmap, 2, 1, 0, 0);
+  setAttributes(testmap, 3, 0, 0, 0xff);
+  AmiroMap map = AmiroMap(&global);
+
+  // --------------------------------------------------
+
+  int tcase = 0;
+  map.initialize(testmap);
+  testres[tcase] = map.get_valid();
+
+  tcase++; // 1
+  setAttributes(testmap, 0, 1, 2, 0xff);
+  map.initialize(testmap);
+  testres[tcase] = !map.get_valid();
+
+  tcase++; // 2
+  setAttributes(testmap, 0, 1, 2, 0);
+  setAttributes(testmap, 2, 1, 0, 1);
+  map.initialize(testmap);
+  testres[tcase] = map.get_current() == 2;
+
+  // --------------------------------------------------
+
+  int failed = 0;
+  int passed = 0;
+  for (int i=0; i<=tcase; i++) {
+    if (testres[i]){
+      passed++;
+    }else{
+      failed++;
+      chprintf(chp, "Test %d Failed\n", i);
+    }
+  }
+  chprintf(chp, "Total: %d, Passed: %d, Failed: %d\n", tcase + 1, passed, failed);
+}
+
+void shellRequestMapTest(BaseSequentialStream *chp, int argc, char *argv[]) {
+
+  // TODO:
+  // For now see everything fixed. Create array with pointer to all node structs. Determine
+
+  // chprintf(chp, "  +-------------------+\n");
+  // chprintf(chp, "  |                   |\n");
+  // chprintf(chp, "  |                   |\n");
+  // chprintf(chp, "+-v--+                |\n");
+  // chprintf(chp, "|  0 |              +-++\n");
+  // chprintf(chp, "+-+--+     +--------+2 | <-------+\n");
+  // chprintf(chp, "  |        |        +--+         |\n");
+  // chprintf(chp, "  |        |        ^            |\n");
+  // chprintf(chp, "  |        |        |            |\n");
+  // chprintf(chp, "  |        v        |            |\n");
+  // chprintf(chp, "  |      +-+-+------+            |\n");
+  // chprintf(chp, "  +------> 1 |                   |\n");
+  // chprintf(chp, "         +---+-------------------+\n");
+
+  uint8_t testmap[MAX_NODES][NODE_ATTRIBUTES] = {0};
+  setAttributes(testmap, 0, 1, 2, 1);
+  setAttributes(testmap, 1, 2, 2, 0);
+  setAttributes(testmap, 2, 1, 0, 0);
+  setAttributes(testmap, 3, 0, 0, 0xff);
+  AmiroMap map = AmiroMap(&global);
+  map.initialize(testmap);
 }
 
 static const ShellCommand commands[] = {
-  {"shutdown", shellRequestShutdown},
-  {"wakeup", shellRequestWakeup},
-  {"check", shellRequestCheck},
-  {"reset_memory", shellRequestResetMemory},
-  {"get_board_id", shellRequestGetBoardId},
-  {"set_board_id", shellRequestSetBoardId},
-  {"get_memory_data", shellRequestGetMemoryData},
-  {"get_vcnl", shellRequestGetVcnl},
-  {"calib_vcnl_offset", shellRequestCalib},
-  {"set_vcnl_offset", shellRequestSetVcnlOffset},
-  {"reset_vcnl_offset", shellRequestResetVcnlOffset},
-  {"get_vcnl_offset", shellRequestGetVcnlOffset},
-  {"reset_Ed_Eb", shellRequestResetCalibrationConstants},
-  {"get_Ed_Eb", shellRequestGetCalibrationConstants},
-  {"set_Ed_Eb", shellRequestSetCalibrationConstants},
-  {"get_robot_id", shellRequestGetRobotId},
-  {"get_system_load", shellRequestGetSystemLoad},
-  {"set_lights", shellRequestSetLights},
-  {"shell_board", shellSwitchBoardCmd},
-  {"get_bootloader_info", shellRequestGetBootloaderInfo},
-  {"motor_drive", shellRequestMotorDrive},
-  {"motor_stop", shellRequestMotorStop},
-  {"motor_calibrate", shellRequestMotorCalibrate},
-  {"motor_getGains", shellRequestMotorGetGains},
-  {"motor_resetGains", shellRequestMotorResetGains},
-  {"calibrate_line_sensors", shellRequestCalibrateLineSensores},
-  {"printProxyBottom", sellRequestgetBottomSensorData},
-  {"printProxyRing", shellRequestProxyRingValues},
-  {"printMagnetometer", shellRequestMagnetoMeter},
-  {"printMagnetometerRes", shellRequestMagnetoMeterPrint},
-  {"printLocation", shellRequestPrintCoordinate},
-  {"checkPowerPins", shellRequestCheckPower},
-  {"infos", shellRequestErrorInfo},
-  {NULL, NULL}
-};
+    {"shutdown", shellRequestShutdown},
+    {"wakeup", shellRequestWakeup},
+    {"check", shellRequestCheck},
+    {"reset_memory", shellRequestResetMemory},
+    {"get_board_id", shellRequestGetBoardId},
+    {"set_board_id", shellRequestSetBoardId},
+    {"get_memory_data", shellRequestGetMemoryData},
+    {"get_vcnl", shellRequestGetVcnl},
+    {"calib_vcnl_offset", shellRequestCalib},
+    {"set_vcnl_offset", shellRequestSetVcnlOffset},
+    {"reset_vcnl_offset", shellRequestResetVcnlOffset},
+    {"get_vcnl_offset", shellRequestGetVcnlOffset},
+    {"reset_Ed_Eb", shellRequestResetCalibrationConstants},
+    {"get_Ed_Eb", shellRequestGetCalibrationConstants},
+    {"set_Ed_Eb", shellRequestSetCalibrationConstants},
+    {"get_robot_id", shellRequestGetRobotId},
+    {"get_system_load", shellRequestGetSystemLoad},
+    {"set_lights", shellRequestSetLights},
+    {"shell_board", shellSwitchBoardCmd},
+    {"get_bootloader_info", shellRequestGetBootloaderInfo},
+    {"motor_drive", shellRequestMotorDrive},
+    {"motor_stop", shellRequestMotorStop},
+    {"motor_calibrate", shellRequestMotorCalibrate},
+    {"motor_getGains", shellRequestMotorGetGains},
+    {"motor_resetGains", shellRequestMotorResetGains},
+    {"calibrate_line_sensors", shellRequestCalibrateLineSensores},
+    {"printProxyBottom", sellRequestgetBottomSensorData},
+    {"printProxyRing", shellRequestProxyRingValues},
+    {"printMagnetometer", shellRequestMagnetoMeter},
+    {"printMagnetometerRes", shellRequestMagnetoMeterPrint},
+    {"printLocation", shellRequestPrintCoordinate},
+    {"checkPowerPins", shellRequestCheckPower},
+    {"stateInfos", shellRequestErrorInfo},
+    {"testMap", shellRequestMapTest},
+    {"test", shellRequestTestSuite },
+    {NULL, NULL}};
 
 static const ShellConfig shell_cfg1 = {
   (BaseSequentialStream *) &global.sercanmux1,
