SerialServer.cpp

00001 /*
00002  * SerialServer.cpp
00003  * Copyright (c) 2018, ZHAW
00004  * All rights reserved.
00005  */
00006 
00007 #include <vector>
00008 #include "SerialServer.h"
00009 
00010 using namespace std;
00011 
00012 inline string float2string(float f) {
00013 
00014     char buffer[32];
00015     sprintf(buffer, "%.3f", f);
00016     
00017     return string(buffer);
00018 }
00019 
00020 inline string int2string(int i) {
00021     
00022     char buffer[32];
00023     sprintf(buffer, "%d", i);
00024     
00025     return string(buffer);
00026 }
00027 
00028 const char SerialServer::INT_TO_CHAR[] = {0x30, 0x31, 0x32, 0x33, 0x34, 0x35, 0x36, 0x37, 0x38, 0x39, 0x41, 0x42, 0x43, 0x44, 0x45, 0x46};
00029 
00030 /**
00031  * Creates a serial server object.
00032  */
00033 SerialServer::SerialServer(RawSerial& serial, LIDAR& lidar, Controller& controller) : serial(serial), lidar(lidar), controller(controller) {
00034     
00035     input.clear();
00036     output.clear();
00037     
00038     serial.attach(callback(this, &SerialServer::receive), Serial::RxIrq);
00039     //serial.attach(callback(this, &SerialServer::transmit), Serial::TxIrq);
00040     ticker.attach(callback(this, &SerialServer::transmit), 0.001);
00041 }
00042 
00043 /**
00044  * Deletes the serial server object.
00045  */
00046 SerialServer::~SerialServer() {}
00047 
00048 /**
00049  * Callback method of serial interface.
00050  */
00051 void SerialServer::receive() {
00052     
00053     // read received characters while input buffer is full
00054     
00055     while (serial.readable()) {
00056         int c = serial.getc();
00057         if (input.size() < BUFFER_SIZE) input += (char)c;
00058     }
00059     
00060     // check if input is complete (terminated with CR & LF)
00061     
00062     if (input.find("\r\n") != string::npos) {
00063 
00064         // parse request
00065         
00066         string request = input.substr(0, input.find("\r\n"));
00067         string name;
00068         vector<string> values;
00069         
00070         if (request.find(' ') != string::npos) {
00071 
00072             name = request.substr(0, request.find(' '));
00073             request = request.substr(request.find(' ')+1);
00074 
00075             while (request.find(' ') != string::npos) {
00076                 values.push_back(request.substr(0, request.find(' ')));
00077                 request = request.substr(request.find(' ')+1);
00078             }
00079             values.push_back(request);
00080 
00081         } else {
00082             
00083             name = request;
00084         }
00085         
00086         input.clear();
00087         
00088         // process request
00089         
00090         if (name.compare("getDistance") == 0) {
00091             short angle = atoi(values[0].c_str());
00092             short distance = lidar.getDistance(angle);
00093             output = "distance ";
00094             for (int i = 0; i < 4; i++) output += INT_TO_CHAR[(distance >> (4*(3-i))) & 0x0F];
00095             output += "\r\n";
00096         } else if (name.compare("getBeacon") == 0) {
00097             short angle = lidar.getAngleOfBeacon();
00098             short distance = lidar.getDistanceOfBeacon();
00099             output = "beacon ";
00100             for (int i = 0; i < 4; i++) output += INT_TO_CHAR[(angle >> (4*(3-i))) & 0x0F];
00101             output += " ";
00102             for (int i = 0; i < 4; i++) output += INT_TO_CHAR[(distance >> (4*(3-i))) & 0x0F];
00103             output += "\r\n";
00104         } else if (name.compare("getRobotPose") == 0) {
00105             float x = controller.getX();
00106             float y = controller.getY();
00107             float alpha = controller.getAlpha();
00108             output = "pose "+float2string(x)+" "+float2string(y)+" "+float2string(alpha)+"\r\n";
00109         } else if (name.compare("getOrientation") == 0) {
00110             float alpha = controller.getAlpha();
00111             output = "orientation "+float2string(alpha)+"\r\n";
00112         } else {
00113             output = "request unknown\r\n";
00114         }
00115         
00116         // transmit first byte of output buffer
00117         
00118         if (serial.writeable() && (output.size() > 0)) {
00119             serial.putc(output[0]);
00120             output.erase(0, 1);
00121         }
00122         
00123     } else if (input.size() >= BUFFER_SIZE) {
00124         
00125         input.clear();
00126     }
00127 }
00128 
00129 /**
00130  * Callback method of serial interface.
00131  */
00132 void SerialServer::transmit() {
00133     
00134     // transmit output
00135     
00136     while (serial.writeable() && (output.size() > 0)) {
00137         serial.putc(output[0]);
00138         output.erase(0, 1);
00139     }
00140 }
00141