mirror of
https://gitlab.fdmci.hva.nl/technische-informatica-sm3/ti-projectten/rooziinuubii79.git
synced 2025-08-03 20:04:58 +00:00
Merge branch 'main' of ssh://gitlab.fdmci.hva.nl/technische-informatica-sm3/ti-projectten/rooziinuubii79
This commit is contained in:
Mees Roelofsz
@@ -6,7 +6,7 @@ set(SOURCE_FILES
|
||||
src/KobukiParser.h
|
||||
src/CKobuki.cpp
|
||||
src/CKobuki.h
|
||||
src/main.cpp)
|
||||
src/test.cpp)
|
||||
|
||||
add_executable(kobuki_control ${SOURCE_FILES})
|
||||
#target_link_libraries(kobuki_control )
|
||||
@@ -17,183 +17,80 @@ int KobukiParser::parseKobukiMessage(TKobukiData &output, unsigned char *data) {
|
||||
switch (dataType) {
|
||||
case 0x01:
|
||||
if (dataLength == 0x0F) {
|
||||
output.timestamp = data[checkedValue + 1] * 256 + 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++;
|
||||
parseBasicData(output, data, checkedValue);
|
||||
} else {
|
||||
std::cerr << "Invalid data length for type 0x01" << std::endl;
|
||||
checkedValue += dataLength;
|
||||
}
|
||||
break;
|
||||
|
||||
case 0x03:
|
||||
if (dataLength == 0x03) {
|
||||
output.IRSensorRight = data[checkedValue];
|
||||
checkedValue++;
|
||||
output.IRSensorCenter = data[checkedValue];
|
||||
checkedValue++;
|
||||
output.IRSensorLeft = data[checkedValue];
|
||||
checkedValue++;
|
||||
parseIRSensorData(output, data, checkedValue);
|
||||
} else {
|
||||
std::cerr << "Invalid data length for type 0x03" << std::endl;
|
||||
checkedValue += dataLength;
|
||||
}
|
||||
break;
|
||||
|
||||
case 0x04:
|
||||
if (dataLength == 0x07) {
|
||||
output.GyroAngle = data[checkedValue + 1] * 256 + data[checkedValue];
|
||||
checkedValue += 2;
|
||||
output.GyroAngleRate = data[checkedValue + 1] * 256 + data[checkedValue];
|
||||
checkedValue += 5; // 3 unused
|
||||
parseGyroData(output, data, checkedValue);
|
||||
} else {
|
||||
std::cerr << "Invalid data length for type 0x04" << std::endl;
|
||||
checkedValue += dataLength;
|
||||
}
|
||||
break;
|
||||
|
||||
case 0x05:
|
||||
if (dataLength == 0x06) {
|
||||
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;
|
||||
parseCliffSensorData(output, data, checkedValue);
|
||||
} else {
|
||||
std::cerr << "Invalid data length for type 0x05" << std::endl;
|
||||
checkedValue += dataLength;
|
||||
}
|
||||
break;
|
||||
|
||||
case 0x06:
|
||||
if (dataLength == 0x02) {
|
||||
output.wheelCurrentLeft = data[checkedValue];
|
||||
checkedValue++;
|
||||
output.wheelCurrentRight = data[checkedValue];
|
||||
checkedValue++;
|
||||
parseWheelCurrentData(output, data, checkedValue);
|
||||
} else {
|
||||
std::cerr << "Invalid data length for type 0x06" << std::endl;
|
||||
checkedValue += dataLength;
|
||||
}
|
||||
break;
|
||||
|
||||
case 0x0A:
|
||||
if (dataLength == 0x04) {
|
||||
output.extraInfo.HardwareVersionPatch = data[checkedValue];
|
||||
checkedValue++;
|
||||
output.extraInfo.HardwareVersionMinor = data[checkedValue];
|
||||
checkedValue++;
|
||||
output.extraInfo.HardwareVersionMajor = data[checkedValue];
|
||||
checkedValue += 2;
|
||||
parseHardwareVersionData(output, data, checkedValue);
|
||||
} else {
|
||||
std::cerr << "Invalid data length for type 0x0A" << std::endl;
|
||||
checkedValue += dataLength;
|
||||
}
|
||||
break;
|
||||
|
||||
case 0x0B:
|
||||
if (dataLength == 0x04) {
|
||||
output.extraInfo.FirmwareVersionPatch = data[checkedValue];
|
||||
checkedValue++;
|
||||
output.extraInfo.FirmwareVersionMinor = data[checkedValue];
|
||||
checkedValue++;
|
||||
output.extraInfo.FirmwareVersionMajor = data[checkedValue];
|
||||
checkedValue += 2;
|
||||
parseFirmwareVersionData(output, data, checkedValue);
|
||||
} else {
|
||||
std::cerr << "Invalid data length for type 0x0B" << std::endl;
|
||||
checkedValue += dataLength;
|
||||
}
|
||||
break;
|
||||
|
||||
case 0x0D:
|
||||
if (dataLength % 2 == 0) {
|
||||
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);
|
||||
}
|
||||
parseXYZData(output, data, checkedValue);
|
||||
} else {
|
||||
std::cerr << "Invalid data length for type 0x0D" << std::endl;
|
||||
checkedValue += dataLength;
|
||||
}
|
||||
break;
|
||||
|
||||
case 0x10:
|
||||
if (dataLength == 0x10) {
|
||||
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
|
||||
parseDigitalAnalogInputData(output, data, checkedValue);
|
||||
} else {
|
||||
std::cerr << "Invalid data length for type 0x10" << std::endl;
|
||||
checkedValue += dataLength;
|
||||
}
|
||||
break;
|
||||
|
||||
case 0x13:
|
||||
if (dataLength == 0x0C) {
|
||||
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;
|
||||
parseUDIDData(output, data, checkedValue);
|
||||
} else {
|
||||
std::cerr << "Invalid data length for type 0x13" << std::endl;
|
||||
checkedValue += dataLength;
|
||||
}
|
||||
break;
|
||||
@@ -206,6 +103,135 @@ int KobukiParser::parseKobukiMessage(TKobukiData &output, unsigned char *data) {
|
||||
}
|
||||
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) {
|
||||
unsigned char chckSum = 0;
|
||||
for (int i = 0; i < data[0] + 2; i++) {
|
||||
|
||||
@@ -38,17 +38,17 @@ public:
|
||||
|
||||
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 parseExtraData(TKobukiData &output, unsigned char *data, int &checkedValue);
|
||||
void parseBasicData(TKobukiData &output, unsigned char *data, int &checkedValue);
|
||||
void parseIRSensorData(TKobukiData &output, unsigned char *data, int &checkedValue);
|
||||
void parseGyroData(TKobukiData &output, unsigned char *data, int &checkedValue);
|
||||
void parseCliffSensorData(TKobukiData &output, unsigned char *data, int &checkedValue);
|
||||
void parseWheelCurrentData(TKobukiData &output, unsigned char *data, int &checkedValue);
|
||||
void parseHardwareVersionData(TKobukiData &output, unsigned char *data, int &checkedValue);
|
||||
void parseFirmwareVersionData(TKobukiData &output, unsigned char *data, int &checkedValue);
|
||||
void parseXYZData(TKobukiData &output, unsigned char *data, int &checkedValue);
|
||||
void parseDigitalAnalogInputData(TKobukiData &output, unsigned char *data, int &checkedValue);
|
||||
void parseUDIDData(TKobukiData &output, unsigned char *data, int &checkedValue);
|
||||
void parseExtraData(TKobukiData &output, unsigned char *data, int &checkedValue);
|
||||
|
||||
};
|
||||
|
||||
|
||||
@@ -8,53 +8,56 @@ using namespace std;
|
||||
CKobuki robot;
|
||||
int command();
|
||||
|
||||
const int forward = 1;
|
||||
const int ROTATE = 2;
|
||||
|
||||
int main()
|
||||
{
|
||||
unsigned char *null_ptr(0);
|
||||
robot.startCommunication("/dev/ttyUSB0", true, null_ptr);
|
||||
usleep(1 * 1000 * 1000);
|
||||
cout << "Enter commando: ";
|
||||
thread mv(command);
|
||||
usleep(30 * 1000 * 1000);
|
||||
mv.join(); //only exit once thread one is done running
|
||||
}
|
||||
|
||||
int checkCenterCliff()
|
||||
{
|
||||
while (true)
|
||||
{
|
||||
std::cout << "cliffsensordata:" << robot.data.CliffSensorCenter << std::endl;
|
||||
}
|
||||
}
|
||||
// int checkCenterCliff()
|
||||
// {
|
||||
// while (true)
|
||||
// {
|
||||
// std::cout << "cliffsensordata:" << robot.parser.data.CliffSensorCenter << std::endl;
|
||||
// }
|
||||
// }
|
||||
|
||||
int checkWheelDrop(){
|
||||
while (true)
|
||||
{
|
||||
std::cout << "wheeldropdata:" << robot.data.WheelDropLeft << std::endl;
|
||||
}
|
||||
}
|
||||
// int checkWheelDrop(){
|
||||
// while (true)
|
||||
// {
|
||||
// std::cout << "wheeldropdata:" << robot.parser.data.WheelDropLeft << std::endl;
|
||||
// }
|
||||
// }
|
||||
|
||||
int command(){
|
||||
int input;
|
||||
|
||||
cout << "choose between forward and rotate" << endl;
|
||||
cout << "What must the robot do?";
|
||||
cin >> input;
|
||||
|
||||
switch(input){
|
||||
case 1:
|
||||
robot.goStraight(1);
|
||||
robot.doRotation(90);
|
||||
robot.forward(1024, -1);
|
||||
case forward:{
|
||||
int distance;
|
||||
std::cout >> "Enter distance to move forward: ";
|
||||
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);
|
||||
break;
|
||||
case 2:
|
||||
robot.doRotation(90);
|
||||
break;
|
||||
case 3:
|
||||
robot.doRotation(-90);
|
||||
break;
|
||||
case 4:
|
||||
robot.forward(1024, -1);
|
||||
break;
|
||||
|
||||
default:
|
||||
cout << "Invalid input" << endl;
|
||||
break;
|
||||
|
||||
Reference in New Issue
Block a user