Smikkelbakje/J2S1-Kobuki
1
0
Fork 0
mirror of https://gitlab.fdmci.hva.nl/technische-informatica-sm3/ti-projectten/rooziinuubii79.git synced 2025-08-03 20:04:58 +00:00
Code Issues Actions Packages Projects Releases Wiki Activity

Merge branch 'main' of ssh://gitlab.fdmci.hva.nl/technische-informatica-sm3/ti-projectten/rooziinuubii79

Browse Source
This commit is contained in:
Mees Roelofsz
2024-10-08 11:55:30 +02:00
parent 2feb19ac61 ead5e1b36a
commit 49dec386d7
4 changed files with 183 additions and 154 deletions
Download Patch File Download Diff File Expand all files Collapse all files

2
src/C++/Driver/CMakeLists.txt
View File

@@ -6,7 +6,7 @@ set(SOURCE_FILES
src/KobukiParser.h src/KobukiParser.h
src/CKobuki.cpp src/CKobuki.cpp
src/CKobuki.h src/CKobuki.h
src/main.cpp) src/test.cpp)
add_executable(kobuki_control ${SOURCE_FILES}) add_executable(kobuki_control ${SOURCE_FILES})
#target_link_libraries(kobuki_control ) #target_link_libraries(kobuki_control )

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

@@ -17,183 +17,80 @@ int KobukiParser::parseKobukiMessage(TKobukiData &output, unsigned char *data) {
switch (dataType) { switch (dataType) {
case 0x01: case 0x01:
if (dataLength == 0x0F) { if (dataLength == 0x0F) {
output.timestamp = data[checkedValue + 1] * 256 + data[checkedValue]; parseBasicData(output, data, checkedValue);
checkedValue += 2;
output.BumperCenter = data[checkedValue] & 0x02;
std::cout << "BumperCenter: " << output.BumperCenter << std::endl;
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 { } else {
std::cerr << "Invalid data length for type 0x01" << std::endl;
checkedValue += dataLength; checkedValue += dataLength;
} }
break; break;
case 0x03: case 0x03:
if (dataLength == 0x03) { if (dataLength == 0x03) {
output.IRSensorRight = data[checkedValue]; parseIRSensorData(output, data, checkedValue);
checkedValue++;
output.IRSensorCenter = data[checkedValue];
checkedValue++;
output.IRSensorLeft = data[checkedValue];
checkedValue++;
} else { } else {
std::cerr << "Invalid data length for type 0x03" << std::endl;
checkedValue += dataLength; checkedValue += dataLength;
} }
break; break;
case 0x04: case 0x04:
if (dataLength == 0x07) { if (dataLength == 0x07) {
output.GyroAngle = data[checkedValue + 1] * 256 + data[checkedValue]; parseGyroData(output, data, checkedValue);
checkedValue += 2;
output.GyroAngleRate = data[checkedValue + 1] * 256 + data[checkedValue];
checkedValue += 5; // 3 unused
} else { } else {
std::cerr << "Invalid data length for type 0x04" << std::endl;
checkedValue += dataLength; checkedValue += dataLength;
} }
break; break;
case 0x05: case 0x05:
if (dataLength == 0x06) { if (dataLength == 0x06) {
output.CliffSensorRight = data[checkedValue + 1] * 256 + data[checkedValue]; parseCliffSensorData(output, 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 { } else {
std::cerr << "Invalid data length for type 0x05" << std::endl;
checkedValue += dataLength; checkedValue += dataLength;
} }
break; break;
case 0x06: case 0x06:
if (dataLength == 0x02) { if (dataLength == 0x02) {
output.wheelCurrentLeft = data[checkedValue]; parseWheelCurrentData(output, data, checkedValue);
checkedValue++;
output.wheelCurrentRight = data[checkedValue];
checkedValue++;
} else { } else {
std::cerr << "Invalid data length for type 0x06" << std::endl;
checkedValue += dataLength; checkedValue += dataLength;
} }
break; break;
case 0x0A: case 0x0A:
if (dataLength == 0x04) { if (dataLength == 0x04) {
output.extraInfo.HardwareVersionPatch = data[checkedValue]; parseHardwareVersionData(output, data, checkedValue);
checkedValue++;
output.extraInfo.HardwareVersionMinor = data[checkedValue];
checkedValue++;
output.extraInfo.HardwareVersionMajor = data[checkedValue];
checkedValue += 2;
} else { } else {
std::cerr << "Invalid data length for type 0x0A" << std::endl;
checkedValue += dataLength; checkedValue += dataLength;
} }
break; break;
case 0x0B: case 0x0B:
if (dataLength == 0x04) { if (dataLength == 0x04) {
output.extraInfo.FirmwareVersionPatch = data[checkedValue]; parseFirmwareVersionData(output, data, checkedValue);
checkedValue++;
output.extraInfo.FirmwareVersionMinor = data[checkedValue];
checkedValue++;
output.extraInfo.FirmwareVersionMajor = data[checkedValue];
checkedValue += 2;
} else { } else {
std::cerr << "Invalid data length for type 0x0B" << std::endl;
checkedValue += dataLength; checkedValue += dataLength;
} }
break; break;
case 0x0D: case 0x0D:
if (dataLength % 2 == 0) { if (dataLength % 2 == 0) {
output.frameId = data[checkedValue]; parseXYZData(output, 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 { } else {
std::cerr << "Invalid data length for type 0x0D" << std::endl;
checkedValue += dataLength; checkedValue += dataLength;
} }
break; break;
case 0x10: case 0x10:
if (dataLength == 0x10) { if (dataLength == 0x10) {
output.digitalInput = data[checkedValue + 1] * 256 + data[checkedValue]; parseDigitalAnalogInputData(output, 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 { } else {
std::cerr << "Invalid data length for type 0x10" << std::endl;
checkedValue += dataLength; checkedValue += dataLength;
} }
break; break;
case 0x13: case 0x13:
if (dataLength == 0x0C) { if (dataLength == 0x0C) {
output.extraInfo.UDID0 = data[checkedValue + 3] * 256 * 256 * 256 + parseUDIDData(output, data, checkedValue);
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 { } else {
std::cerr << "Invalid data length for type 0x13" << std::endl;
checkedValue += dataLength; checkedValue += dataLength;
} }
break; break;
@@ -206,6 +103,135 @@ int KobukiParser::parseKobukiMessage(TKobukiData &output, unsigned char *data) {
} }
return 0; return 0;
} }
void KobukiParser::parseBasicData(TKobukiData &output, unsigned char *data, int &checkedValue){
output.timestamp = data[checkedValue + 1] * 256 + data[checkedValue];
checkedValue += 2;
output.BumperCenter = (data[checkedValue] & 0x02) >> 1;
output.BumperLeft = (data[checkedValue] & 0x04) >> 2;
output.BumperRight = data[checkedValue] & 0x01;
checkedValue++;
output.WheelDropLeft = (data[checkedValue] & 0x02) >> 1;
output.WheelDropRight = data[checkedValue] & 0x01;
checkedValue++;
output.CliffCenter = (data[checkedValue] & 0x02) >> 1;
output.CliffLeft = (data[checkedValue] & 0x04) >> 2;
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++;
}
void KobukiParser::parseIRSensorData(TKobukiData &output, unsigned char *data, int &checkedValue){
output.IRSensorRight = data[checkedValue];
checkedValue++;
output.IRSensorCenter = data[checkedValue];
checkedValue++;
output.IRSensorLeft = data[checkedValue];
checkedValue++;
}
void KobukiParser::parseGyroData(TKobukiData &output, unsigned char *data, int &checkedValue){
output.GyroAngle = data[checkedValue + 1] * 256 + data[checkedValue];
checkedValue += 2;
output.GyroAngleRate = data[checkedValue + 1] * 256 + data[checkedValue];
checkedValue += 5; // 3 unused
}
void KobukiParser::parseCliffSensorData(TKobukiData &output, unsigned char *data, int &checkedValue){
output.CliffSensorRight = data[checkedValue];
checkedValue++;
output.CliffSensorCenter = data[checkedValue];
checkedValue++;
output.CliffSensorLeft = data[checkedValue];
checkedValue++;
}
void KobukiParser::parseWheelCurrentData(TKobukiData &output, unsigned char *data, int &checkedValue){
output.wheelCurrentLeft = data[checkedValue];
checkedValue++;
output.wheelCurrentRight = data[checkedValue];
checkedValue++;
}
void KobukiParser::parseHardwareVersionData(TKobukiData &output, unsigned char *data, int &checkedValue){
output.extraInfo.HardwareVersionPatch = data[checkedValue];
checkedValue++;
output.extraInfo.HardwareVersionMinor = data[checkedValue];
checkedValue++;
output.extraInfo.HardwareVersionMajor = data[checkedValue];
checkedValue += 2;
}
void KobukiParser::parseFirmwareVersionData(TKobukiData &output, unsigned char *data, int &checkedValue){
output.extraInfo.FirmwareVersionPatch = data[checkedValue];
checkedValue++;
output.extraInfo.FirmwareVersionMinor = data[checkedValue];
checkedValue++;
output.extraInfo.FirmwareVersionMajor = data[checkedValue];
checkedValue += 2;
}
void KobukiParser::parseXYZData(TKobukiData &output, unsigned char *data, int &checkedValue){
output.gyroData.reserve(data[checkedValue]);
output.gyroData.clear();
for (int hk = 0; hk < data[checkedValue]; 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);
}
}
void KobukiParser::parseDigitalAnalogInputData(TKobukiData &output, unsigned char *data, int &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
}
void KobukiParser::parseUDIDData(TKobukiData &output, unsigned char *data, int &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;
}
int KobukiParser::checkChecksum(unsigned char *data) { int KobukiParser::checkChecksum(unsigned char *data) {
unsigned char chckSum = 0; unsigned char chckSum = 0;
for (int i = 0; i < data[0] + 2; i++) { for (int i = 0; i < data[0] + 2; i++) {

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

@@ -38,17 +38,17 @@ public:
private: private:
int checkChecksum(unsigned char *data); int checkChecksum(unsigned char *data);
int parseBasicData(TKobukiData &output, unsigned char *data, int &checkedValue); void parseBasicData(TKobukiData &output, unsigned char *data, int &checkedValue);
int parseIRSensorData(TKobukiData &output, unsigned char *data, int &checkedValue); void parseIRSensorData(TKobukiData &output, unsigned char *data, int &checkedValue);
int parseGyroData(TKobukiData &output, unsigned char *data, int &checkedValue); void parseGyroData(TKobukiData &output, unsigned char *data, int &checkedValue);
int parseCliffSensorData(TKobukiData &output, unsigned char *data, int &checkedValue); void parseCliffSensorData(TKobukiData &output, unsigned char *data, int &checkedValue);
int parseWheelCurrentData(TKobukiData &output, unsigned char *data, int &checkedValue); void parseWheelCurrentData(TKobukiData &output, unsigned char *data, int &checkedValue);
int parseHardwareVersionData(TKobukiData &output, unsigned char *data, int &checkedValue); void parseHardwareVersionData(TKobukiData &output, unsigned char *data, int &checkedValue);
int parseFirmwareVersionData(TKobukiData &output, unsigned char *data, int &checkedValue); void parseFirmwareVersionData(TKobukiData &output, unsigned char *data, int &checkedValue);
int parseXYZData(TKobukiData &output, unsigned char *data, int &checkedValue); void parseXYZData(TKobukiData &output, unsigned char *data, int &checkedValue);
int parseDigitalAnalogInputData(TKobukiData &output, unsigned char *data, int &checkedValue); void parseDigitalAnalogInputData(TKobukiData &output, unsigned char *data, int &checkedValue);
int parseUDIDData(TKobukiData &output, unsigned char *data, int &checkedValue); void parseUDIDData(TKobukiData &output, unsigned char *data, int &checkedValue);
int parseExtraData(TKobukiData &output, unsigned char *data, int &checkedValue); void parseExtraData(TKobukiData &output, unsigned char *data, int &checkedValue);
}; };

61
src/C++/Driver/src/test.cpp
View File

@@ -8,53 +8,56 @@ using namespace std;
CKobuki robot; CKobuki robot;
int command(); int command();
const int forward = 1;
const int ROTATE = 2;
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); usleep(1 * 1000 * 1000);
cout << "Enter commando: ";
thread mv(command); thread mv(command);
usleep(30 * 1000 * 1000); usleep(30 * 1000 * 1000);
mv.join(); //only exit once thread one is done running 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:" << robot.parser.data.CliffSensorCenter << std::endl;
} // }
} // }
int checkWheelDrop(){ // int checkWheelDrop(){
while (true) // while (true)
{ // {
std::cout << "wheeldropdata:" << robot.data.WheelDropLeft << std::endl; // std::cout << "wheeldropdata:" << robot.parser.data.WheelDropLeft << std::endl;
} // }
} // }
int command(){ int command(){
int input; cout << "choose between forward and rotate" << endl;
cout << "What must the robot do?";
cin >> input; cin >> input;
switch(input){ switch(input){
case 1: case forward:{
robot.goStraight(1); int distance;
robot.doRotation(90); std::cout >> "Enter distance to move forward: ";
robot.forward(1024, -1); std::cin >> distance;
robot.goStraight(distance);
}
case ROTATE:{
int angle;
std::cout >> "Enter angle to rotate: ";
std::cin >> angle;
robot.doRotation(angle);
}
robot.goStraight(-1); robot.goStraight(-1);
break; break;
case 2:
robot.doRotation(90);
break;
case 3:
robot.doRotation(-90);
break;
case 4:
robot.forward(1024, -1);
break;
default: default:
cout << "Invalid input" << endl; cout << "Invalid input" << endl;
break; break;