diff --git a/src/C++/Driver/src/CKobuki.cpp b/src/C++/Driver/src/CKobuki.cpp index ffde869..7731f31 100755 --- a/src/C++/Driver/src/CKobuki.cpp +++ b/src/C++/Driver/src/CKobuki.cpp @@ -9,6 +9,8 @@ static std::vector vectorX; static std::vector vectorY; static std::vector 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) {