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

new kobuki parser testing

Browse Source
This commit is contained in:
Sam Hos
2024-10-07 11:21:22 +02:00
parent 83f7d447d3
commit e5c58bff81
4 changed files with 156 additions and 142 deletions
Download Patch File Download Diff File Expand all files Collapse all files

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

@@ -32,91 +32,92 @@
#include <sstream> #include <sstream>
#include "graph.h" #include "graph.h"
#include "KobukiParser.h"
using namespace std; using namespace std;
typedef struct // typedef struct
{ // {
unsigned short x; // unsigned short x;
unsigned short y; // unsigned short y;
unsigned short z; // unsigned short z;
}TRawGyroData; // }TRawGyroData;
typedef struct // typedef struct
{ // {
//Hardware Version // //Hardware Version
unsigned char HardwareVersionMajor; // unsigned char HardwareVersionMajor;
unsigned char HardwareVersionMinor; // unsigned char HardwareVersionMinor;
unsigned char HardwareVersionPatch; // unsigned char HardwareVersionPatch;
//Firmware Version // //Firmware Version
unsigned char FirmwareVersionMajor; // unsigned char FirmwareVersionMajor;
unsigned char FirmwareVersionMinor; // unsigned char FirmwareVersionMinor;
unsigned char FirmwareVersionPatch; // unsigned char FirmwareVersionPatch;
//Unique Device IDentifier(UDID) // //Unique Device IDentifier(UDID)
unsigned int UDID0; // unsigned int UDID0;
unsigned int UDID1; // unsigned int UDID1;
unsigned int UDID2; // unsigned int UDID2;
//Controller Info // //Controller Info
unsigned char PIDtype; // unsigned char PIDtype;
unsigned int PIDgainP; // unsigned int PIDgainP;
unsigned int PIDgainI; // unsigned int PIDgainI;
unsigned int PIDgainD; // unsigned int PIDgainD;
}TExtraRequestData; // }TExtraRequestData;
typedef struct // typedef struct
{ // {
//---basic package // //---basic package
unsigned short timestamp; // unsigned short timestamp;
//bumpers // //bumpers
bool BumperLeft; // bool BumperLeft;
bool BumperCenter; // bool BumperCenter;
bool BumperRight; // bool BumperRight;
//cliff // //cliff
bool CliffLeft; // bool CliffLeft;
bool CliffCenter; // bool CliffCenter;
bool CliffRight; // bool CliffRight;
// wheel drop // // wheel drop
bool WheelDropLeft; // bool WheelDropLeft;
bool WheelDropRight; // bool WheelDropRight;
//wheel rotation // //wheel rotation
unsigned short EncoderRight; // unsigned short EncoderRight;
unsigned short EncoderLeft; // unsigned short EncoderLeft;
unsigned char PWMright; // unsigned char PWMright;
unsigned char PWMleft; // unsigned char PWMleft;
//buttons // //buttons
unsigned char ButtonPress;// 0 no, 1 2 4 for button 0 1 2 (7 is all three) // unsigned char ButtonPress;// 0 no, 1 2 4 for button 0 1 2 (7 is all three)
//power // //power
unsigned char Charger; // unsigned char Charger;
unsigned char Battery; // unsigned char Battery;
unsigned char overCurrent; // unsigned char overCurrent;
//---docking ir // //---docking ir
unsigned char IRSensorRight; // unsigned char IRSensorRight;
unsigned char IRSensorCenter; // unsigned char IRSensorCenter;
unsigned char IRSensorLeft; // unsigned char IRSensorLeft;
//---Inertial Sensor Data // //---Inertial Sensor Data
signed short GyroAngle; // signed short GyroAngle;
unsigned short GyroAngleRate; // unsigned short GyroAngleRate;
//---Cliff Sensor Data // //---Cliff Sensor Data
unsigned short CliffSensorRight; // unsigned short CliffSensorRight;
unsigned short CliffSensorCenter; // unsigned short CliffSensorCenter;
unsigned short CliffSensorLeft; // unsigned short CliffSensorLeft;
//---Current // //---Current
unsigned char wheelCurrentLeft; // unsigned char wheelCurrentLeft;
unsigned char wheelCurrentRight; // unsigned char wheelCurrentRight;
//---Raw Data Of 3D Gyro // //---Raw Data Of 3D Gyro
unsigned char frameId; // unsigned char frameId;
std::vector<TRawGyroData> gyroData; // std::vector<TRawGyroData> gyroData;
//---General Purpose Input // //---General Purpose Input
unsigned short digitalInput; // unsigned short digitalInput;
unsigned short analogInputCh0; // unsigned short analogInputCh0;
unsigned short analogInputCh1; // unsigned short analogInputCh1;
unsigned short analogInputCh2; // unsigned short analogInputCh2;
unsigned short analogInputCh3; // unsigned short analogInputCh3;
//---structure with data that appears only on request // //---structure with data that appears only on request
TExtraRequestData extraInfo; // TExtraRequestData extraInfo;
}TKobukiData; // }TKobukiData;
typedef long(*src_callback_kobuki_data) (void *user_data, TKobukiData &Kobuki_data); typedef long(*src_callback_kobuki_data) (void *user_data, TKobukiData &Kobuki_data);
@@ -163,6 +164,7 @@ public:
private: private:
int HCom; int HCom;
pthread_t threadHandle; // handle to the thread pthread_t threadHandle; // handle to the thread
@@ -171,10 +173,8 @@ private:
src_callback_kobuki_data callbackFunction; src_callback_kobuki_data callbackFunction;
void *userData; void *userData;
bool enabledCommands; bool enabledCommands;
int parseKobukiMessage(TKobukiData &output, unsigned char *data );
int connect(char *portname); int connect(char *portname);
unsigned char *readKobukiMessage(); 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) //--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) static void * KobukiProcess(void *param)

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

@@ -2,7 +2,7 @@
int KobukiParser::parseKobukiMessage(TKobukiData &output, unsigned char *data) { int KobukiParser::parseKobukiMessage(TKobukiData &output, unsigned char *data) {
//check if the checksum is correct //check if the checksum is correct
int rtrnvalue = CKobuki.checkChecksum(data); int rtrnvalue = checkChecksum(data);
if (rtrnvalue != 0) if (rtrnvalue != 0)
return -2; return -2;
@@ -17,51 +17,51 @@ int KobukiParser::parseKobukiMessage(TKobukiData &output, unsigned char *data) {
parseBasicData(output, data, checkedValue); parseBasicData(output, data, checkedValue);
} }
break; break;
case 0x03: // case 0x03:
if (length == 0x03) { // if (length == 0x03) {
parseIRSensorData(output, data, checkedValue); // parseIRSensorData(output, data, checkedValue);
} // }
break; // break;
case 0x04: // case 0x04:
if (length == 0x07) { // if (length == 0x07) {
parseGyroData(output, data, checkedValue); // parseGyroData(output, data, checkedValue);
} // }
break; // break;
case 0x05: // case 0x05:
if (length == 0x06) { // if (length == 0x06) {
parseCliffSensorData(output, data, checkedValue); // parseCliffSensorData(output, data, checkedValue);
} // }
break; // break;
case 0x06: // case 0x06:
if (length == 0x02) { // if (length == 0x02) {
parseWheelCurrentData(output, data, checkedValue); // parseWheelCurrentData(output, data, checkedValue);
} // }
break; // break;
case 0x0A: // case 0x0A:
if (length == 0x04) { // if (length == 0x04) {
parseHardwareVersionData(output, data, checkedValue); // parseHardwareVersionData(output, data, checkedValue);
} // }
break; // break;
case 0x0B: // case 0x0B:
if (length == 0x04) { // if (length == 0x04) {
parseFirmwareVersionData(output, data, checkedValue); // parseFirmwareVersionData(output, data, checkedValue);
} // }
break; // break;
case 0x0D: // case 0x0D:
if (length == 0x0D) { // if (length == 0x0D) {
parseXYZData(output, data, checkedValue); // parseXYZData(output, data, checkedValue);
} // }
break; // break;
case 0x10: // case 0x10:
if (length == 0x10) { // if (length == 0x10) {
parseDigitalAnalogInputData(output, data, checkedValue); // parseDigitalAnalogInputData(output, data, checkedValue);
} // }
break; // break;
case 0x13: // case 0x13:
if (length == 0x0C) { // if (length == 0x0C) {
parseUDIDData(output, data, checkedValue); // parseUDIDData(output, data, checkedValue);
} // }
break; // break;
default: default:
checkedValue += length + 2; // Skip data block if nothing matches checkedValue += length + 2; // Skip data block if nothing matches
break; break;
@@ -104,3 +104,14 @@ int KobukiParser::parseBasicData(TKobukiData &output, unsigned char *data, int &
return 0; 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
}

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

@@ -2,7 +2,6 @@
#define KOBUKIPARSER_H #define KOBUKIPARSER_H
#include <vector> #include <vector>
#include "CKobuki.h"
struct TRawGyroData { struct TRawGyroData {
int x, y, z; int x, y, z;
@@ -32,19 +31,22 @@ struct TKobukiData {
class KobukiParser { class KobukiParser {
public: public:
int parseKobukiMessage(TKobukiData &output, unsigned char *data); int parseKobukiMessage(TKobukiData &output, unsigned char *data);
TKobukiData data;
private: private:
int checkChecksum(unsigned char *data); int checkChecksum(unsigned char *data);
int parseBasicData(TKobukiData &output, unsigned char *data, int &checkedValue); int parseBasicData(TKobukiData &output, unsigned char *data, int &checkedValue);
int parseIRSensorData(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 parseGyroData(TKobukiData &output, unsigned char *data, int &checkedValue);
int parseCliffSensorData(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 parseWheelCurrentData(TKobukiData &output, unsigned char *data, int &checkedValue);
int parseHardwareVersionData(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 parseFirmwareVersionData(TKobukiData &output, unsigned char *data, int &checkedValue);
int parseXYZData(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 parseDigitalAnalogInputData(TKobukiData &output, unsigned char *data, int &checkedValue);
int parseUDIDData(TKobukiData &output, unsigned char *data, int &checkedValue); // int parseUDIDData(TKobukiData &output, unsigned char *data, int &checkedValue);
}; };
#endif #endif

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

@@ -4,26 +4,27 @@
#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 main() int main()
{ {
unsigned char *null_ptr(0); unsigned char *null_ptr(0);
robot.startCommunication("/dev/ttyUSB0", true, null_ptr); robot.startCommunication("/dev/ttyUSB0", true, null_ptr);
usleep(1 * 1000 * 1000); // thread mv(movement);
cout << "Enter commando"; // mv.join(); //only exit once thread one is done running
thread mv(movement);
usleep(30 * 1000 * 1000); checkCenterCliff();
mv.join(); //only exit once thread one is done running
} }
int checkCenterCliff() int checkCenterCliff()
{ {
while (true) while (true)
{ {
std::cout << "cliffsensordata:" << robot.data.CliffSensorCenter << std::endl; std::cout << "cliffsensordata:" << parser.data.CliffSensorCenter << std::endl;
} }
} }