diff --git a/src/C++/Driver/src/KobukiDriver/CKobuki.cpp b/src/C++/Driver/src/KobukiDriver/CKobuki.cpp
index 08e819e..8ed7dd3 100755
--- a/src/C++/Driver/src/KobukiDriver/CKobuki.cpp
+++ b/src/C++/Driver/src/KobukiDriver/CKobuki.cpp
@@ -282,198 +282,6 @@ int CKobuki::measure() {
   return 0;
 }
 
-// int CKobuki::parseKobukiMessage(TKobukiData &output, unsigned char *data)
-// {
-//     int rtrnvalue = checkChecksum(data);
-//     // ak je zly checksum,tak kaslat na to
-//     if (rtrnvalue != 0)
-//         return -2;
-
-//     int checkedValue = 1;
-//     // kym neprejdeme celu dlzku
-//     while (checkedValue < data[0])
-//     {
-//         // basic data subload
-//         if (data[checkedValue] == 0x01)
-//         {
-//             checkedValue++;
-//             if (data[checkedValue] != 0x0F)
-//                 return -1;
-//             checkedValue++;
-//             output.timestamp = data[checkedValue + 1] * 256 +
-//             data[checkedValue]; checkedValue += 2; output.BumperCenter =
-//             data[checkedValue] && 0x02; output.BumperLeft =
-//             data[checkedValue] && 0x04; output.BumperRight =
-//             data[checkedValue] && 0x01; checkedValue++; output.WheelDropLeft
-//             = data[checkedValue] && 0x02; output.WheelDropRight =
-//             data[checkedValue] && 0x01; checkedValue++; output.CliffCenter =
-//             data[checkedValue] && 0x02; output.CliffLeft = data[checkedValue]
-//             && 0x04; output.CliffRight = data[checkedValue] && 0x01;
-//             checkedValue++;
-//             output.EncoderLeft = data[checkedValue + 1] * 256 +
-//             data[checkedValue]; checkedValue += 2; output.EncoderRight =
-//             data[checkedValue + 1] * 256 + data[checkedValue]; checkedValue
-//             += 2; output.PWMleft = data[checkedValue]; checkedValue++;
-//             output.PWMright = data[checkedValue];
-//             checkedValue++;
-//             output.ButtonPress = data[checkedValue];
-//             checkedValue++;
-//             output.Charger = data[checkedValue];
-//             checkedValue++;
-//             output.Battery = data[checkedValue];
-//             checkedValue++;
-//             output.overCurrent = data[checkedValue];
-//             checkedValue++;
-//         }
-//         else if (data[checkedValue] == 0x03)
-//         {
-//             checkedValue++;
-//             if (data[checkedValue] != 0x03)
-//                 return -3;
-//             checkedValue++;
-//             output.IRSensorRight = data[checkedValue];
-//             checkedValue++;
-//             output.IRSensorCenter = data[checkedValue];
-//             checkedValue++;
-//             output.IRSensorLeft = data[checkedValue];
-//             checkedValue++;
-//         }
-//         else if (data[checkedValue] == 0x04)
-//         {
-//             checkedValue++;
-//             if (data[checkedValue] != 0x07)
-//                 return -4;
-//             checkedValue++;
-//             output.GyroAngle = data[checkedValue + 1] * 256 +
-//             data[checkedValue]; checkedValue += 2; output.GyroAngleRate =
-//             data[checkedValue + 1] * 256 + data[checkedValue]; checkedValue
-//             += 5; // 3 unsued
-//         }
-//         else if (data[checkedValue] == 0x05)
-//         {
-//             checkedValue++;
-//             if (data[checkedValue] != 0x06)
-//                 return -5;
-//             checkedValue++;
-//             output.CliffSensorRight =
-//                 data[checkedValue + 1] * 256 + data[checkedValue];
-//             checkedValue += 2;
-//             output.CliffSensorCenter =
-//                 data[checkedValue + 1] * 256 + data[checkedValue];
-//             checkedValue += 2;
-//             output.CliffSensorLeft =
-//                 data[checkedValue + 1] * 256 + data[checkedValue];
-//             checkedValue += 2;
-//         }
-//         else if (data[checkedValue] == 0x06)
-//         {
-//             checkedValue++;
-//             if (data[checkedValue] != 0x02)
-//                 return -6;
-//             checkedValue++;
-//             output.wheelCurrentLeft = data[checkedValue];
-//             checkedValue++;
-//             output.wheelCurrentRight = data[checkedValue];
-//             checkedValue++;
-//         }
-//         else if (data[checkedValue] == 0x0A)
-//         {
-//             checkedValue++;
-//             if (data[checkedValue] != 0x04)
-//                 return -7;
-//             checkedValue++;
-//             output.extraInfo.HardwareVersionPatch = data[checkedValue];
-//             checkedValue++;
-//             output.extraInfo.HardwareVersionMinor = data[checkedValue];
-//             checkedValue++;
-//             output.extraInfo.HardwareVersionMajor = data[checkedValue];
-//             checkedValue += 2;
-//         }
-//         else if (data[checkedValue] == 0x0B)
-//         {
-//             checkedValue++;
-//             if (data[checkedValue] != 0x04)
-//                 return -8;
-//             checkedValue++;
-//             output.extraInfo.FirmwareVersionPatch = data[checkedValue];
-//             checkedValue++;
-//             output.extraInfo.FirmwareVersionMinor = data[checkedValue];
-//             checkedValue++;
-//             output.extraInfo.FirmwareVersionMajor = data[checkedValue];
-//             checkedValue += 2;
-//         }
-//         else if (data[checkedValue] == 0x0D)
-//         {
-//             checkedValue++;
-//             if (data[checkedValue] % 2 != 0)
-//                 return -9;
-//             checkedValue++;
-//             output.frameId = data[checkedValue];
-//             checkedValue++;
-//             int howmanyFrames = data[checkedValue] / 3;
-//             checkedValue++;
-//             output.gyroData.reserve(howmanyFrames);
-//             output.gyroData.clear();
-//             for (int hk = 0; hk < howmanyFrames; hk++)
-//             {
-//                 TRawGyroData temp;
-//                 temp.x = data[checkedValue + 1] * 256 + data[checkedValue];
-//                 checkedValue += 2;
-//                 temp.y = data[checkedValue + 1] * 256 + data[checkedValue];
-//                 checkedValue += 2;
-//                 temp.z = data[checkedValue + 1] * 256 + data[checkedValue];
-//                 checkedValue += 2;
-//                 output.gyroData.push_back(temp);
-//             }
-//         }
-//         else if (data[checkedValue] == 0x10)
-//         {
-//             checkedValue++;
-//             if (data[checkedValue] != 0x10)
-//                 return -10;
-//             checkedValue++;
-//             output.digitalInput = data[checkedValue + 1] * 256 +
-//             data[checkedValue]; checkedValue += 2; output.analogInputCh0 =
-//             data[checkedValue + 1] * 256 + data[checkedValue]; checkedValue
-//             += 2; output.analogInputCh1 = data[checkedValue + 1] * 256 +
-//             data[checkedValue]; checkedValue += 2; output.analogInputCh2 =
-//             data[checkedValue + 1] * 256 + data[checkedValue]; checkedValue
-//             += 2; output.analogInputCh3 = data[checkedValue + 1] * 256 +
-//             data[checkedValue]; checkedValue += 8; // 2+6
-//         }
-//         else if (data[checkedValue] == 0x13)
-//         {
-//             checkedValue++;
-//             if (data[checkedValue] != 0x0C)
-//                 return -11;
-//             checkedValue++;
-//             output.extraInfo.UDID0 = data[checkedValue + 3] * 256 * 256 * 256
-//             +
-//                                      data[checkedValue + 2] * 256 * 256 +
-//                                      data[checkedValue + 1] * 256 +
-//                                      data[checkedValue];
-//             checkedValue += 4;
-//             output.extraInfo.UDID1 = data[checkedValue + 3] * 256 * 256 * 256
-//             +
-//                                      data[checkedValue + 2] * 256 * 256 +
-//                                      data[checkedValue + 1] * 256 +
-//                                      data[checkedValue];
-//             checkedValue += 4;
-//             output.extraInfo.UDID2 = data[checkedValue + 3] * 256 * 256 * 256
-//             +
-//                                      data[checkedValue + 2] * 256 * 256 +
-//                                      data[checkedValue + 1] * 256 +
-//                                      data[checkedValue];
-//             checkedValue += 4;
-//         }
-//         else
-//         {
-//             checkedValue++;
-//             checkedValue += data[checkedValue] + 1;
-//         }
-//     }
-//     return 0;
-// }
 
 long double CKobuki::gyroToRad(signed short GyroAngle) {
 
diff --git a/src/C++/Driver/src/main.cpp b/src/C++/Driver/src/main.cpp
index a06969b..5d358fd 100644
--- a/src/C++/Driver/src/main.cpp
+++ b/src/C++/Driver/src/main.cpp
@@ -8,7 +8,6 @@
 
 using namespace std;
 CKobuki robot;
-int movement();
 std::string readMQTT();
 void parseMQTT(std::string message);
 MqttClient client("mqtt://145.92.224.21:1883", "KobukiRPI", "ishak", "kobuki"); //create a client object
@@ -64,50 +63,13 @@ void parseMQTT(std::string message){
         robot.sendNullMessage();
     }
     else if (message == "estop"){
-        robot.forward(-500);
+        robot.forward(-400);
     }
     else{
         std::cout << "Invalid command" << std::endl;
     }
 }
 
-
-int movement()
-{
-    int text;
-    while (true)
-    {
-        cout << "gimme input: ";
-        cin >> text;
-
-        if (text == 1)
-        {
-            robot.forward(400);
-        }
-        else if (text == 2)
-        {
-            // 1 is full circle
-            robot.Rotate(90);
-        }
-        else if (text == 3)
-        {
-            // Add your code here for text == 3
-        }
-        else
-        {
-            try
-            {
-                robot.doRotation(text);
-                throw "NaN";
-            }
-            catch (const char *msg)
-            {
-                cerr << msg << endl;
-            }
-        }
-    }
-}
-
 void logToFile()
 {
     while (true)