From cede3689f46965b5b3626c44c4bba49d90b1b834 Mon Sep 17 00:00:00 2001 From: Sam Hos Date: Tue, 8 Oct 2024 12:39:31 +0200 Subject: [PATCH] cleanup and logger creation --- src/C++/Driver/CMakeLists.txt | 2 +- src/C++/Driver/src/CKobuki.cpp | 197 --------------------------------- src/C++/Driver/src/CKobuki.h | 85 -------------- src/C++/Driver/src/main.cpp | 20 +++- 4 files changed, 19 insertions(+), 285 deletions(-) diff --git a/src/C++/Driver/CMakeLists.txt b/src/C++/Driver/CMakeLists.txt index 7bb90d4..f10f0a6 100644 --- a/src/C++/Driver/CMakeLists.txt +++ b/src/C++/Driver/CMakeLists.txt @@ -6,7 +6,7 @@ set(SOURCE_FILES src/KobukiParser.h src/CKobuki.cpp src/CKobuki.h - src/test.cpp) + src/main.cpp) add_executable(kobuki_control ${SOURCE_FILES}) #target_link_libraries(kobuki_control ) \ No newline at end of file diff --git a/src/C++/Driver/src/CKobuki.cpp b/src/C++/Driver/src/CKobuki.cpp index 9b4687d..2f132e5 100755 --- a/src/C++/Driver/src/CKobuki.cpp +++ b/src/C++/Driver/src/CKobuki.cpp @@ -302,203 +302,6 @@ int CKobuki::measure() return 0; } -// int CKobuki::parseKobukiMessage(TKobukiData &output, unsigned char *data) -// { -// int rtrnvalue = checkChecksum(data); -// // ak je zly checksum,tak kaslat na to -// if (rtrnvalue != 0) -// return -2; - -// int checkedValue = 1; -// // kym neprejdeme celu dlzku -// while (checkedValue < data[0]) -// { -// // basic data subload -// if (data[checkedValue] == 0x01) -// { -// checkedValue++; -// if (data[checkedValue] != 0x0F) -// return -1; -// checkedValue++; -// output.timestamp = data[checkedValue + 1] * 256 + data[checkedValue]; -// checkedValue += 2; -// output.BumperCenter = data[checkedValue] && 0x02; -// 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 if (data[checkedValue] == 0x03) -// { -// checkedValue++; -// if (data[checkedValue] != 0x03) -// return -3; -// checkedValue++; -// output.IRSensorRight = data[checkedValue]; -// checkedValue++; -// output.IRSensorCenter = data[checkedValue]; -// checkedValue++; -// output.IRSensorLeft = data[checkedValue]; -// checkedValue++; -// } -// else if (data[checkedValue] == 0x04) -// { -// checkedValue++; -// if (data[checkedValue] != 0x07) -// return -4; -// checkedValue++; -// output.GyroAngle = data[checkedValue + 1] * 256 + data[checkedValue]; -// checkedValue += 2; -// output.GyroAngleRate = data[checkedValue + 1] * 256 + data[checkedValue]; -// checkedValue += 5; // 3 unsued -// } -// else if (data[checkedValue] == 0x05) -// { -// checkedValue++; -// if (data[checkedValue] != 0x06) -// return -5; -// checkedValue++; -// 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; -// } -// else if (data[checkedValue] == 0x06) -// { -// checkedValue++; -// if (data[checkedValue] != 0x02) -// return -6; -// checkedValue++; -// output.wheelCurrentLeft = data[checkedValue]; -// checkedValue++; -// output.wheelCurrentRight = data[checkedValue]; -// checkedValue++; -// } -// else if (data[checkedValue] == 0x0A) -// { -// checkedValue++; -// if (data[checkedValue] != 0x04) -// return -7; -// checkedValue++; -// output.extraInfo.HardwareVersionPatch = data[checkedValue]; -// checkedValue++; -// output.extraInfo.HardwareVersionMinor = data[checkedValue]; -// checkedValue++; -// output.extraInfo.HardwareVersionMajor = data[checkedValue]; -// checkedValue += 2; -// } -// else if (data[checkedValue] == 0x0B) -// { -// checkedValue++; -// if (data[checkedValue] != 0x04) -// return -8; -// checkedValue++; -// output.extraInfo.FirmwareVersionPatch = data[checkedValue]; -// checkedValue++; -// output.extraInfo.FirmwareVersionMinor = data[checkedValue]; -// checkedValue++; -// output.extraInfo.FirmwareVersionMajor = data[checkedValue]; -// checkedValue += 2; -// } -// else if (data[checkedValue] == 0x0D) -// { -// checkedValue++; -// if (data[checkedValue] % 2 != 0) -// return -9; -// checkedValue++; -// 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); -// } -// } -// else if (data[checkedValue] == 0x10) -// { -// checkedValue++; -// if (data[checkedValue] != 0x10) -// return -10; -// 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 -// } -// else if (data[checkedValue] == 0x13) -// { -// checkedValue++; -// if (data[checkedValue] != 0x0C) -// return -11; -// 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; -// } -// else -// { -// checkedValue++; -// checkedValue += data[checkedValue] + 1; -// } -// } -// return 0; -// } - long double CKobuki::gyroToRad(signed short GyroAngle) { diff --git a/src/C++/Driver/src/CKobuki.h b/src/C++/Driver/src/CKobuki.h index f7f9e4e..551c28e 100755 --- a/src/C++/Driver/src/CKobuki.h +++ b/src/C++/Driver/src/CKobuki.h @@ -35,91 +35,6 @@ using namespace std; - -// typedef struct -// { - -// 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; - -// //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 long(*src_callback_kobuki_data) (void *user_data, TKobukiData &Kobuki_data); class CKobuki diff --git a/src/C++/Driver/src/main.cpp b/src/C++/Driver/src/main.cpp index 62d4cec..24f2ce4 100644 --- a/src/C++/Driver/src/main.cpp +++ b/src/C++/Driver/src/main.cpp @@ -8,6 +8,7 @@ using namespace std; CKobuki robot; int movement(); int checkCenterCliff(); +void logToFile(); int main() { @@ -16,7 +17,7 @@ int main() // thread mv(movement); // mv.join(); //only exit once thread one is done running - checkCenterCliff(); + logToFile(); return 0; } @@ -24,7 +25,7 @@ int main() int checkCenterCliff() { while(true){ - std::cout << robot.parser.data.CliffSensorCenter << endl; + std::cout << robot.parser.data.CliffCenter << endl; } } @@ -63,3 +64,18 @@ int movement() } } } + + +void logToFile(){ + std::ofstream outputFile("log"); + + if (outputFile.is_open()) { // check if the file was opened successfully + outputFile << robot.parser.data.analogInputCh0; // write data to the file + outputFile.close(); // close the file when done + } + else { + std::cerr << "Error opening file\n"; + } + +} +