From 76c088bd50a82fa6605e4245fbe77e0542404ed2 Mon Sep 17 00:00:00 2001 From: Sam Hos Date: Mon, 7 Oct 2024 15:49:52 +0200 Subject: [PATCH] can finaly read from main now parser cleanup (again) --- src/C++/Driver/src/CKobuki.h | 2 +- src/C++/Driver/src/KobukiParser.cpp | 291 ++++++++++++++++++---------- src/C++/Driver/src/KobukiParser.h | 2 + src/C++/Driver/src/main.cpp | 10 +- 4 files changed, 197 insertions(+), 108 deletions(-) diff --git a/src/C++/Driver/src/CKobuki.h b/src/C++/Driver/src/CKobuki.h index 9a99197..f7f9e4e 100755 --- a/src/C++/Driver/src/CKobuki.h +++ b/src/C++/Driver/src/CKobuki.h @@ -160,13 +160,13 @@ public: void doRotation(long double th); void goToXy(long double xx, long double yy); std::ofstream odometry_log; + KobukiParser parser; private: - KobukiParser parser; int HCom; pthread_t threadHandle; // handle to the thread diff --git a/src/C++/Driver/src/KobukiParser.cpp b/src/C++/Driver/src/KobukiParser.cpp index c7ae3b5..2dcd1f0 100644 --- a/src/C++/Driver/src/KobukiParser.cpp +++ b/src/C++/Driver/src/KobukiParser.cpp @@ -2,119 +2,206 @@ #include int KobukiParser::parseKobukiMessage(TKobukiData &output, unsigned char *data) { - //check if the checksum is correct - std::cout << "Parsing message" << std::endl; int rtrnvalue = checkChecksum(data); + // ak je zly checksum,tak kaslat na to if (rtrnvalue != 0) return -2; int checkedValue = 1; - while (checkedValue < data[0]) { - unsigned char header = data[checkedValue]; - unsigned char length = data[checkedValue + 1]; - - switch (header) { - case 0x01: - if (length == 0x0F) { - parseBasicData(output, data, checkedValue); - } - break; - // case 0x03: - // if (length == 0x03) { - // parseIRSensorData(output, data, checkedValue); - // } - // break; - // case 0x04: - // if (length == 0x07) { - // parseGyroData(output, data, checkedValue); - // } - // break; - // case 0x05: - // if (length == 0x06) { - // parseCliffSensorData(output, data, checkedValue); - // } - // break; - // case 0x06: - // if (length == 0x02) { - // parseWheelCurrentData(output, data, checkedValue); - // } - // break; - // case 0x0A: - // if (length == 0x04) { - // parseHardwareVersionData(output, data, checkedValue); - // } - // break; - // case 0x0B: - // if (length == 0x04) { - // parseFirmwareVersionData(output, data, checkedValue); - // } - // break; - // case 0x0D: - // if (length == 0x0D) { - // parseXYZData(output, data, checkedValue); - // } - // break; - // case 0x10: - // if (length == 0x10) { - // parseDigitalAnalogInputData(output, data, checkedValue); - // } - // break; - // case 0x13: - // if (length == 0x0C) { - // parseUDIDData(output, data, checkedValue); - // } - // break; - default: - checkedValue += length + 2; // Skip data block if nothing matches - std::cout << "Unknown header: " << header << std::endl; - break; + // 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; + 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++; + } + 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; } -int KobukiParser::parseBasicData(TKobukiData &output, unsigned char *data, int &checkedValue) { - checkedValue += 2; // Skip header and length - 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++; - return 0; -} - - - -int KobukiParser::checkChecksum(unsigned char *data) -{ // najprv hlavicku +int KobukiParser::checkChecksum(unsigned char *data) { unsigned char chckSum = 0; - for (int i = 0; i < data[0] + 2; i++) - { + for (int i = 0; i < data[0] + 2; i++) { chckSum ^= data[i]; } - return chckSum; // 0 ak je vsetko v poriadku,inak nejake cislo + return chckSum; // 0 if everything is correct, otherwise some number } \ No newline at end of file diff --git a/src/C++/Driver/src/KobukiParser.h b/src/C++/Driver/src/KobukiParser.h index 783ddf4..1e79bbd 100644 --- a/src/C++/Driver/src/KobukiParser.h +++ b/src/C++/Driver/src/KobukiParser.h @@ -26,6 +26,8 @@ struct TKobukiData { int wheelCurrentLeft, wheelCurrentRight; int digitalInput, analogInputCh0, analogInputCh1, analogInputCh2, analogInputCh3; int frameId; + TExtraInfo extraInfo; // Add extraInfo member + std::vector gyroData; // Add gyroData member }; class KobukiParser { diff --git a/src/C++/Driver/src/main.cpp b/src/C++/Driver/src/main.cpp index a52cfdd..62d4cec 100644 --- a/src/C++/Driver/src/main.cpp +++ b/src/C++/Driver/src/main.cpp @@ -4,12 +4,10 @@ #include #include "graph.h" - using namespace std; CKobuki robot; int movement(); int checkCenterCliff(); - KobukiParser parser; int main() { @@ -19,16 +17,18 @@ int main() // mv.join(); //only exit once thread one is done running checkCenterCliff(); + + return 0; } int checkCenterCliff() { - while (true) - { - std::cout << "cliffsensordata:" << parser.data.CliffCenter << std::endl; + while(true){ + std::cout << robot.parser.data.CliffSensorCenter << endl; } } + int movement() { int text;