diff --git a/src/C++/Driver/src/CKobuki.h b/src/C++/Driver/src/CKobuki.h index bf0a75c..963f20d 100755 --- a/src/C++/Driver/src/CKobuki.h +++ b/src/C++/Driver/src/CKobuki.h @@ -32,91 +32,92 @@ #include #include "graph.h" +#include "KobukiParser.h" using namespace std; -typedef struct -{ +// typedef struct +// { - unsigned short x; - unsigned short y; - unsigned short z; +// unsigned short x; +// unsigned short y; +// unsigned short z; -}TRawGyroData; -typedef struct -{ - //Hardware Version - unsigned char HardwareVersionMajor; - unsigned char HardwareVersionMinor; - unsigned char HardwareVersionPatch; - //Firmware Version - unsigned char FirmwareVersionMajor; - unsigned char FirmwareVersionMinor; - unsigned char FirmwareVersionPatch; +// }TRawGyroData; +// typedef struct +// { +// //Hardware Version +// unsigned char HardwareVersionMajor; +// unsigned char HardwareVersionMinor; +// unsigned char HardwareVersionPatch; +// //Firmware Version +// unsigned char FirmwareVersionMajor; +// unsigned char FirmwareVersionMinor; +// unsigned char FirmwareVersionPatch; - //Unique Device IDentifier(UDID) - unsigned int UDID0; - unsigned int UDID1; - unsigned int UDID2; - //Controller Info - unsigned char PIDtype; - unsigned int PIDgainP; - unsigned int PIDgainI; - unsigned int PIDgainD; -}TExtraRequestData; +// //Unique Device IDentifier(UDID) +// unsigned int UDID0; +// unsigned int UDID1; +// unsigned int UDID2; +// //Controller Info +// unsigned char PIDtype; +// unsigned int PIDgainP; +// unsigned int PIDgainI; +// unsigned int PIDgainD; +// }TExtraRequestData; -typedef struct -{ - //---basic package - unsigned short timestamp; - //bumpers - bool BumperLeft; - bool BumperCenter; - bool BumperRight; - //cliff - bool CliffLeft; - bool CliffCenter; - bool CliffRight; - // wheel drop - bool WheelDropLeft; - bool WheelDropRight; - //wheel rotation - unsigned short EncoderRight; - unsigned short EncoderLeft; - unsigned char PWMright; - unsigned char PWMleft; - //buttons - unsigned char ButtonPress;// 0 no, 1 2 4 for button 0 1 2 (7 is all three) - //power - unsigned char Charger; - unsigned char Battery; - unsigned char overCurrent; - //---docking ir - unsigned char IRSensorRight; - unsigned char IRSensorCenter; - unsigned char IRSensorLeft; - //---Inertial Sensor Data - signed short GyroAngle; - unsigned short GyroAngleRate; - //---Cliff Sensor Data - unsigned short CliffSensorRight; - unsigned short CliffSensorCenter; - unsigned short CliffSensorLeft; - //---Current - unsigned char wheelCurrentLeft; - unsigned char wheelCurrentRight; - //---Raw Data Of 3D Gyro - unsigned char frameId; - std::vector gyroData; - //---General Purpose Input - unsigned short digitalInput; - unsigned short analogInputCh0; - unsigned short analogInputCh1; - unsigned short analogInputCh2; - unsigned short analogInputCh3; - //---structure with data that appears only on request - TExtraRequestData extraInfo; -}TKobukiData; +// typedef struct +// { +// //---basic package +// unsigned short timestamp; +// //bumpers +// bool BumperLeft; +// bool BumperCenter; +// bool BumperRight; +// //cliff +// bool CliffLeft; +// bool CliffCenter; +// bool CliffRight; +// // wheel drop +// bool WheelDropLeft; +// bool WheelDropRight; +// //wheel rotation +// unsigned short EncoderRight; +// unsigned short EncoderLeft; +// unsigned char PWMright; +// unsigned char PWMleft; +// //buttons +// unsigned char ButtonPress;// 0 no, 1 2 4 for button 0 1 2 (7 is all three) +// //power +// unsigned char Charger; +// unsigned char Battery; +// unsigned char overCurrent; +// //---docking ir +// unsigned char IRSensorRight; +// unsigned char IRSensorCenter; +// unsigned char IRSensorLeft; +// //---Inertial Sensor Data +// signed short GyroAngle; +// unsigned short GyroAngleRate; +// //---Cliff Sensor Data +// unsigned short CliffSensorRight; +// unsigned short CliffSensorCenter; +// unsigned short CliffSensorLeft; +// //---Current +// unsigned char wheelCurrentLeft; +// unsigned char wheelCurrentRight; +// //---Raw Data Of 3D Gyro +// unsigned char frameId; +// std::vector gyroData; +// //---General Purpose Input +// unsigned short digitalInput; +// unsigned short analogInputCh0; +// unsigned short analogInputCh1; +// unsigned short analogInputCh2; +// unsigned short analogInputCh3; +// //---structure with data that appears only on request +// TExtraRequestData extraInfo; +// }TKobukiData; typedef long(*src_callback_kobuki_data) (void *user_data, TKobukiData &Kobuki_data); @@ -163,6 +164,7 @@ public: + private: int HCom; pthread_t threadHandle; // handle to the thread @@ -171,10 +173,8 @@ private: src_callback_kobuki_data callbackFunction; void *userData; bool enabledCommands; - int parseKobukiMessage(TKobukiData &output, unsigned char *data ); int connect(char *portname); unsigned char *readKobukiMessage(); - int checkChecksum(unsigned char *data); //--start measurement in a new thread (reading runs in a new thread. it needs to be stopped if we want to send a request) static void * KobukiProcess(void *param) diff --git a/src/C++/Driver/src/KobukiParser.cpp b/src/C++/Driver/src/KobukiParser.cpp index 1b36454..a0cf0b0 100644 --- a/src/C++/Driver/src/KobukiParser.cpp +++ b/src/C++/Driver/src/KobukiParser.cpp @@ -2,7 +2,7 @@ int KobukiParser::parseKobukiMessage(TKobukiData &output, unsigned char *data) { //check if the checksum is correct - int rtrnvalue = CKobuki.checkChecksum(data); + int rtrnvalue = checkChecksum(data); if (rtrnvalue != 0) return -2; @@ -17,51 +17,51 @@ int KobukiParser::parseKobukiMessage(TKobukiData &output, unsigned char *data) { 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; + // 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 break; @@ -104,3 +104,14 @@ int KobukiParser::parseBasicData(TKobukiData &output, unsigned char *data, int & return 0; } + + +int KobukiParser::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 +} \ No newline at end of file diff --git a/src/C++/Driver/src/KobukiParser.h b/src/C++/Driver/src/KobukiParser.h index 37ddbf1..783ddf4 100644 --- a/src/C++/Driver/src/KobukiParser.h +++ b/src/C++/Driver/src/KobukiParser.h @@ -2,7 +2,6 @@ #define KOBUKIPARSER_H #include -#include "CKobuki.h" struct TRawGyroData { int x, y, z; @@ -32,19 +31,22 @@ struct TKobukiData { class KobukiParser { public: int parseKobukiMessage(TKobukiData &output, unsigned char *data); + TKobukiData 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); + // 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 \ No newline at end of file diff --git a/src/C++/Driver/src/main.cpp b/src/C++/Driver/src/main.cpp index e17fe97..04e90f4 100644 --- a/src/C++/Driver/src/main.cpp +++ b/src/C++/Driver/src/main.cpp @@ -4,26 +4,27 @@ #include #include "graph.h" + using namespace std; CKobuki robot; int movement(); +int checkCenterCliff(); int main() { unsigned char *null_ptr(0); robot.startCommunication("/dev/ttyUSB0", true, null_ptr); - usleep(1 * 1000 * 1000); - cout << "Enter commando"; - thread mv(movement); - usleep(30 * 1000 * 1000); - mv.join(); //only exit once thread one is done running + // thread mv(movement); + // mv.join(); //only exit once thread one is done running + + checkCenterCliff(); } int checkCenterCliff() { while (true) { - std::cout << "cliffsensordata:" << robot.data.CliffSensorCenter << std::endl; + std::cout << "cliffsensordata:" << parser.data.CliffSensorCenter << std::endl; } }