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 "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<TRawGyroData> 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<TRawGyroData> 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)

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

@@ -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
}

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

@@ -2,7 +2,6 @@
#define KOBUKIPARSER_H
#include <vector>
#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

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

@@ -4,26 +4,27 @@
#include <thread>
#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;
}
}