diff --git a/hardwareModules/RoFICoM/software/control_board/src/main.cpp b/hardwareModules/RoFICoM/software/control_board/src/main.cpp
index b125ec111..fbc16f13b 100644
--- a/hardwareModules/RoFICoM/software/control_board/src/main.cpp
+++ b/hardwareModules/RoFICoM/software/control_board/src/main.cpp
@@ -219,8 +219,9 @@ int main() {
         bsp::internalSwitchPin, bsp::internalVoltagePin, bsp::internalCurrentPin,
         bsp::externalSwitchPin, bsp::externalVoltagePin, bsp::externalCurrentPin );
     ConnectorStatus connectorStatus ( bsp::connectorSenseA, bsp::connectorSenseB );
-    //Lidar lidar( &*bsp::i2c, bsp::lidarEnablePin, bsp::lidarIRQPin );
-    //lidarInit( lidar, currentLidarMeasurement );
+    Lidar lidar( &*bsp::i2c, bsp::lidarEnablePin, bsp::lidarIRQPin );
+
+    lidarInit( lidar, currentLidarMeasurement );
 
     ConnComInterface connComInterface( std::move( bsp::connectorComm ).value() );
 
@@ -232,7 +233,7 @@ int main() {
                 onCmdVersion( spiInterface );
                 break;
             case Command::STATUS:
-                //onCmdStatus( spiInterface, std::move( b ), connComInterface, connectorStatus, slider, powerInterface, lidar, currentLidarMeasurement );
+                onCmdStatus( spiInterface, std::move( b ), connComInterface, connectorStatus, slider, powerInterface, lidar, currentLidarMeasurement );
                 break;
             case Command::INTERRUPT:
                 onCmdInterrupt( spiInterface, std::move( b ) );
diff --git a/hardwareModules/RoFICoM/software/control_board/src/motor.hpp b/hardwareModules/RoFICoM/software/control_board/src/motor.hpp
index 2448dc30a..8b10cc874 100644
--- a/hardwareModules/RoFICoM/software/control_board/src/motor.hpp
+++ b/hardwareModules/RoFICoM/software/control_board/src/motor.hpp
@@ -61,12 +61,6 @@ static const float kRetractedPosition = 0.0;
 static const float kExpandedPosition = 1.0;
 }
 
-/**
-     * Returns direction coeffiecient  -1 or 1 based on direction
-     * @param position
-     * @param goal_position
-     * @return -1 if goal < position, 1 if goal > position, 0 if goal==position
- */
 float getDirection(float position, float goal);
 float getPower(float position, float goal_position);
 
@@ -164,7 +158,7 @@ class Slider {
      * Computes position of the connector as value <0.0,1.0>.
      * 0.0 means fully retracted, 1.0 represents fully extended/locked
      *
-     * Note: when fully retracted not sensor is actually triggered,
+     * Note: when fully retracted no sensor is actually triggered,
      *
      * @return
      */