diff --git a/modules/catkin_ws/src/car/include/mainNode/PlatoonController.h b/modules/catkin_ws/src/car/include/mainNode/PlatoonController.h
index d68df79b1c1e630aa5c0b74b4fc2f7443248f546..7f858bf2303aa8869bde325224760c4ca2bf2a6a 100644
--- a/modules/catkin_ws/src/car/include/mainNode/PlatoonController.h
+++ b/modules/catkin_ws/src/car/include/mainNode/PlatoonController.h
@@ -41,7 +41,7 @@ private:
     bool wantsPlatoon = false;
 
     // TODO check these values!
-    PlatoonConfig platoonConfig {50.0, 0.0}; // IPD=50.0 PS=0.0
+    platoonProtocol::PlatoonConfig platoonConfig {50.0, 0.0}; // IPD=50.0 PS=0.0
     float desSpeed = 0.0;
 
     // METHODS
diff --git a/modules/catkin_ws/src/car/src/mainNode/MainNode.cpp b/modules/catkin_ws/src/car/src/mainNode/MainNode.cpp
index 1cad6c10170a097e7fe9c03228a5dffce0871901..29e50dfc3be6ce8cbc171539376f423787e50351 100644
--- a/modules/catkin_ws/src/car/src/mainNode/MainNode.cpp
+++ b/modules/catkin_ws/src/car/src/mainNode/MainNode.cpp
@@ -22,7 +22,7 @@ MainNode::MainNode(ros::NodeHandle & nh, std::string & name) :
   egoMotion.cruiseControllerNotify = cruiseControllerNotify;
   egoMotion.platoonControllerNotify = platoonControllerNotify;
   platoonController.cruiseControllerNotify = cruiseControllerNotify;
-  // std::cout << "Called MainNode(ros::NodeHandle & nh, std::string & name)." << std::endl;
+  // pc->reciveCommands([this] (auto commandCode) {platoonControllerThread.notify();} )
 }
 
 MainNode::MainNode() : 
@@ -41,7 +41,7 @@ MainNode::MainNode() :
   egoMotion.cruiseControllerNotify = cruiseControllerNotify;
   egoMotion.platoonControllerNotify = platoonControllerNotify;
   platoonController.cruiseControllerNotify = cruiseControllerNotify;
-  // std::cout << "Called MainNode()." << std::endl;
+  // pc->reciveCommands([this] (auto commandCode) {platoonControllerThread.notify();} )
 }
 
 MainNode::~MainNode()
diff --git a/modules/catkin_ws/src/car/src/mainNode/PlatoonController.cpp b/modules/catkin_ws/src/car/src/mainNode/PlatoonController.cpp
index 2adb912f4e015ae9912323475695d2035c354271..9eaf82972afe498ca085ab148c007e5c09541131 100644
--- a/modules/catkin_ws/src/car/src/mainNode/PlatoonController.cpp
+++ b/modules/catkin_ws/src/car/src/mainNode/PlatoonController.cpp
@@ -3,6 +3,7 @@
 //
 
 #include "../../include/mainNode/PlatoonController.h"
+#include "PlatoonProtocolLib/FollowerVehicle.h"
 #include <iostream>
 
 namespace car
@@ -46,7 +47,7 @@ void car::PlatoonController::run_ACC() {
 
 void car::PlatoonController::run_CACC_FV() {
    bool inPlatoon = c2c->isPlatoonRunning(); 
-   platoonConfig = c2c->getPlatoonConfig();
+   platoonConfig = std::dynamic_pointer_cast<platoonProtocol::FollowerVehicle>(c2c)->getPlatoonConfig();
    wantsPlatoon = pc->isPlatoonEnabled().get();
    
    
@@ -62,7 +63,7 @@ void car::PlatoonController::run_CACC_FV() {
      c2c->leavePlatoon();
    }
    
-   c2c
+   // TODO: c2c->reset()
    curState = State::ACC;
    cruiseControllerNotify();
    run_ACC();