diff --git a/src/C++/Driver/src/main.cpp b/src/C++/Driver/src/main.cpp index caaa8a6..cbfa3dc 100644 --- a/src/C++/Driver/src/main.cpp +++ b/src/C++/Driver/src/main.cpp @@ -5,68 +5,75 @@ #include "MQTT/MqttClient.h" #include "KobukiDriver/CKobuki.h" - using namespace std; CKobuki robot; std::string readMQTT(); 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"; - -void setup(){ - unsigned char *null_ptr(0); - robot.startCommunication("/dev/ttyUSB0", true, null_ptr); +std::string serializeKobukiData(const TKobukiData &data); +void setup() +{ + unsigned char *null_ptr(0); + robot.startCommunication("/dev/ttyUSB0", true, null_ptr); client.connect(); client.subscribe("home/commands"); - - } -int main(){ - setup(); +int main() +{ + setup(); // std::thread safety([&]() { robot.robotSafety(&message); }); - // while(true){ + // while(true){ // parseMQTT(readMQTT()); - // } + // } // safety.join(); - // return 0; - client.publishMessage("kobuki/data", "aaaa"); + // return 0; + client.publishMessage("kobuki/data", serializeKobukiData(robot.parser.data)); } std::string readMQTT() { - message = client.getLastMessage(); - if (!message.empty()) { - std::cout << "MQTT Message: " << message << std::endl; - } + message = client.getLastMessage(); + if (!message.empty()) + { + std::cout << "MQTT Message: " << message << std::endl; + } - // Add a small delay to avoid busy-waiting - std::this_thread::sleep_for(std::chrono::milliseconds(100)); - return message; + // Add a small delay to avoid busy-waiting + std::this_thread::sleep_for(std::chrono::milliseconds(100)); + return message; } -void parseMQTT(std::string message){ - if(message == "up"){ +void parseMQTT(std::string message) +{ + if (message == "up") + { robot.forward(1024); } - else if(message == "left"){ + else if (message == "left") + { robot.setRotationSpeed(4); - } - else if(message == "right"){ + else if (message == "right") + { robot.setRotationSpeed(-4); } - else if(message == "down"){ + else if (message == "down") + { robot.forward(-800); } - else if(message == "stop"){ + else if (message == "stop") + { robot.sendNullMessage(); robot.sendNullMessage(); } - else if (message == "estop"){ + else if (message == "estop") + { robot.forward(-400); } - else{ + else + { 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::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; +} \ No newline at end of file