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

whole struct to json

Browse Source
This commit is contained in:
Sam Hos
2024-11-05 13:47:26 +01:00
parent 222b14b9e5
commit 47f29f0f0a
1 changed files with 94 additions and 30 deletions
Download Patch File Download Diff File Expand all files Collapse all files

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

@@ -5,68 +5,75 @@
#include "MQTT/MqttClient.h" #include "MQTT/MqttClient.h"
#include "KobukiDriver/CKobuki.h" #include "KobukiDriver/CKobuki.h"
using namespace std; using namespace std;
CKobuki robot; CKobuki robot;
std::string readMQTT(); std::string readMQTT();
void parseMQTT(std::string message); void parseMQTT(std::string message);
MqttClient client("mqtt://145.92.224.21:1883", "KobukiRPI", "ishak", "kobuki"); //create a client object MqttClient client("mqtt://145.92.224.21:1883", "KobukiRPI", "ishak", "kobuki"); // create a client object
std::string message = "stop"; std::string message = "stop";
std::string serializeKobukiData(const TKobukiData &data);
void setup(){ void setup()
unsigned char *null_ptr(0); {
robot.startCommunication("/dev/ttyUSB0", true, null_ptr); unsigned char *null_ptr(0);
robot.startCommunication("/dev/ttyUSB0", true, null_ptr);
client.connect(); client.connect();
client.subscribe("home/commands"); client.subscribe("home/commands");
} }
int main(){ int main()
setup(); {
setup();
// std::thread safety([&]() { robot.robotSafety(&message); }); // std::thread safety([&]() { robot.robotSafety(&message); });
// while(true){ // while(true){
// parseMQTT(readMQTT()); // parseMQTT(readMQTT());
// } // }
// safety.join(); // safety.join();
// return 0; // return 0;
client.publishMessage("kobuki/data", "aaaa"); client.publishMessage("kobuki/data", serializeKobukiData(robot.parser.data));
} }
std::string readMQTT() std::string readMQTT()
{ {
message = client.getLastMessage(); message = client.getLastMessage();
if (!message.empty()) { if (!message.empty())
std::cout << "MQTT Message: " << message << std::endl; {
} std::cout << "MQTT Message: " << message << std::endl;
}
// Add a small delay to avoid busy-waiting // Add a small delay to avoid busy-waiting
std::this_thread::sleep_for(std::chrono::milliseconds(100)); std::this_thread::sleep_for(std::chrono::milliseconds(100));
return message; return message;
} }
void parseMQTT(std::string message){ void parseMQTT(std::string message)
if(message == "up"){ {
if (message == "up")
{
robot.forward(1024); robot.forward(1024);
} }
else if(message == "left"){ else if (message == "left")
{
robot.setRotationSpeed(4); robot.setRotationSpeed(4);
} }
else if(message == "right"){ else if (message == "right")
{
robot.setRotationSpeed(-4); robot.setRotationSpeed(-4);
} }
else if(message == "down"){ else if (message == "down")
{
robot.forward(-800); robot.forward(-800);
} }
else if(message == "stop"){ else if (message == "stop")
{
robot.sendNullMessage(); robot.sendNullMessage();
robot.sendNullMessage(); robot.sendNullMessage();
} }
else if (message == "estop"){ else if (message == "estop")
{
robot.forward(-400); robot.forward(-400);
} }
else{ else
{
std::cout << "Invalid command" << std::endl; std::cout << "Invalid command" << std::endl;
} }
} }
@@ -137,3 +144,60 @@ void logToFile()
std::this_thread::sleep_for(std::chrono::seconds(2)); // Sleep for 2 seconds std::this_thread::sleep_for(std::chrono::seconds(2)); // Sleep for 2 seconds
} }
} }
std::string serializeKobukiData(const TKobukiData &data) {
std::string json = "{\"timestamp\":" + std::to_string(data.timestamp) +
",\"BumperCenter\":" + std::to_string(data.BumperCenter) +
",\"BumperLeft\":" + std::to_string(data.BumperLeft) +
",\"BumperRight\":" + std::to_string(data.BumperRight) +
",\"WheelDropLeft\":" + std::to_string(data.WheelDropLeft) +
",\"WheelDropRight\":" + std::to_string(data.WheelDropRight) +
",\"CliffCenter\":" + std::to_string(data.CliffCenter) +
",\"CliffLeft\":" + std::to_string(data.CliffLeft) +
",\"CliffRight\":" + std::to_string(data.CliffRight) +
",\"EncoderLeft\":" + std::to_string(data.EncoderLeft) +
",\"EncoderRight\":" + std::to_string(data.EncoderRight) +
",\"PWMleft\":" + std::to_string(data.PWMleft) +
",\"PWMright\":" + std::to_string(data.PWMright) +
",\"ButtonPress1\":" + std::to_string(data.ButtonPress1) +
",\"ButtonPress2\":" + std::to_string(data.ButtonPress2) +
",\"ButtonPress3\":" + std::to_string(data.ButtonPress3) +
",\"Charger\":" + std::to_string(data.Charger) +
",\"Battery\":" + std::to_string(data.Battery) +
",\"overCurrent\":" + std::to_string(data.overCurrent) +
",\"IRSensorRight\":" + std::to_string(data.IRSensorRight) +
",\"IRSensorCenter\":" + std::to_string(data.IRSensorCenter) +
",\"IRSensorLeft\":" + std::to_string(data.IRSensorLeft) +
",\"GyroAngle\":" + std::to_string(data.GyroAngle) +
",\"GyroAngleRate\":" + std::to_string(data.GyroAngleRate) +
",\"CliffSensorRight\":" + std::to_string(data.CliffSensorRight) +
",\"CliffSensorCenter\":" + std::to_string(data.CliffSensorCenter) +
",\"CliffSensorLeft\":" + std::to_string(data.CliffSensorLeft) +
",\"wheelCurrentLeft\":" + std::to_string(data.wheelCurrentLeft) +
",\"wheelCurrentRight\":" + std::to_string(data.wheelCurrentRight) +
",\"digitalInput\":" + std::to_string(data.digitalInput) +
",\"analogInputCh0\":" + std::to_string(data.analogInputCh0) +
",\"analogInputCh1\":" + std::to_string(data.analogInputCh1) +
",\"analogInputCh2\":" + std::to_string(data.analogInputCh2) +
",\"analogInputCh3\":" + std::to_string(data.analogInputCh3) +
",\"frameId\":" + std::to_string(data.frameId) +
",\"extraInfo\":{\"HardwareVersionPatch\":" + std::to_string(data.extraInfo.HardwareVersionPatch) +
",\"HardwareVersionMinor\":" + std::to_string(data.extraInfo.HardwareVersionMinor) +
",\"HardwareVersionMajor\":" + std::to_string(data.extraInfo.HardwareVersionMajor) +
",\"FirmwareVersionPatch\":" + std::to_string(data.extraInfo.FirmwareVersionPatch) +
",\"FirmwareVersionMinor\":" + std::to_string(data.extraInfo.FirmwareVersionMinor) +
",\"FirmwareVersionMajor\":" + std::to_string(data.extraInfo.FirmwareVersionMajor) +
",\"UDID0\":" + std::to_string(data.extraInfo.UDID0) +
",\"UDID1\":" + std::to_string(data.extraInfo.UDID1) +
",\"UDID2\":" + std::to_string(data.extraInfo.UDID2) + "},\"gyroData\":[";
if (!data.gyroData.empty()) {
const auto& latestGyro = data.gyroData.back();
json += "{\"x\":" + std::to_string(latestGyro.x) +
",\"y\":" + std::to_string(latestGyro.y) +
",\"z\":" + std::to_string(latestGyro.z) + "}";
}
json += "]}";
return json;
}