commit 4d54a507e898e4c3487bda3bc419cb09c7de5c40
Author: galberding <galberding@techfak.uni-bielefeld.de>
Date:   Fri Jul 10 13:00:52 2020 +0200

    Add addNode() and revise initialize()

diff --git a/devices/DiWheelDrive/amiro_map.cpp b/devices/DiWheelDrive/amiro_map.cpp
index eab9909..818bf56 100644
--- a/devices/DiWheelDrive/amiro_map.cpp
+++ b/devices/DiWheelDrive/amiro_map.cpp
@@ -20,21 +20,18 @@ uint8_t AmiroMap::initialize(){
       return 255;
     }
 
-    //look for start node (default is Node 0)
-    if (global->testmap[i][2] == 1 ) {
-      this->state.current = i;
-    }
+    addNode(global->testmap[i][0], global->testmap[i][1],
+            global->testmap[i][2]);
 
-    this->nodeList[i].id = i;
-    this->nodeList[i].edge.edge_id.left = global->testmap[i][0];
-    this->nodeList[i].edge.edge_id.right = global->testmap[i][1];
-    this->nodeList[i].flag = global->testmap[i][2];
-    this->nodeCount++;
+    // look for start node (default is Node 0)
+    if (global->testmap[i][2] == 1) {
+      this->state.current = this->nodeCount;
+    }
   }
-  this->state.next = this->nodeList[this->state.current].edge.edge_id.right;
+  // Set the next node according to the current strategy
+  this->state.next = this->nodeList[this->state.current].edge.arr[state.strategy-1];
 
   // TODO make validity check
-
   for (int j=0; j<nodeCount; j++) {
     this->nodeList[j].visited = 0;
     visitNode(j);
@@ -209,8 +206,16 @@ void AmiroMap::createInitNode() {
   state.next = 255;
 }
 
+uint8_t AmiroMap::addNode(uint8_t l, uint8_t r, uint8_t flags){
+  uint8_t id = nodeCount;
+  nodeCount++;
+  nodeList[id].edge.edge_id.left = l;
+  nodeList[id].edge.edge_id.right = r;
+  nodeList[id].flag = flags;
+  return id;
+}
 
-uint8_t AmiroMap::getNearest(types::position *p1) {
+    uint8_t AmiroMap::getNearest(types::position *p1) {
 
   uint8_t actualStrategy = this->lfStrategy == EDGE_LEFT ? 1 : 2;
   uint32_t thresh = global->nodeDistThresh;          // TODO: find good thresh value in cm
@@ -278,5 +283,6 @@ void AmiroMap::reset(){
   this->nodeCount = 0;
   this->state.current = 0;
   this->state.next= 0;
-
+  this->state.valid = false;
+  this->state.strategy = this->lfStrategy == EDGE_LEFT ? 1 : 2;
 }
diff --git a/devices/DiWheelDrive/amiro_map.hpp b/devices/DiWheelDrive/amiro_map.hpp
index df0248d..f600c1a 100644
--- a/devices/DiWheelDrive/amiro_map.hpp
+++ b/devices/DiWheelDrive/amiro_map.hpp
@@ -37,7 +37,7 @@ public:
   node * getNodeList(){return nodeList; }
 
 
-  AmiroMap(Global *global) : global{global} {}
+  AmiroMap(Global *global, LineFollowStrategy lfStrategy) : global(global), lfStrategy(lfStrategy) {}
 
   /**
    * Initialize a new map from configuration.
@@ -46,6 +46,7 @@ public:
    */
   uint8_t initialize();
 
+  void reset();
 
   /**
    * Update the internal map state according to the detected fixpoint
@@ -127,7 +128,7 @@ private:
 
   // Either create new fxpoint or assign point to existing one
   uint8_t assignFxp(types::position *p1);
-  void reset();
+
 };
 
 }; // namespace amiro
diff --git a/devices/DiWheelDrive/global.hpp b/devices/DiWheelDrive/global.hpp
index f6f9976..a7919b6 100644
--- a/devices/DiWheelDrive/global.hpp
+++ b/devices/DiWheelDrive/global.hpp
@@ -1,7 +1,7 @@
 #ifndef AMIRO_GLOBAL_HPP_
 #define AMIRO_GLOBAL_HPP_
 
-#include <cstdint>
+// #include <cstdint>
 #include <hal.h>
 #include <qei.h>
 
diff --git a/devices/DiWheelDrive/userthread.cpp b/devices/DiWheelDrive/userthread.cpp
index a1ca1db..68e8737 100644
--- a/devices/DiWheelDrive/userthread.cpp
+++ b/devices/DiWheelDrive/userthread.cpp
@@ -1,5 +1,4 @@
 #include "userthread.hpp"
-#include "amiro/Constants.h"
 #include "amiro_map.hpp"
 #include "global.hpp"
 #include "linefollow.hpp"
@@ -284,7 +283,7 @@ UserThread::main()
   running = false;
   LineFollowStrategy lStrategy = LineFollowStrategy::EDGE_RIGHT;
   LineFollow lf(&global);
-  AmiroMap map(&global);
+  AmiroMap map(&global, lStrategy);
   /*
    * LOOP
    */
@@ -808,8 +807,12 @@ UserThread::main()
                  "ID: %d, l: %d, r: %d \n", map.getNodeList()[i].id,
                  map.getNodeList()[i].left, map.getNodeList()[i].right);
       }
-      newState = UT_IDLE;
+      // Start test suit for trackUpdate
+      map.reset();
+      global.tcase = 0;
+      newState = UT_TEST_MAP_STATE;
       break;
+      // --------------------------------------------------
     case ut_states::UT_TEST_MAP_STATE:{
       // Test suit for amiro map
 
@@ -819,7 +822,7 @@ UserThread::main()
 
       // --------------------------------------------------
 
-      global.tcase = 0;
+      global.tcase++;
       // Set basic valid map configuration
       setAttributes(global.testmap, 0, 1, 2, 1);
       setAttributes(global.testmap, 1, 2, 2, 0);
