diff --git a/src/C++/Driver/src/KobukiParser.cpp b/src/C++/Driver/src/KobukiParser.cpp index 006f341..0c1e957 100644 --- a/src/C++/Driver/src/KobukiParser.cpp +++ b/src/C++/Driver/src/KobukiParser.cpp @@ -2,6 +2,7 @@ #include //checkedValue const maken //bitwise operators nachecken met website van kobuki serial website +//cliffsensor kan rauwe getallen zijn int KobukiParser::parseKobukiMessage(TKobukiData &output, unsigned char *data) { int rtrnvalue = checkChecksum(data); if (rtrnvalue != 0) { @@ -152,13 +153,13 @@ void KobukiParser::parseGyroData(TKobukiData &output, unsigned char *data, int & 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::parseCliffSensorData(TKobukiData &output, unsigned char *data, int &checkedValue) { + output.CliffSensorRight = (data[checkedValue] << 8) | data[checkedValue + 1]; + checkedValue += 2; + output.CliffSensorCenter = (data[checkedValue] << 8) | data[checkedValue + 1]; + checkedValue += 2; + output.CliffSensorLeft = (data[checkedValue] << 8) | data[checkedValue + 1]; + checkedValue += 2; } void KobukiParser::parseWheelCurrentData(TKobukiData &output, unsigned char *data, int &checkedValue){