mirror of
https://gitlab.fdmci.hva.nl/technische-informatica-sm3/ti-projectten/rooziinuubii79.git
synced 2025-08-03 20:04:58 +00:00
changed funtions so they point to new class
This commit is contained in:
@@ -9,6 +9,8 @@ static std::vector<float> vectorX;
|
||||
static std::vector<float> vectorY;
|
||||
static std::vector<float> vectorGyroTheta;
|
||||
|
||||
KobukiParser parser;
|
||||
|
||||
// obsluha tty pod unixom
|
||||
int set_interface_attribs2(int fd, int speed, int parity)
|
||||
{
|
||||
@@ -163,16 +165,6 @@ unsigned char *CKobuki::readKobukiMessage()
|
||||
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)
|
||||
{
|
||||
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");
|
||||
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
|
||||
// citat
|
||||
@@ -310,202 +302,202 @@ int CKobuki::measure()
|
||||
return 0;
|
||||
}
|
||||
|
||||
int CKobuki::parseKobukiMessage(TKobukiData &output, unsigned char *data)
|
||||
{
|
||||
int rtrnvalue = checkChecksum(data);
|
||||
// ak je zly checksum,tak kaslat na to
|
||||
if (rtrnvalue != 0)
|
||||
return -2;
|
||||
// int CKobuki::parseKobukiMessage(TKobukiData &output, unsigned char *data)
|
||||
// {
|
||||
// int rtrnvalue = checkChecksum(data);
|
||||
// // ak je zly checksum,tak kaslat na to
|
||||
// if (rtrnvalue != 0)
|
||||
// return -2;
|
||||
|
||||
int checkedValue = 1;
|
||||
// kym neprejdeme celu dlzku
|
||||
while (checkedValue < data[0])
|
||||
{
|
||||
// basic data subload
|
||||
if (data[checkedValue] == 0x01)
|
||||
{
|
||||
checkedValue++;
|
||||
if (data[checkedValue] != 0x0F)
|
||||
return -1;
|
||||
checkedValue++;
|
||||
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++;
|
||||
}
|
||||
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;
|
||||
}
|
||||
// int checkedValue = 1;
|
||||
// // kym neprejdeme celu dlzku
|
||||
// while (checkedValue < data[0])
|
||||
// {
|
||||
// // basic data subload
|
||||
// if (data[checkedValue] == 0x01)
|
||||
// {
|
||||
// checkedValue++;
|
||||
// if (data[checkedValue] != 0x0F)
|
||||
// return -1;
|
||||
// checkedValue++;
|
||||
// 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++;
|
||||
// }
|
||||
// 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;
|
||||
// }
|
||||
|
||||
long double CKobuki::gyroToRad(signed short GyroAngle)
|
||||
{
|
||||
|
||||
Reference in New Issue
Block a user