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

changed funtions so they point to new class

Browse Source
This commit is contained in:
Sam Hos
2024-10-07 11:21:11 +02:00
parent 3184a09dc7
commit 83f7d447d3
1 changed files with 198 additions and 206 deletions
Download Patch File Download Diff File Expand all files Collapse all files

404
src/C++/Driver/src/CKobuki.cpp
View File

@@ -9,6 +9,8 @@ static std::vector<float> vectorX;
static std::vector<float> vectorY; static std::vector<float> vectorY;
static std::vector<float> vectorGyroTheta; static std::vector<float> vectorGyroTheta;
KobukiParser parser;
// obsluha tty pod unixom // obsluha tty pod unixom
int set_interface_attribs2(int fd, int speed, int parity) int set_interface_attribs2(int fd, int speed, int parity)
{ {
@@ -163,16 +165,6 @@ unsigned char *CKobuki::readKobukiMessage()
return null_buffer; return null_buffer;
} }
int CKobuki::checkChecksum(unsigned char *data)
{ // najprv hlavicku
unsigned char chckSum = 0;
for (int i = 0; i < data[0] + 2; i++)
{
chckSum ^= data[i];
}
return chckSum; // 0 ak je vsetko v poriadku,inak nejake cislo
}
void CKobuki::setLed(int led1, int led2) void CKobuki::setLed(int led1, int led2)
{ {
unsigned char message[8] = {0xaa, 0x55, 0x04, 0x0c, 0x02, 0x00, (unsigned char)((led1 + led2 * 4) % 256), 0x00}; unsigned char message[8] = {0xaa, 0x55, 0x04, 0x0c, 0x02, 0x00, (unsigned char)((led1 + led2 * 4) % 256), 0x00};
@@ -297,7 +289,7 @@ int CKobuki::measure()
// printf("vratil null message\n"); // printf("vratil null message\n");
continue; continue;
} }
int ok = parseKobukiMessage(data, message); int ok = parser.parseKobukiMessage(parser.data, message);
// maximalne moze trvat callback funkcia 20 ms, ak by trvala viac, nestihame // maximalne moze trvat callback funkcia 20 ms, ak by trvala viac, nestihame
// citat // citat
@@ -310,202 +302,202 @@ int CKobuki::measure()
return 0; return 0;
} }
int CKobuki::parseKobukiMessage(TKobukiData &output, unsigned char *data) // int CKobuki::parseKobukiMessage(TKobukiData &output, unsigned char *data)
{ // {
int rtrnvalue = checkChecksum(data); // int rtrnvalue = checkChecksum(data);
// ak je zly checksum,tak kaslat na to // // ak je zly checksum,tak kaslat na to
if (rtrnvalue != 0) // if (rtrnvalue != 0)
return -2; // return -2;
int checkedValue = 1; // int checkedValue = 1;
// kym neprejdeme celu dlzku // // kym neprejdeme celu dlzku
while (checkedValue < data[0]) // while (checkedValue < data[0])
{ // {
// basic data subload // // basic data subload
if (data[checkedValue] == 0x01) // if (data[checkedValue] == 0x01)
{ // {
checkedValue++; // checkedValue++;
if (data[checkedValue] != 0x0F) // if (data[checkedValue] != 0x0F)
return -1; // return -1;
checkedValue++; // checkedValue++;
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; // output.BumperLeft = data[checkedValue] && 0x04;
output.BumperRight = data[checkedValue] && 0x01; // 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;
output.EncoderRight = data[checkedValue + 1] * 256 + data[checkedValue]; // output.EncoderRight = data[checkedValue + 1] * 256 + data[checkedValue];
checkedValue += 2; // checkedValue += 2;
output.PWMleft = data[checkedValue]; // output.PWMleft = data[checkedValue];
checkedValue++; // checkedValue++;
output.PWMright = data[checkedValue]; // output.PWMright = data[checkedValue];
checkedValue++; // checkedValue++;
output.ButtonPress = data[checkedValue]; // output.ButtonPress = data[checkedValue];
checkedValue++; // checkedValue++;
output.Charger = data[checkedValue]; // output.Charger = data[checkedValue];
checkedValue++; // checkedValue++;
output.Battery = data[checkedValue]; // output.Battery = data[checkedValue];
checkedValue++; // checkedValue++;
output.overCurrent = data[checkedValue]; // output.overCurrent = data[checkedValue];
checkedValue++; // checkedValue++;
} // }
else if (data[checkedValue] == 0x03) // else if (data[checkedValue] == 0x03)
{ // {
checkedValue++; // checkedValue++;
if (data[checkedValue] != 0x03) // if (data[checkedValue] != 0x03)
return -3; // return -3;
checkedValue++; // checkedValue++;
output.IRSensorRight = data[checkedValue]; // output.IRSensorRight = data[checkedValue];
checkedValue++; // checkedValue++;
output.IRSensorCenter = data[checkedValue]; // output.IRSensorCenter = data[checkedValue];
checkedValue++; // checkedValue++;
output.IRSensorLeft = data[checkedValue]; // output.IRSensorLeft = data[checkedValue];
checkedValue++; // checkedValue++;
} // }
else if (data[checkedValue] == 0x04) // else if (data[checkedValue] == 0x04)
{ // {
checkedValue++; // checkedValue++;
if (data[checkedValue] != 0x07) // if (data[checkedValue] != 0x07)
return -4; // return -4;
checkedValue++; // checkedValue++;
output.GyroAngle = data[checkedValue + 1] * 256 + data[checkedValue]; // output.GyroAngle = data[checkedValue + 1] * 256 + data[checkedValue];
checkedValue += 2; // checkedValue += 2;
output.GyroAngleRate = data[checkedValue + 1] * 256 + data[checkedValue]; // output.GyroAngleRate = data[checkedValue + 1] * 256 + data[checkedValue];
checkedValue += 5; // 3 unsued // checkedValue += 5; // 3 unsued
} // }
else if (data[checkedValue] == 0x05) // else if (data[checkedValue] == 0x05)
{ // {
checkedValue++; // checkedValue++;
if (data[checkedValue] != 0x06) // if (data[checkedValue] != 0x06)
return -5; // return -5;
checkedValue++; // checkedValue++;
output.CliffSensorRight = // output.CliffSensorRight =
data[checkedValue + 1] * 256 + data[checkedValue]; // data[checkedValue + 1] * 256 + data[checkedValue];
checkedValue += 2; // checkedValue += 2;
output.CliffSensorCenter = // output.CliffSensorCenter =
data[checkedValue + 1] * 256 + data[checkedValue]; // data[checkedValue + 1] * 256 + data[checkedValue];
checkedValue += 2; // checkedValue += 2;
output.CliffSensorLeft = // output.CliffSensorLeft =
data[checkedValue + 1] * 256 + data[checkedValue]; // data[checkedValue + 1] * 256 + data[checkedValue];
checkedValue += 2; // checkedValue += 2;
} // }
else if (data[checkedValue] == 0x06) // else if (data[checkedValue] == 0x06)
{ // {
checkedValue++; // checkedValue++;
if (data[checkedValue] != 0x02) // if (data[checkedValue] != 0x02)
return -6; // return -6;
checkedValue++; // checkedValue++;
output.wheelCurrentLeft = data[checkedValue]; // output.wheelCurrentLeft = data[checkedValue];
checkedValue++; // checkedValue++;
output.wheelCurrentRight = data[checkedValue]; // output.wheelCurrentRight = data[checkedValue];
checkedValue++; // checkedValue++;
} // }
else if (data[checkedValue] == 0x0A) // else if (data[checkedValue] == 0x0A)
{ // {
checkedValue++; // checkedValue++;
if (data[checkedValue] != 0x04) // if (data[checkedValue] != 0x04)
return -7; // return -7;
checkedValue++; // checkedValue++;
output.extraInfo.HardwareVersionPatch = data[checkedValue]; // output.extraInfo.HardwareVersionPatch = data[checkedValue];
checkedValue++; // checkedValue++;
output.extraInfo.HardwareVersionMinor = data[checkedValue]; // output.extraInfo.HardwareVersionMinor = data[checkedValue];
checkedValue++; // checkedValue++;
output.extraInfo.HardwareVersionMajor = data[checkedValue]; // output.extraInfo.HardwareVersionMajor = data[checkedValue];
checkedValue += 2; // checkedValue += 2;
} // }
else if (data[checkedValue] == 0x0B) // else if (data[checkedValue] == 0x0B)
{ // {
checkedValue++; // checkedValue++;
if (data[checkedValue] != 0x04) // if (data[checkedValue] != 0x04)
return -8; // return -8;
checkedValue++; // checkedValue++;
output.extraInfo.FirmwareVersionPatch = data[checkedValue]; // output.extraInfo.FirmwareVersionPatch = data[checkedValue];
checkedValue++; // checkedValue++;
output.extraInfo.FirmwareVersionMinor = data[checkedValue]; // output.extraInfo.FirmwareVersionMinor = data[checkedValue];
checkedValue++; // checkedValue++;
output.extraInfo.FirmwareVersionMajor = data[checkedValue]; // output.extraInfo.FirmwareVersionMajor = data[checkedValue];
checkedValue += 2; // checkedValue += 2;
} // }
else if (data[checkedValue] == 0x0D) // else if (data[checkedValue] == 0x0D)
{ // {
checkedValue++; // checkedValue++;
if (data[checkedValue] % 2 != 0) // if (data[checkedValue] % 2 != 0)
return -9; // return -9;
checkedValue++; // checkedValue++;
output.frameId = data[checkedValue]; // output.frameId = data[checkedValue];
checkedValue++; // checkedValue++;
int howmanyFrames = data[checkedValue] / 3; // int howmanyFrames = data[checkedValue] / 3;
checkedValue++; // checkedValue++;
output.gyroData.reserve(howmanyFrames); // output.gyroData.reserve(howmanyFrames);
output.gyroData.clear(); // output.gyroData.clear();
for (int hk = 0; hk < howmanyFrames; hk++) // for (int hk = 0; hk < howmanyFrames; hk++)
{ // {
TRawGyroData temp; // TRawGyroData temp;
temp.x = data[checkedValue + 1] * 256 + data[checkedValue]; // temp.x = data[checkedValue + 1] * 256 + data[checkedValue];
checkedValue += 2; // checkedValue += 2;
temp.y = data[checkedValue + 1] * 256 + data[checkedValue]; // temp.y = data[checkedValue + 1] * 256 + data[checkedValue];
checkedValue += 2; // checkedValue += 2;
temp.z = data[checkedValue + 1] * 256 + data[checkedValue]; // temp.z = data[checkedValue + 1] * 256 + data[checkedValue];
checkedValue += 2; // checkedValue += 2;
output.gyroData.push_back(temp); // output.gyroData.push_back(temp);
} // }
} // }
else if (data[checkedValue] == 0x10) // else if (data[checkedValue] == 0x10)
{ // {
checkedValue++; // checkedValue++;
if (data[checkedValue] != 0x10) // if (data[checkedValue] != 0x10)
return -10; // return -10;
checkedValue++; // checkedValue++;
output.digitalInput = data[checkedValue + 1] * 256 + data[checkedValue]; // output.digitalInput = data[checkedValue + 1] * 256 + data[checkedValue];
checkedValue += 2; // checkedValue += 2;
output.analogInputCh0 = data[checkedValue + 1] * 256 + data[checkedValue]; // output.analogInputCh0 = data[checkedValue + 1] * 256 + data[checkedValue];
checkedValue += 2; // checkedValue += 2;
output.analogInputCh1 = data[checkedValue + 1] * 256 + data[checkedValue]; // output.analogInputCh1 = data[checkedValue + 1] * 256 + data[checkedValue];
checkedValue += 2; // checkedValue += 2;
output.analogInputCh2 = data[checkedValue + 1] * 256 + data[checkedValue]; // output.analogInputCh2 = data[checkedValue + 1] * 256 + data[checkedValue];
checkedValue += 2; // checkedValue += 2;
output.analogInputCh3 = data[checkedValue + 1] * 256 + data[checkedValue]; // output.analogInputCh3 = data[checkedValue + 1] * 256 + data[checkedValue];
checkedValue += 8; // 2+6 // checkedValue += 8; // 2+6
} // }
else if (data[checkedValue] == 0x13) // else if (data[checkedValue] == 0x13)
{ // {
checkedValue++; // checkedValue++;
if (data[checkedValue] != 0x0C) // if (data[checkedValue] != 0x0C)
return -11; // return -11;
checkedValue++; // checkedValue++;
output.extraInfo.UDID0 = data[checkedValue + 3] * 256 * 256 * 256 + // output.extraInfo.UDID0 = data[checkedValue + 3] * 256 * 256 * 256 +
data[checkedValue + 2] * 256 * 256 + // data[checkedValue + 2] * 256 * 256 +
data[checkedValue + 1] * 256 + // data[checkedValue + 1] * 256 +
data[checkedValue]; // data[checkedValue];
checkedValue += 4; // checkedValue += 4;
output.extraInfo.UDID1 = data[checkedValue + 3] * 256 * 256 * 256 + // output.extraInfo.UDID1 = data[checkedValue + 3] * 256 * 256 * 256 +
data[checkedValue + 2] * 256 * 256 + // data[checkedValue + 2] * 256 * 256 +
data[checkedValue + 1] * 256 + // data[checkedValue + 1] * 256 +
data[checkedValue]; // data[checkedValue];
checkedValue += 4; // checkedValue += 4;
output.extraInfo.UDID2 = data[checkedValue + 3] * 256 * 256 * 256 + // output.extraInfo.UDID2 = data[checkedValue + 3] * 256 * 256 * 256 +
data[checkedValue + 2] * 256 * 256 + // data[checkedValue + 2] * 256 * 256 +
data[checkedValue + 1] * 256 + // data[checkedValue + 1] * 256 +
data[checkedValue]; // data[checkedValue];
checkedValue += 4; // checkedValue += 4;
} // }
else // else
{ // {
checkedValue++; // checkedValue++;
checkedValue += data[checkedValue] + 1; // checkedValue += data[checkedValue] + 1;
} // }
} // }
return 0; // return 0;
} // }
long double CKobuki::gyroToRad(signed short GyroAngle) long double CKobuki::gyroToRad(signed short GyroAngle)
{ {