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

cleanup and logger creation

Browse Source
This commit is contained in:
Sam Hos
2024-10-08 12:39:31 +02:00
parent 49dec386d7
commit cede3689f4
4 changed files with 19 additions and 285 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/CKobuki.cpp
src/CKobuki.h
src/test.cpp)
src/main.cpp)
add_executable(kobuki_control ${SOURCE_FILES})
#target_link_libraries(kobuki_control )

197
src/C++/Driver/src/CKobuki.cpp
View File

@@ -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)
{

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

@@ -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<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);
class CKobuki

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

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