mirror of
https://gitlab.fdmci.hva.nl/technische-informatica-sm3/ti-projectten/rooziinuubii79.git
synced 2025-08-04 04:14:58 +00:00
start parser cleanup
This commit is contained in:
105
src/C++/Driver/src/KobukiParser.cpp
Normal file
105
src/C++/Driver/src/KobukiParser.cpp
Normal file
@@ -0,0 +1,105 @@
|
|||||||
|
#include "KobukiParser.h"
|
||||||
|
|
||||||
|
int KobukiParser::parseKobukiMessage(TKobukiData &output, unsigned char *data) {
|
||||||
|
int rtrnvalue = checkChecksum(data);
|
||||||
|
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 unknown data block
|
||||||
|
break;
|
||||||
|
}
|
||||||
|
}
|
||||||
|
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;
|
||||||
|
}
|
||||||
|
|
51
src/C++/Driver/src/KobukiParser.h
Normal file
51
src/C++/Driver/src/KobukiParser.h
Normal file
@@ -0,0 +1,51 @@
|
|||||||
|
#ifndef KOBUKIPARSER_H
|
||||||
|
#define KOBUKIPARSER_H
|
||||||
|
|
||||||
|
#include <vector>
|
||||||
|
|
||||||
|
struct TRawGyroData {
|
||||||
|
int x, y, z;
|
||||||
|
};
|
||||||
|
|
||||||
|
struct TExtraInfo {
|
||||||
|
int HardwareVersionPatch, HardwareVersionMinor, HardwareVersionMajor;
|
||||||
|
int FirmwareVersionPatch, FirmwareVersionMinor, FirmwareVersionMajor;
|
||||||
|
int UDID0, UDID1, UDID2;
|
||||||
|
};
|
||||||
|
|
||||||
|
struct TKobukiData {
|
||||||
|
int timestamp, BumperCenter, BumperLeft, BumperRight;
|
||||||
|
int WheelDropLeft, WheelDropRight;
|
||||||
|
int CliffCenter, CliffLeft, CliffRight;
|
||||||
|
int EncoderLeft, EncoderRight;
|
||||||
|
int PWMleft, PWMright;
|
||||||
|
int ButtonPress, Charger, Battery, overCurrent;
|
||||||
|
int IRSensorRight, IRSensorCenter, IRSensorLeft;
|
||||||
|
int GyroAngle, GyroAngleRate;
|
||||||
|
int CliffSensorRight, CliffSensorCenter, CliffSensorLeft;
|
||||||
|
int wheelCurrentLeft, wheelCurrentRight;
|
||||||
|
int digitalInput, analogInputCh0, analogInputCh1, analogInputCh2, analogInputCh3;
|
||||||
|
int frameId;
|
||||||
|
std::vector<TRawGyroData> gyroData;
|
||||||
|
TExtraInfo extraInfo;
|
||||||
|
};
|
||||||
|
|
||||||
|
class KobukiParser {
|
||||||
|
public:
|
||||||
|
int parseKobukiMessage(TKobukiData &output, unsigned char *data);
|
||||||
|
|
||||||
|
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);
|
||||||
|
};
|
||||||
|
|
||||||
|
#endif // KOBUKIPARSER_H
|
Reference in New Issue
Block a user