Diff: SerialServer.cpp

Revision:

6:67263dc2c2cf

diff -r 59079b76ac7f -r 67263dc2c2cf SerialServer.cpp
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/SerialServer.cpp	Thu Apr 26 12:22:58 2018 +0000
@@ -0,0 +1,128 @@
+/*
+ * SerialServer.cpp
+ * Copyright (c) 2018, ZHAW
+ * All rights reserved.
+ */
+
+#include <vector>
+#include "SerialServer.h"
+
+using namespace std;
+
+inline string float2string(float f) {
+
+    char buffer[32];
+    sprintf(buffer, "%.3f", f);
+    
+    return string(buffer);
+}
+
+inline string int2string(int i) {
+    
+    char buffer[32];
+    sprintf(buffer, "%d", i);
+    
+    return string(buffer);
+}
+
+const float SerialServer::PERIOD = 0.001f; // period of transmit task, given in [s]
+
+/**
+ * Creates a serial server object.
+ */
+SerialServer::SerialServer(Serial& serial, IMU& imu, Controller& controller) : serial(serial), imu(imu), controller(controller) {
+    
+    input.clear();
+    output.clear();
+    
+    serial.attach(callback(this, &SerialServer::receive), Serial::RxIrq);
+    //serial.attach(callback(this, &SerialServer::transmit), Serial::TxIrq);
+    ticker.attach(callback(this, &SerialServer::transmit), PERIOD);
+}
+
+/**
+ * Deletes the serial server object.
+ */
+SerialServer::~SerialServer() {}
+
+/**
+ * Callback method of serial interface.
+ */
+void SerialServer::receive() {
+    
+    // read received characters while input buffer is full
+    
+    while (serial.readable()) {
+        int c = serial.getc();
+        if (input.size() < BUFFER_SIZE) input += (char)c;
+    }
+    
+    // check if input is complete (terminated with CR & LF)
+    
+    if (input.find("\r\n") != string::npos) {
+
+        // parse request
+        
+        string request = input.substr(0, input.find("\r\n"));
+        string name;
+        vector<string> values;
+        
+        if (request.find(' ') != string::npos) {
+
+            name = request.substr(0, request.find(' '));
+            request = request.substr(request.find(' ')+1);
+
+            while (request.find(' ') != string::npos) {
+                values.push_back(request.substr(0, request.find(' ')));
+                request = request.substr(request.find(' ')+1);
+            }
+            values.push_back(request);
+
+        } else {
+            
+            name = request;
+        }
+        
+        input.clear();
+        
+        // process request
+        
+        if (name.compare("getRobotPose") == 0) {
+            float x = controller.getX();
+            float y = controller.getY();
+            float alpha = controller.getAlpha();
+            output = "pose "+float2string(x)+" "+float2string(y)+" "+float2string(alpha)+"\r\n";
+        } else if (name.compare("getOrientation") == 0) {
+            float heading = imu.readHeading();
+            float alpha = controller.getAlpha();
+            output = "orientation "+float2string(heading)+" "+float2string(alpha)+"\r\n";
+        } else {
+            output = "request unknown\r\n";
+        }
+        
+        // transmit first byte of output buffer
+        
+        if (serial.writeable() && (output.size() > 0)) {
+            serial.putc(output[0]);
+            output.erase(0, 1);
+        }
+        
+    } else if (input.size() >= BUFFER_SIZE) {
+        
+        input.clear();
+    }
+}
+
+/**
+ * Callback method of serial interface.
+ */
+void SerialServer::transmit() {
+    
+    // transmit output
+    
+    while (serial.writeable() && (output.size() > 0)) {
+        serial.putc(output[0]);
+        output.erase(0, 1);
+    }
+}
+