diff --git a/src/C++/Driver/src/KobukiParser.cpp b/src/C++/Driver/src/KobukiParser.cpp
index b43c487..925fed4 100644
--- a/src/C++/Driver/src/KobukiParser.cpp
+++ b/src/C++/Driver/src/KobukiParser.cpp
@@ -17,10 +17,97 @@ int KobukiParser::parseKobukiMessage(TKobukiData &output, unsigned char *data) {
         switch (dataType) {
             case 0x01:
                 if (dataLength == 0x0F) {
+                    parseBasicData(output, data, checkedValue);
+                } else {
+                    checkedValue += dataLength;
+                }
+                break;
+
+            case 0x03:
+                if (dataLength == 0x03) {
+                    parseIRSensorData(output, data, checkedValue);
+                } else {
+                    checkedValue += dataLength;
+                }
+                break;
+
+            case 0x04:
+                if (dataLength == 0x07) {
+                    parseGyroData(output, data, checkedValue);
+                } else {
+                    checkedValue += dataLength;
+                }
+                break;
+
+            case 0x05:
+                if (dataLength == 0x06) {
+                    parseCliffSensorData(output, data, checkedValue); 
+                } else {
+                    checkedValue += dataLength;
+                }
+                break;
+
+            case 0x06:
+                if (dataLength == 0x02) {
+                    parseWheelCurrentData(output, data, checkedValue);  
+                } else {
+                    checkedValue += dataLength;
+                }
+                break;
+
+            case 0x0A:
+                if (dataLength == 0x04) {
+                    parseHardwareVersionData(output, data, checkedValue);
+                } else {
+                    checkedValue += dataLength;
+                }
+                break;
+
+            case 0x0B:
+                if (dataLength == 0x04) {
+                    parseFirmwareVersionData(output, data, checkedValue);
+                } else {
+                    checkedValue += dataLength;
+                }
+                break;
+
+            case 0x0D:
+                if (dataLength % 2 == 0) {
+                    parseXYZData(TKobukiData &output, unsigned char *data, int &checkedValue);
+                } else {
+                    checkedValue += dataLength;
+                }
+                break;
+
+            case 0x10:
+                if (dataLength == 0x10) {
+                    parseDigitalAnalogInputData(output, data, checkedValue);
+                } else {
+                    checkedValue += dataLength;
+                }
+                break;
+
+            case 0x13:
+                if (dataLength == 0x0C) {
+                  parseUDIDData(output, data, checkedValue);
+                } else {
+                    checkedValue += dataLength;
+                }
+                break;
+
+            default:
+                std::cerr << "Unknown data type: " << std::hex << static_cast<int>(dataType) << std::dec << std::endl;
+                checkedValue += dataLength;
+                break;
+        }
+    }
+    return 0;
+}
+
+int KobukiParser::parseBasicData(TKobukiData &output, unsigned char *data, int &checkedValue){
                     output.timestamp = data[checkedValue + 1] * 256 + data[checkedValue];
                     checkedValue += 2;
                     output.BumperCenter = data[checkedValue] & 0x02;
-                    std::cout << "BumperCenter: " << output.BumperCenter << std::endl;
                     output.BumperLeft = data[checkedValue] & 0x04;
                     output.BumperRight = data[checkedValue] & 0x01;
                     checkedValue++;
@@ -47,136 +134,60 @@ int KobukiParser::parseKobukiMessage(TKobukiData &output, unsigned char *data) {
                     checkedValue++;
                     output.overCurrent = data[checkedValue];
                     checkedValue++;
-                } else {
-                    std::cerr << "Invalid data length for type 0x01" << std::endl;
-                    checkedValue += dataLength;
-                }
-                break;
+}
 
-            case 0x03:
-                if (dataLength == 0x03) {
-                    output.IRSensorRight = data[checkedValue];
+
+int KobukiParser::parseIRSensorData(TKobukiData &output, unsigned char *data, int &checkedValue){
+                        output.IRSensorRight = data[checkedValue];
                     checkedValue++;
                     output.IRSensorCenter = data[checkedValue];
                     checkedValue++;
                     output.IRSensorLeft = data[checkedValue];
                     checkedValue++;
-                } else {
-                    std::cerr << "Invalid data length for type 0x03" << std::endl;
-                    checkedValue += dataLength;
-                }
-                break;
+}
 
-            case 0x04:
-                if (dataLength == 0x07) {
+int KobukiParser::parseGyroData(TKobukiData &output, unsigned char *data, int &checkedValue){
                     output.GyroAngle = data[checkedValue + 1] * 256 + data[checkedValue];
                     checkedValue += 2;
                     output.GyroAngleRate = data[checkedValue + 1] * 256 + data[checkedValue];
                     checkedValue += 5; // 3 unused
-                } else {
-                    std::cerr << "Invalid data length for type 0x04" << std::endl;
-                    checkedValue += dataLength;
-                }
-                break;
+}
 
-            case 0x05:
-                if (dataLength == 0x06) {
-                    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 {
-                    std::cerr << "Invalid data length for type 0x05" << std::endl;
-                    checkedValue += dataLength;
-                }
-                break;
+int KobukiParser::parseCliffSensorData(TKobukiData &output, unsigned char *data, int &checkedValue){
+                    output.CliffSensorRight = data[checkedValue];
+                    checkedValue++;
+                    output.CliffSensorCenter = data[checkedValue];
+                    checkedValue++;
+                    output.CliffSensorLeft = data[checkedValue];
+                    checkedValue++;
+}
 
-            case 0x06:
-                if (dataLength == 0x02) {
+int KobukiParser::parseWheelCurrentData(TKobukiData &output, unsigned char *data, int &checkedValue){
                     output.wheelCurrentLeft = data[checkedValue];
                     checkedValue++;
                     output.wheelCurrentRight = data[checkedValue];
                     checkedValue++;
-                } else {
-                    std::cerr << "Invalid data length for type 0x06" << std::endl;
-                    checkedValue += dataLength;
-                }
-                break;
+}
 
-            case 0x0A:
-                if (dataLength == 0x04) {
+int KobukiParser::parseHardwareVersionData(TKobukiData &output, unsigned char *data, int &checkedValue){
                     output.extraInfo.HardwareVersionPatch = data[checkedValue];
                     checkedValue++;
                     output.extraInfo.HardwareVersionMinor = data[checkedValue];
                     checkedValue++;
                     output.extraInfo.HardwareVersionMajor = data[checkedValue];
                     checkedValue += 2;
-                } else {
-                    std::cerr << "Invalid data length for type 0x0A" << std::endl;
-                    checkedValue += dataLength;
-                }
-                break;
+}
 
-            case 0x0B:
-                if (dataLength == 0x04) {
+int KobukiParser::parseFirmwareVersionData(TKobukiData &output, unsigned char *data, int &checkedValue){
                     output.extraInfo.FirmwareVersionPatch = data[checkedValue];
                     checkedValue++;
                     output.extraInfo.FirmwareVersionMinor = data[checkedValue];
                     checkedValue++;
                     output.extraInfo.FirmwareVersionMajor = data[checkedValue];
                     checkedValue += 2;
-                } else {
-                    std::cerr << "Invalid data length for type 0x0B" << std::endl;
-                    checkedValue += dataLength;
-                }
-                break;
+}
 
-            case 0x0D:
-                if (dataLength % 2 == 0) {
-                    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 {
-                    std::cerr << "Invalid data length for type 0x0D" << std::endl;
-                    checkedValue += dataLength;
-                }
-                break;
-
-            case 0x10:
-                if (dataLength == 0x10) {
-                    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 {
-                    std::cerr << "Invalid data length for type 0x10" << std::endl;
-                    checkedValue += dataLength;
-                }
-                break;
-
-            case 0x13:
-                if (dataLength == 0x0C) {
+int KobukiParser::parseExtraData(TKobukiData &output, unsigned char *data, int &checkedValue){
                     output.extraInfo.UDID0 = data[checkedValue + 3] * 256 * 256 * 256 +
                                              data[checkedValue + 2] * 256 * 256 +
                                              data[checkedValue + 1] * 256 +
@@ -192,20 +203,55 @@ int KobukiParser::parseKobukiMessage(TKobukiData &output, unsigned char *data) {
                                              data[checkedValue + 1] * 256 +
                                              data[checkedValue];
                     checkedValue += 4;
-                } else {
-                    std::cerr << "Invalid data length for type 0x13" << std::endl;
-                    checkedValue += dataLength;
-                }
-                break;
-
-            default:
-                std::cerr << "Unknown data type: " << std::hex << static_cast<int>(dataType) << std::dec << std::endl;
-                checkedValue += dataLength;
-                break;
-        }
-    }
-    return 0;
 }
+
+int KobukiParser::parseDigitalAnalogInputData(TKobukiData &output, unsigned char *data, int &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
+}
+
+int KobukiParser::parseUDIDData(TKobukiData &output, unsigned char *data, int &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;
+}
+
+int KobukiParser::parseXYZData(TKobukiData &output, unsigned char *data, int &checkedValue){
+                    output.gyroData.reserve(data[checkedValue]);
+                    output.gyroData.clear();
+                    for (int hk = 0; hk < data[checkedValue]; 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);
+                    }
+}
+
+
 int KobukiParser::checkChecksum(unsigned char *data) {
     unsigned char chckSum = 0;
     for (int i = 0; i < data[0] + 2; i++) {
diff --git a/src/C++/Driver/src/KobukiParser.h b/src/C++/Driver/src/KobukiParser.h
index 1e79bbd..130fb3d 100644
--- a/src/C++/Driver/src/KobukiParser.h
+++ b/src/C++/Driver/src/KobukiParser.h
@@ -39,15 +39,16 @@ public:
 private:
     int checkChecksum(unsigned char *data);
     int parseBasicData(TKobukiData &output, unsigned char *data, int &checkedValue);
-    // int parseIRSensorData(TKobukiData &output, unsigned char *data, int &checkedValue);
-    // int parseGyroData(TKobukiData &output, unsigned char *data, int &checkedValue);
-    // int parseCliffSensorData(TKobukiData &output, unsigned char *data, int &checkedValue);
-    // int parseWheelCurrentData(TKobukiData &output, unsigned char *data, int &checkedValue);
-    // int parseHardwareVersionData(TKobukiData &output, unsigned char *data, int &checkedValue);
-    // int parseFirmwareVersionData(TKobukiData &output, unsigned char *data, int &checkedValue);
-    // int parseXYZData(TKobukiData &output, unsigned char *data, int &checkedValue);
-    // int parseDigitalAnalogInputData(TKobukiData &output, unsigned char *data, int &checkedValue);
-    // int parseUDIDData(TKobukiData &output, unsigned char *data, int &checkedValue);
+    int parseIRSensorData(TKobukiData &output, unsigned char *data, int &checkedValue);
+    int parseGyroData(TKobukiData &output, unsigned char *data, int &checkedValue);
+    int parseCliffSensorData(TKobukiData &output, unsigned char *data, int &checkedValue);
+    int parseWheelCurrentData(TKobukiData &output, unsigned char *data, int &checkedValue);
+    int parseHardwareVersionData(TKobukiData &output, unsigned char *data, int &checkedValue);
+    int parseFirmwareVersionData(TKobukiData &output, unsigned char *data, int &checkedValue);
+    int parseXYZData(TKobukiData &output, unsigned char *data, int &checkedValue);
+    int parseDigitalAnalogInputData(TKobukiData &output, unsigned char *data, int &checkedValue);
+    int parseUDIDData(TKobukiData &output, unsigned char *data, int &checkedValue);
+    int parseExtraData(TKobukiData &output, unsigned char *data, int &checkedValue)
 
 };