commit d02c536e886f43a8f26b41b6e2981e59a7d8a40f
Author: galberding <galberding@techfak.uni-bielefeld.de>
Date:   Sat Jun 27 17:39:41 2020 +0200

    Calculate distance between nodes and traveled distance since last node

diff --git a/devices/DiWheelDrive/amiro_map.cpp b/devices/DiWheelDrive/amiro_map.cpp
index 70acdaf..cb73127 100644
--- a/devices/DiWheelDrive/amiro_map.cpp
+++ b/devices/DiWheelDrive/amiro_map.cpp
@@ -137,7 +137,7 @@ uint8_t AmiroMap::update(uint16_t WL, uint16_t WR, LineFollowStrategy strategy)
         state.eLength = calculateDist(&nodeList[state.next].pR,
                                       &nodeList[state.current].pR);
       }
-      state.dist = calculateDist(&nodeList[state.next].pR, &currentPos) * 100 /
+      state.dist = calculateDist(&nodeList[state.current].pR, &currentPos) * 100 /
                    state.eLength;
     } else {
       // Driving on the left edge
@@ -145,7 +145,7 @@ uint8_t AmiroMap::update(uint16_t WL, uint16_t WR, LineFollowStrategy strategy)
         state.eLength =
             calculateDist(&nodeList[state.next].pL, &nodeList[state.current].pL);
       }
-      state.dist = calculateDist(&nodeList[state.next].pL, &currentPos) * 100 / state.eLength;
+      state.dist = calculateDist(&nodeList[state.current].pL, &currentPos) * 100 / state.eLength;
 
     }
   }
@@ -153,6 +153,6 @@ uint8_t AmiroMap::update(uint16_t WL, uint16_t WR, LineFollowStrategy strategy)
 }
 
 uint32_t AmiroMap::calculateDist(types::position *p1, types::position *p2) {
-  return (uint32_t)  sqrt(pow(p2->x - p1->x, 2) +
-              pow(p2->y - p1->y, 2));
+  return (uint32_t)  sqrt(pow((p2->x - p1->x)/10000, 2) +
+                          pow((p2->y - p1->y)/10000, 2));
 }
diff --git a/devices/DiWheelDrive/amiro_map.hpp b/devices/DiWheelDrive/amiro_map.hpp
index a3c849b..f931198 100644
--- a/devices/DiWheelDrive/amiro_map.hpp
+++ b/devices/DiWheelDrive/amiro_map.hpp
@@ -5,6 +5,7 @@
 #include "linefollow.hpp"
 #include <amiroosconf.h>
 #include <ch.hpp>
+#include <math.h>
 
 
 namespace amiro {
diff --git a/devices/DiWheelDrive/userthread.cpp b/devices/DiWheelDrive/userthread.cpp
index f7bd74a..d9ffe41 100644
--- a/devices/DiWheelDrive/userthread.cpp
+++ b/devices/DiWheelDrive/userthread.cpp
@@ -1,8 +1,7 @@
-#include "global.hpp"
-#include <cmath>
-#include "linefollow.hpp"
 #include "userthread.hpp"
 #include "amiro_map.hpp"
+#include "global.hpp"
+#include "linefollow.hpp"
 
 using namespace amiro;
 
@@ -842,6 +841,7 @@ UserThread::main()
 
 
       global.odometry.setPosition(1.0, 0.0, 0.0);
+      chprintf((BaseSequentialStream *)&global.sercanmux1, "Current Point: %d\n", global.odometry.getPosition().x);
       global.tcase++; // 6
       // Fixpoint on left side
       ret = map.update(0, 20000, LineFollowStrategy::EDGE_RIGHT);
@@ -858,6 +858,8 @@ UserThread::main()
 
 
       global.odometry.setPosition(1.5, 0.0, 0.0);
+      chprintf((BaseSequentialStream *)&global.sercanmux1,
+               "Current Point: %d\n", global.odometry.getPosition().x);
       global.tcase++; // 7
       // Fixpoint on left side, no update should appear because fixpoint already
       // marked
@@ -871,9 +873,57 @@ UserThread::main()
         && (map.get_state()->strategy == 0x01);
         // && (map.get_state()->dist == 0);
 
+      global.odometry.setPosition(1.2, 0.0, 0.0);
+      global.tcase++; // 8
+      // Fixpoint on left side, no update should appear because fixpoint already
+      // marked
+      ret = map.update(20000, 20000, LineFollowStrategy::EDGE_RIGHT);
+      chprintf((BaseSequentialStream *)&global.sercanmux1,
+               "Update test %d: Ret %d, cur %d, nex %d, dist %d, len %d\n", global.tcase, ret,
+               map.get_state()->current, map.get_state()->next, map.get_state()->dist, map.get_state()->eLength);
+      // No case should be true because neither was a node visited nor
+      // was a fixpoint detected.
+      global.testres[global.tcase] =
+          (ret == 0x04) && (map.get_state()->strategy == 0x01)
+        && (map.get_state()->dist == 0);
 
+      global.odometry.setPosition(.5, 0.0, 0.0);
+      chprintf((BaseSequentialStream *)&global.sercanmux1,
+               "Current Point: %d\n", global.odometry.getPosition().x);
+      global.tcase++; // 9
+      // Fixpoint on left side, no update should appear because fixpoint already
+      // marked
+      ret = map.update(0, 20000, LineFollowStrategy::EDGE_RIGHT);
+      chprintf((BaseSequentialStream *)&global.sercanmux1,
+               "Update test %d: Ret %d, cur %d, nex %d, dist %d, len %d\n",
+               global.tcase, ret, map.get_state()->current,
+               map.get_state()->next, map.get_state()->dist,
+               map.get_state()->eLength);
+      // No case should be true because neither was a node visited nor
+      // was a fixpoint detected.
+      global.testres[global.tcase] =
+        (ret == 9) &&
+        (map.get_state()->strategy == 1) &&
+        (map.get_state()->dist == 0) &&
+        (map.get_state()->eLength == 50);
 
-
+      global.odometry.setPosition(.75, 0.0, 0.0);
+      chprintf((BaseSequentialStream *)&global.sercanmux1,
+               "Current Point: %d\n", global.odometry.getPosition().x);
+      global.tcase++; // 10
+      // Fixpoint on left side, no update should appear because fixpoint already
+      // marked
+      ret = map.update(20000, 20000, LineFollowStrategy::EDGE_RIGHT);
+      chprintf((BaseSequentialStream *)&global.sercanmux1,
+               "Update test %d: Ret %d, cur %d, nex %d, dist %d, len %d\n",
+               global.tcase, ret, map.get_state()->current,
+               map.get_state()->next, map.get_state()->dist,
+               map.get_state()->eLength);
+      // No case should be true because neither was a node visited nor
+      // was a fixpoint detected.
+      global.testres[global.tcase] =
+          (ret == 12) && (map.get_state()->strategy == 1) &&
+          (map.get_state()->dist == 50) && (map.get_state()->eLength == 50);
 
       int failed = 0;
       int passed = 0;
