diff --git a/src/C++/Driver/CMakeLists.txt b/src/C++/Driver/CMakeLists.txt index f10f0a6..7bb90d4 100644 --- a/src/C++/Driver/CMakeLists.txt +++ b/src/C++/Driver/CMakeLists.txt @@ -6,7 +6,7 @@ set(SOURCE_FILES src/KobukiParser.h src/CKobuki.cpp src/CKobuki.h - src/main.cpp) + src/test.cpp) add_executable(kobuki_control ${SOURCE_FILES}) #target_link_libraries(kobuki_control ) \ No newline at end of file diff --git a/src/C++/Driver/src/KobukiParser.cpp b/src/C++/Driver/src/KobukiParser.cpp index b43c487..8a62c73 100644 --- a/src/C++/Driver/src/KobukiParser.cpp +++ b/src/C++/Driver/src/KobukiParser.cpp @@ -17,183 +17,80 @@ int KobukiParser::parseKobukiMessage(TKobukiData &output, unsigned char *data) { switch (dataType) { case 0x01: if (dataLength == 0x0F) { - 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++; - 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++; + parseBasicData(output, data, checkedValue); } else { - std::cerr << "Invalid data length for type 0x01" << std::endl; checkedValue += dataLength; } break; case 0x03: if (dataLength == 0x03) { - output.IRSensorRight = data[checkedValue]; - checkedValue++; - output.IRSensorCenter = data[checkedValue]; - checkedValue++; - output.IRSensorLeft = data[checkedValue]; - checkedValue++; + parseIRSensorData(output, data, checkedValue); } else { - std::cerr << "Invalid data length for type 0x03" << std::endl; checkedValue += dataLength; } break; case 0x04: if (dataLength == 0x07) { - output.GyroAngle = data[checkedValue + 1] * 256 + data[checkedValue]; - checkedValue += 2; - output.GyroAngleRate = data[checkedValue + 1] * 256 + data[checkedValue]; - checkedValue += 5; // 3 unused + parseGyroData(output, data, checkedValue); } 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; + parseCliffSensorData(output, data, checkedValue); } else { - std::cerr << "Invalid data length for type 0x05" << std::endl; checkedValue += dataLength; } break; case 0x06: if (dataLength == 0x02) { - output.wheelCurrentLeft = data[checkedValue]; - checkedValue++; - output.wheelCurrentRight = data[checkedValue]; - checkedValue++; + parseWheelCurrentData(output, data, checkedValue); } else { - std::cerr << "Invalid data length for type 0x06" << std::endl; checkedValue += dataLength; } break; case 0x0A: if (dataLength == 0x04) { - output.extraInfo.HardwareVersionPatch = data[checkedValue]; - checkedValue++; - output.extraInfo.HardwareVersionMinor = data[checkedValue]; - checkedValue++; - output.extraInfo.HardwareVersionMajor = data[checkedValue]; - checkedValue += 2; + parseHardwareVersionData(output, data, checkedValue); } else { - std::cerr << "Invalid data length for type 0x0A" << std::endl; checkedValue += dataLength; } break; case 0x0B: if (dataLength == 0x04) { - output.extraInfo.FirmwareVersionPatch = data[checkedValue]; - checkedValue++; - output.extraInfo.FirmwareVersionMinor = data[checkedValue]; - checkedValue++; - output.extraInfo.FirmwareVersionMajor = data[checkedValue]; - checkedValue += 2; + parseFirmwareVersionData(output, data, checkedValue); } 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); - } + parseXYZData(output, data, checkedValue); } 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 + parseDigitalAnalogInputData(output, data, checkedValue); } else { - std::cerr << "Invalid data length for type 0x10" << std::endl; checkedValue += dataLength; } break; case 0x13: if (dataLength == 0x0C) { - 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; + parseUDIDData(output, data, checkedValue); } else { - std::cerr << "Invalid data length for type 0x13" << std::endl; checkedValue += dataLength; } break; @@ -206,6 +103,135 @@ int KobukiParser::parseKobukiMessage(TKobukiData &output, unsigned char *data) { } return 0; } + +void KobukiParser::parseBasicData(TKobukiData &output, unsigned char *data, int &checkedValue){ + output.timestamp = data[checkedValue + 1] * 256 + data[checkedValue]; + checkedValue += 2; + output.BumperCenter = (data[checkedValue] & 0x02) >> 1; + output.BumperLeft = (data[checkedValue] & 0x04) >> 2; + output.BumperRight = data[checkedValue] & 0x01; + checkedValue++; + output.WheelDropLeft = (data[checkedValue] & 0x02) >> 1; + output.WheelDropRight = data[checkedValue] & 0x01; + checkedValue++; + output.CliffCenter = (data[checkedValue] & 0x02) >> 1; + output.CliffLeft = (data[checkedValue] & 0x04) >> 2; + 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++; +} + +void KobukiParser::parseIRSensorData(TKobukiData &output, unsigned char *data, int &checkedValue){ + output.IRSensorRight = data[checkedValue]; + checkedValue++; + output.IRSensorCenter = data[checkedValue]; + checkedValue++; + output.IRSensorLeft = data[checkedValue]; + checkedValue++; +} + +void 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 +} + +void KobukiParser::parseCliffSensorData(TKobukiData &output, unsigned char *data, int &checkedValue){ + output.CliffSensorRight = data[checkedValue]; + checkedValue++; + output.CliffSensorCenter = data[checkedValue]; + checkedValue++; + output.CliffSensorLeft = data[checkedValue]; + checkedValue++; +} + +void KobukiParser::parseWheelCurrentData(TKobukiData &output, unsigned char *data, int &checkedValue){ + output.wheelCurrentLeft = data[checkedValue]; + checkedValue++; + output.wheelCurrentRight = data[checkedValue]; + checkedValue++; +} + +void 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; +} + +void 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; +} + +void 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); + } +} + +void 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 +} + +void 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::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 5bdb216..aca3eb0 100644 --- a/src/C++/Driver/src/KobukiParser.h +++ b/src/C++/Driver/src/KobukiParser.h @@ -38,17 +38,17 @@ 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 parseExtraData(TKobukiData &output, unsigned char *data, int &checkedValue); + void parseBasicData(TKobukiData &output, unsigned char *data, int &checkedValue); + void parseIRSensorData(TKobukiData &output, unsigned char *data, int &checkedValue); + void parseGyroData(TKobukiData &output, unsigned char *data, int &checkedValue); + void parseCliffSensorData(TKobukiData &output, unsigned char *data, int &checkedValue); + void parseWheelCurrentData(TKobukiData &output, unsigned char *data, int &checkedValue); + void parseHardwareVersionData(TKobukiData &output, unsigned char *data, int &checkedValue); + void parseFirmwareVersionData(TKobukiData &output, unsigned char *data, int &checkedValue); + void parseXYZData(TKobukiData &output, unsigned char *data, int &checkedValue); + void parseDigitalAnalogInputData(TKobukiData &output, unsigned char *data, int &checkedValue); + void parseUDIDData(TKobukiData &output, unsigned char *data, int &checkedValue); + void parseExtraData(TKobukiData &output, unsigned char *data, int &checkedValue); }; diff --git a/src/C++/Driver/src/test.cpp b/src/C++/Driver/src/test.cpp index 9609b6b..f846e7d 100644 --- a/src/C++/Driver/src/test.cpp +++ b/src/C++/Driver/src/test.cpp @@ -8,53 +8,56 @@ using namespace std; CKobuki robot; int command(); +const int forward = 1; +const int ROTATE = 2; + int main() { unsigned char *null_ptr(0); robot.startCommunication("/dev/ttyUSB0", true, null_ptr); usleep(1 * 1000 * 1000); - cout << "Enter commando: "; thread mv(command); usleep(30 * 1000 * 1000); mv.join(); //only exit once thread one is done running } -int checkCenterCliff() -{ - while (true) - { - std::cout << "cliffsensordata:" << robot.data.CliffSensorCenter << std::endl; - } -} +// int checkCenterCliff() +// { +// while (true) +// { +// std::cout << "cliffsensordata:" << robot.parser.data.CliffSensorCenter << std::endl; +// } +// } -int checkWheelDrop(){ - while (true) - { - std::cout << "wheeldropdata:" << robot.data.WheelDropLeft << std::endl; - } -} +// int checkWheelDrop(){ +// while (true) +// { +// std::cout << "wheeldropdata:" << robot.parser.data.WheelDropLeft << std::endl; +// } +// } int command(){ - int input; - + cout << "choose between forward and rotate" << endl; + cout << "What must the robot do?"; cin >> input; switch(input){ - case 1: - robot.goStraight(1); - robot.doRotation(90); - robot.forward(1024, -1); + case forward:{ + int distance; + std::cout >> "Enter distance to move forward: "; + std::cin >> distance; + robot.goStraight(distance); + } + + case ROTATE:{ + int angle; + std::cout >> "Enter angle to rotate: "; + std::cin >> angle; + robot.doRotation(angle); + } robot.goStraight(-1); break; - case 2: - robot.doRotation(90); - break; - case 3: - robot.doRotation(-90); - break; - case 4: - robot.forward(1024, -1); - break; + default: cout << "Invalid input" << endl; break;