commit d607fcef6eef470bed23f892796fac4c5b233ba3
Author: galberding <galberding@techfak.uni-bielefeld.de>
Date:   Tue Oct 22 15:34:06 2019 +0200

    Fix docking behaviour, implement ability to trigger line following (different strategys as well as just start and stop) over CAN

diff --git a/devices/DiWheelDrive/DiWheelDrive.cpp b/devices/DiWheelDrive/DiWheelDrive.cpp
index ce36f64..7286cdc 100644
--- a/devices/DiWheelDrive/DiWheelDrive.cpp
+++ b/devices/DiWheelDrive/DiWheelDrive.cpp
@@ -2,6 +2,7 @@
 #include "hal.h"
 #include "qei.h"
 #include "DiWheelDrive.h"
+#include <chprintf.h>
 
 
 #include <global.hpp>
@@ -118,9 +119,11 @@ msg_t DiWheelDrive::receiveMessage(CANRxFrame *frame) {
         return RDY_OK;
       }
       break;
-    case CAN::SET_LINE_FOLLOW_STRATEGY:
+    case CAN::SET_LINE_FOLLOW_MSG:
+      chprintf((BaseSequentialStream*) &SD1, "Received Strategy!\n");
       if (frame->DLC == 1) {
         global.lfStrategy = frame->data8[0];
+        global.msgReceived = true;
         return RDY_OK;
       }
       break;
@@ -246,6 +249,8 @@ void DiWheelDrive::periodicBroadcast() {
   frame.DLC = 6;
   this->transmitMessage(&frame);
 
+  
+
   // Send the board ID (board ID of DiWheelDrive = Robot ID)
   if (this->bcCounter % 10 == 0) {
     frame.SID = 0;
diff --git a/devices/DiWheelDrive/global.hpp b/devices/DiWheelDrive/global.hpp
index 3959618..80211c7 100644
--- a/devices/DiWheelDrive/global.hpp
+++ b/devices/DiWheelDrive/global.hpp
@@ -206,10 +206,16 @@ public:
   // 3 DOCKING
   int lfStrategy = 3;
 
+  // CAN communication line:
+  // Will be set to true when message was received 
+  bool msgReceived = false; 
 // Debugging stuff --------------
   int forwardSpeed = 10;
   int enableRecord = 0;
 
+  bool triggerCan = false;
+  uint8_t strategyTest = 0;
+
   
 
   // Buffer for sensor values
diff --git a/devices/DiWheelDrive/main.cpp b/devices/DiWheelDrive/main.cpp
index 6f17881..600b91e 100644
--- a/devices/DiWheelDrive/main.cpp
+++ b/devices/DiWheelDrive/main.cpp
@@ -1170,12 +1170,17 @@ void disableCharging(){
 
 
 void setGlobalStrategy(BaseSequentialStream *chp, int argc, char *argv[]){
+  uint8_t strategy = 0;
   if(argc == 1){
-  
-    global.lfStrategy = atoi(argv[0]); 
+    strategy = atoi(argv[0]); 
   }
+  // send over can
+  global.strategyTest = strategy;
+  global.triggerCan = true;
 }
 
+
+
 void getPosition(BaseSequentialStream *chp, int argc, char *argv[]){
   types::position pos = global.odometry.getPosition();
   chprintf(chp, "Start: Position X: %d, Y: %d,  Rotation X: %d, Y: %d, Z: %d\n", global.startPos.x, global.startPos.y, global.startPos.f_x, global.startPos.f_y, global.startPos.f_z);
diff --git a/devices/DiWheelDrive/userthread.cpp b/devices/DiWheelDrive/userthread.cpp
index 07827b2..5f76b90 100644
--- a/devices/DiWheelDrive/userthread.cpp
+++ b/devices/DiWheelDrive/userthread.cpp
@@ -15,6 +15,15 @@ enum class states{
   CHARGING
 };
 
+enum msg_content{
+  STOP, 
+  START,
+  EDGE_LEFT,
+  EDGE_RIGHT,
+  FUZZY,
+  DOCKING
+};
+
 // User thread state:
 states utState = states::FOLLOW_LINE;
 
@@ -147,6 +156,7 @@ UserThread::main()
   int rpmSpeed[2] = {0};
   int stop[2] = {0};
   int proxyWheelThresh = 18000;
+  LineFollowStrategy lStrategy;
   for (uint8_t led = 0; led < 8; ++led) {
     global.robot.setLightColor(led, Color(Color::BLACK));
   }
@@ -184,11 +194,40 @@ UserThread::main()
         this->sleep(1000);
         lightAllLeds(Color::BLACK);
         running = !running;
+    // If message was received handle it here:
+    } else if(global.msgReceived){
+
+      global.msgReceived = false;
+      running = true;
+      switch(global.lfStrategy){
+        case msg_content::START:
+          break;
+        case msg_content::STOP:
+          running = false;
+          break;
+        case msg_content::EDGE_RIGHT:
+          lStrategy = LineFollowStrategy::EDGE_RIGHT;
+          break;
+        case  msg_content::EDGE_LEFT:
+          lStrategy = LineFollowStrategy::EDGE_LEFT;
+          break;
+        case msg_content::FUZZY:
+          lStrategy = LineFollowStrategy::FUZZY;
+          break;
+        case msg_content::DOCKING:
+          utState = states::OCCUPY;
+          lStrategy = LineFollowStrategy::EDGE_RIGHT;
+          break;
+        default:
+          break;
+      }
+    
     }
+
     if(running){
       switch(utState){
         case states::FOLLOW_LINE:{
-          LineFollowStrategy lStrategy = determineStrategy();
+          // LineFollowStrategy lStrategy = determineStrategy();
           if(lStrategy == LineFollowStrategy::DOCKING){
             utState = states::OCCUPY;
             break;
@@ -230,7 +269,7 @@ UserThread::main()
           }
 
           // Set correct following
-          lf.setStrategy(LineFollowStrategy::EDGE_RIGHT);
+          // lf.setStrategy(LineFollowStrategy::EDGE_RIGHT);
           lf.followLine(rpmSpeed);
           setRpmSpeed(rpmSpeed);
           
@@ -240,6 +279,7 @@ UserThread::main()
           break;
         }
         case states::CHARGING:{
+          setRpmSpeed(stop);
           break;
         }
         default:{
