tmp commit; not working

modules/catkin_ws/src/PC/CMakeLists.txt

-cmake_minimum_required(VERSION 3.9)
+cmake_minimum_required(VERSION 3.5.1)
 project(PC)
 
 set(CMAKE_CXX_STANDARD 14)
@@ -12,4 +12,4 @@ set(SOURCE_FILES
 
 add_executable(PC ${SOURCE_FILES})
 
-target_link_libraries(PC ${Boost_LIBRARIES})
\ No newline at end of file
+target_link_libraries(PC ${Boost_LIBRARIES})

modules/catkin_ws/src/car/include/mainNode/PlatoonController.h

@@ -8,12 +8,15 @@
 #include <functional>
 
 #include "PlatoonProtocolLib/Vehicle.h"
+#include "PlatoonProtocolLib/Protocol.h"
 #include "PC2CarLib/CommandReceiver.h"
 #include "EgoMotion.h"
 
 namespace car
 {
 
+enum class State { ACC, CACC_FV, CACC_LV };
+
 class PlatoonController
 {
 public:
@@ -24,6 +27,7 @@ public:
                       EgoMotion& egoMotion);
       
     void run();
+    
     Callback cruiseControllerNotify;
 
 private:
@@ -31,7 +35,21 @@ private:
     platoonProtocol::Vehicle::Ptr c2c;
     pc2car::CommandReceiver::Ptr pc;
     EgoMotion& egoMotion;
+    
+    State curState = State::CACC_FV;
+
+    bool wantsPlatoon = false;
 
+    // TODO check these values!
+    PlatoonConfig platoonConfig {50.0, 0.0}; // IPD=50.0 PS=0.0
+    float desSpeed = 0.0;
+
+    // METHODS
+
+    void run_ACC();
+    void run_CACC_FV();
+    void run_CACC_LV();
+    
 };
 
 }

modules/catkin_ws/src/car/src/mainNode/PlatoonController.cpp

@@ -18,8 +18,58 @@ PlatoonController::PlatoonController(platoonProtocol::Vehicle::Ptr c2c,
 
 void car::PlatoonController::run()
 {
-    // TODO: implementation
-    std::cout << "PlatoonController was run." << std::endl;
+  std::cout << "PlatoonController was run." << std::endl;
+  
+  switch (curState) {
+    case State::ACC: {
+      run_ACC();
+      break;
+    }
+                     
+    case State::CACC_FV: {
+      run_CACC_FV();
+      break;
+    }
+                     
+    case State::CACC_LV: {
+      run_CACC_LV();
+      break;
+    }
+  }
+  
+}
+
+
+void car::PlatoonController::run_ACC() {
+
+}
+
+void car::PlatoonController::run_CACC_FV() {
+   bool inPlatoon = c2c->isPlatoonRunning(); 
+   platoonConfig = c2c->getPlatoonConfig();
+   wantsPlatoon = pc->isPlatoonEnabled().get();
+   
+   
+   std::cout << "Running PlatoonController::run_CACC_FV: inPlatoon = " << inPlatoon
+     << ", wantsPlatoon = " << wantsPlatoon << std::endl;
+
+   if (inPlatoon && wantsPlatoon) {
+    cruiseControllerNotify();
+    return;
+   } 
+   
+   if (inPlatoon && !wantsPlatoon) {
+     c2c->leavePlatoon();
+   }
+   
+   c2c
+   curState = State::ACC;
+   cruiseControllerNotify();
+   run_ACC();
+}
+
+void car::PlatoonController::run_CACC_LV() {
+
 }
 
 }