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

can finaly read from main

Browse Source 
now parser cleanup (again)
This commit is contained in:
Sam Hos
2024-10-07 15:49:52 +02:00
parent 7e4884f1bc
commit 76c088bd50
4 changed files with 197 additions and 108 deletions
Download Patch File Download Diff File Expand all files Collapse all files

2
src/C++/Driver/src/CKobuki.h
View File

@@ -160,13 +160,13 @@ public:
void doRotation(long double th); void doRotation(long double th);
void goToXy(long double xx, long double yy); void goToXy(long double xx, long double yy);
std::ofstream odometry_log; std::ofstream odometry_log;
KobukiParser parser;
private: private:
KobukiParser parser;
int HCom; int HCom;
pthread_t threadHandle; // handle to the thread pthread_t threadHandle; // handle to the thread

253
src/C++/Driver/src/KobukiParser.cpp
View File

@@ -2,91 +2,35 @@
#include <iostream> #include <iostream>
int KobukiParser::parseKobukiMessage(TKobukiData &output, unsigned char *data) { int KobukiParser::parseKobukiMessage(TKobukiData &output, unsigned char *data) {
//check if the checksum is correct
std::cout << "Parsing message" << std::endl;
int rtrnvalue = checkChecksum(data); int rtrnvalue = checkChecksum(data);
// ak je zly checksum,tak kaslat na to
if (rtrnvalue != 0) if (rtrnvalue != 0)
return -2; return -2;
int checkedValue = 1; int checkedValue = 1;
while (checkedValue < data[0]) { // kym neprejdeme celu dlzku
unsigned char header = data[checkedValue]; while (checkedValue < data[0])
unsigned char length = data[checkedValue + 1]; {
// basic data subload
switch (header) { if (data[checkedValue] == 0x01)
case 0x01: {
if (length == 0x0F) { checkedValue++;
parseBasicData(output, data, checkedValue); if (data[checkedValue] != 0x0F)
} return -1;
break; checkedValue++;
// 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;
}
}
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]; output.timestamp = data[checkedValue + 1] * 256 + data[checkedValue];
checkedValue += 2; checkedValue += 2;
output.BumperCenter = data[checkedValue] & 0x02; output.BumperCenter = data[checkedValue] && 0x02;
output.BumperLeft = data[checkedValue] & 0x04; std::cout << "BumperCenter: " << output.BumperCenter << std::endl;
output.BumperRight = data[checkedValue] & 0x01; output.BumperLeft = data[checkedValue] && 0x04;
output.BumperRight = data[checkedValue] && 0x01;
checkedValue++; checkedValue++;
output.WheelDropLeft = data[checkedValue] & 0x02; output.WheelDropLeft = data[checkedValue] && 0x02;
output.WheelDropRight = data[checkedValue] & 0x01; output.WheelDropRight = data[checkedValue] && 0x01;
checkedValue++; checkedValue++;
output.CliffCenter = data[checkedValue] & 0x02; output.CliffCenter = data[checkedValue] && 0x02;
output.CliffLeft = data[checkedValue] & 0x04; output.CliffLeft = data[checkedValue] && 0x04;
output.CliffRight = data[checkedValue] & 0x01; output.CliffRight = data[checkedValue] && 0x01;
checkedValue++; checkedValue++;
output.EncoderLeft = data[checkedValue + 1] * 256 + data[checkedValue]; output.EncoderLeft = data[checkedValue + 1] * 256 + data[checkedValue];
checkedValue += 2; checkedValue += 2;
@@ -104,17 +48,160 @@ int KobukiParser::parseBasicData(TKobukiData &output, unsigned char *data, int &
checkedValue++; checkedValue++;
output.overCurrent = data[checkedValue]; output.overCurrent = data[checkedValue];
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; return 0;
} }
int KobukiParser::checkChecksum(unsigned char *data) {
int KobukiParser::checkChecksum(unsigned char *data)
{ // najprv hlavicku
unsigned char chckSum = 0; 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]; chckSum ^= data[i];
} }
return chckSum; // 0 ak je vsetko v poriadku,inak nejake cislo return chckSum; // 0 if everything is correct, otherwise some number
} }

2
src/C++/Driver/src/KobukiParser.h
View File

@@ -26,6 +26,8 @@ struct TKobukiData {
int wheelCurrentLeft, wheelCurrentRight; int wheelCurrentLeft, wheelCurrentRight;
int digitalInput, analogInputCh0, analogInputCh1, analogInputCh2, analogInputCh3; int digitalInput, analogInputCh0, analogInputCh1, analogInputCh2, analogInputCh3;
int frameId; int frameId;
TExtraInfo extraInfo; // Add extraInfo member
std::vector<TRawGyroData> gyroData; // Add gyroData member
}; };
class KobukiParser { class KobukiParser {

10
src/C++/Driver/src/main.cpp
View File

@@ -4,12 +4,10 @@
#include <thread> #include <thread>
#include "graph.h" #include "graph.h"
using namespace std; using namespace std;
CKobuki robot; CKobuki robot;
int movement(); int movement();
int checkCenterCliff(); int checkCenterCliff();
KobukiParser parser;
int main() int main()
{ {
@@ -19,16 +17,18 @@ int main()
// mv.join(); //only exit once thread one is done running // mv.join(); //only exit once thread one is done running
checkCenterCliff(); checkCenterCliff();
return 0;
} }
int checkCenterCliff() int checkCenterCliff()
{ {
while (true) while(true){
{ std::cout << robot.parser.data.CliffSensorCenter << endl;
std::cout << "cliffsensordata:" << parser.data.CliffCenter << std::endl;
} }
} }
int movement() int movement()
{ {
int text; int text;