Smikkelbakje/J2S1-Kobuki
1
0
Fork 0
mirror of https://gitlab.fdmci.hva.nl/technische-informatica-sm3/ti-projectten/rooziinuubii79.git synced 2025-08-04 04:14:58 +00:00
Code Issues Actions Packages Projects Releases Wiki Activity

start parser cleanup

Browse Source
This commit is contained in:
Sam Hos
2024-10-05 15:44:28 +02:00
parent 34db8b8e35
commit 50fc14d92d
2 changed files with 156 additions and 0 deletions
Download Patch File Download Diff File Expand all files Collapse all files

105
src/C++/Driver/src/KobukiParser.cpp Normal file
View 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
View 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