ROME2 / Mbed 2 deprecated ROME2_P5

ROME_P5

Dependencies:   mbed

SerialServer.cpp@0:29be10cb0afc, 2018-04-27 (annotated)

Committer:
Inaueadr
Date:
Fri Apr 27 08:47:34 2018 +0000
Revision:
0:29be10cb0afc
Hallo

Who changed what in which revision?

UserRevisionLine numberNew contents of line
Inaueadr 0:29be10cb0afc 1 /*
Inaueadr 0:29be10cb0afc 2 * SerialServer.cpp
Inaueadr 0:29be10cb0afc 3 * Copyright (c) 2018, ZHAW
Inaueadr 0:29be10cb0afc 4 * All rights reserved.
Inaueadr 0:29be10cb0afc 5 */
Inaueadr 0:29be10cb0afc 6
Inaueadr 0:29be10cb0afc 7 #include <vector>
Inaueadr 0:29be10cb0afc 8 #include "SerialServer.h"
Inaueadr 0:29be10cb0afc 9
Inaueadr 0:29be10cb0afc 10 using namespace std;
Inaueadr 0:29be10cb0afc 11
Inaueadr 0:29be10cb0afc 12 inline string float2string(float f) {
Inaueadr 0:29be10cb0afc 13
Inaueadr 0:29be10cb0afc 14 char buffer[32];
Inaueadr 0:29be10cb0afc 15 sprintf(buffer, "%.3f", f);
Inaueadr 0:29be10cb0afc 16
Inaueadr 0:29be10cb0afc 17 return string(buffer);
Inaueadr 0:29be10cb0afc 18 }
Inaueadr 0:29be10cb0afc 19
Inaueadr 0:29be10cb0afc 20 inline string int2string(int i) {
Inaueadr 0:29be10cb0afc 21
Inaueadr 0:29be10cb0afc 22 char buffer[32];
Inaueadr 0:29be10cb0afc 23 sprintf(buffer, "%d", i);
Inaueadr 0:29be10cb0afc 24
Inaueadr 0:29be10cb0afc 25 return string(buffer);
Inaueadr 0:29be10cb0afc 26 }
Inaueadr 0:29be10cb0afc 27
Inaueadr 0:29be10cb0afc 28 const char SerialServer::INT_TO_CHAR[] = {0x30, 0x31, 0x32, 0x33, 0x34, 0x35, 0x36, 0x37, 0x38, 0x39, 0x41, 0x42, 0x43, 0x44, 0x45, 0x46};
Inaueadr 0:29be10cb0afc 29
Inaueadr 0:29be10cb0afc 30 /**
Inaueadr 0:29be10cb0afc 31 * Creates a serial server object.
Inaueadr 0:29be10cb0afc 32 */
Inaueadr 0:29be10cb0afc 33 SerialServer::SerialServer(RawSerial& serial, LIDAR& lidar, Controller& controller) : serial(serial), lidar(lidar), controller(controller) {
Inaueadr 0:29be10cb0afc 34
Inaueadr 0:29be10cb0afc 35 input.clear();
Inaueadr 0:29be10cb0afc 36 output.clear();
Inaueadr 0:29be10cb0afc 37
Inaueadr 0:29be10cb0afc 38 serial.attach(callback(this, &SerialServer::receive), Serial::RxIrq);
Inaueadr 0:29be10cb0afc 39 //serial.attach(callback(this, &SerialServer::transmit), Serial::TxIrq);
Inaueadr 0:29be10cb0afc 40 ticker.attach(callback(this, &SerialServer::transmit), 0.001);
Inaueadr 0:29be10cb0afc 41 }
Inaueadr 0:29be10cb0afc 42
Inaueadr 0:29be10cb0afc 43 /**
Inaueadr 0:29be10cb0afc 44 * Deletes the serial server object.
Inaueadr 0:29be10cb0afc 45 */
Inaueadr 0:29be10cb0afc 46 SerialServer::~SerialServer() {}
Inaueadr 0:29be10cb0afc 47
Inaueadr 0:29be10cb0afc 48 /**
Inaueadr 0:29be10cb0afc 49 * Callback method of serial interface.
Inaueadr 0:29be10cb0afc 50 */
Inaueadr 0:29be10cb0afc 51 void SerialServer::receive() {
Inaueadr 0:29be10cb0afc 52
Inaueadr 0:29be10cb0afc 53 // read received characters while input buffer is full
Inaueadr 0:29be10cb0afc 54
Inaueadr 0:29be10cb0afc 55 while (serial.readable()) {
Inaueadr 0:29be10cb0afc 56 int c = serial.getc();
Inaueadr 0:29be10cb0afc 57 if (input.size() < BUFFER_SIZE) input += (char)c;
Inaueadr 0:29be10cb0afc 58 }
Inaueadr 0:29be10cb0afc 59
Inaueadr 0:29be10cb0afc 60 // check if input is complete (terminated with CR & LF)
Inaueadr 0:29be10cb0afc 61
Inaueadr 0:29be10cb0afc 62 if (input.find("\r\n") != string::npos) {
Inaueadr 0:29be10cb0afc 63
Inaueadr 0:29be10cb0afc 64 // parse request
Inaueadr 0:29be10cb0afc 65
Inaueadr 0:29be10cb0afc 66 string request = input.substr(0, input.find("\r\n"));
Inaueadr 0:29be10cb0afc 67 string name;
Inaueadr 0:29be10cb0afc 68 vector<string> values;
Inaueadr 0:29be10cb0afc 69
Inaueadr 0:29be10cb0afc 70 if (request.find(' ') != string::npos) {
Inaueadr 0:29be10cb0afc 71
Inaueadr 0:29be10cb0afc 72 name = request.substr(0, request.find(' '));
Inaueadr 0:29be10cb0afc 73 request = request.substr(request.find(' ')+1);
Inaueadr 0:29be10cb0afc 74
Inaueadr 0:29be10cb0afc 75 while (request.find(' ') != string::npos) {
Inaueadr 0:29be10cb0afc 76 values.push_back(request.substr(0, request.find(' ')));
Inaueadr 0:29be10cb0afc 77 request = request.substr(request.find(' ')+1);
Inaueadr 0:29be10cb0afc 78 }
Inaueadr 0:29be10cb0afc 79 values.push_back(request);
Inaueadr 0:29be10cb0afc 80
Inaueadr 0:29be10cb0afc 81 } else {
Inaueadr 0:29be10cb0afc 82
Inaueadr 0:29be10cb0afc 83 name = request;
Inaueadr 0:29be10cb0afc 84 }
Inaueadr 0:29be10cb0afc 85
Inaueadr 0:29be10cb0afc 86 input.clear();
Inaueadr 0:29be10cb0afc 87
Inaueadr 0:29be10cb0afc 88 // process request
Inaueadr 0:29be10cb0afc 89
Inaueadr 0:29be10cb0afc 90 if (name.compare("getDistance") == 0) {
Inaueadr 0:29be10cb0afc 91 short angle = atoi(values[0].c_str());
Inaueadr 0:29be10cb0afc 92 short distance = lidar.getDistance(angle);
Inaueadr 0:29be10cb0afc 93 output = "distance ";
Inaueadr 0:29be10cb0afc 94 for (int i = 0; i < 4; i++) output += INT_TO_CHAR[(distance >> (4*(3-i))) & 0x0F];
Inaueadr 0:29be10cb0afc 95 output += "\r\n";
Inaueadr 0:29be10cb0afc 96 } else if (name.compare("getBeacon") == 0) {
Inaueadr 0:29be10cb0afc 97 short angle = lidar.getAngleOfBeacon();
Inaueadr 0:29be10cb0afc 98 short distance = lidar.getDistanceOfBeacon();
Inaueadr 0:29be10cb0afc 99 output = "beacon ";
Inaueadr 0:29be10cb0afc 100 for (int i = 0; i < 4; i++) output += INT_TO_CHAR[(angle >> (4*(3-i))) & 0x0F];
Inaueadr 0:29be10cb0afc 101 output += " ";
Inaueadr 0:29be10cb0afc 102 for (int i = 0; i < 4; i++) output += INT_TO_CHAR[(distance >> (4*(3-i))) & 0x0F];
Inaueadr 0:29be10cb0afc 103 output += "\r\n";
Inaueadr 0:29be10cb0afc 104 } else if (name.compare("getRobotPose") == 0) {
Inaueadr 0:29be10cb0afc 105 float x = controller.getX();
Inaueadr 0:29be10cb0afc 106 float y = controller.getY();
Inaueadr 0:29be10cb0afc 107 float alpha = controller.getAlpha();
Inaueadr 0:29be10cb0afc 108 output = "pose "+float2string(x)+" "+float2string(y)+" "+float2string(alpha)+"\r\n";
Inaueadr 0:29be10cb0afc 109 } else if (name.compare("getOrientation") == 0) {
Inaueadr 0:29be10cb0afc 110 float alpha = controller.getAlpha();
Inaueadr 0:29be10cb0afc 111 output = "orientation "+float2string(alpha)+"\r\n";
Inaueadr 0:29be10cb0afc 112 } else {
Inaueadr 0:29be10cb0afc 113 output = "request unknown\r\n";
Inaueadr 0:29be10cb0afc 114 }
Inaueadr 0:29be10cb0afc 115
Inaueadr 0:29be10cb0afc 116 // transmit first byte of output buffer
Inaueadr 0:29be10cb0afc 117
Inaueadr 0:29be10cb0afc 118 if (serial.writeable() && (output.size() > 0)) {
Inaueadr 0:29be10cb0afc 119 serial.putc(output[0]);
Inaueadr 0:29be10cb0afc 120 output.erase(0, 1);
Inaueadr 0:29be10cb0afc 121 }
Inaueadr 0:29be10cb0afc 122
Inaueadr 0:29be10cb0afc 123 } else if (input.size() >= BUFFER_SIZE) {
Inaueadr 0:29be10cb0afc 124
Inaueadr 0:29be10cb0afc 125 input.clear();
Inaueadr 0:29be10cb0afc 126 }
Inaueadr 0:29be10cb0afc 127 }
Inaueadr 0:29be10cb0afc 128
Inaueadr 0:29be10cb0afc 129 /**
Inaueadr 0:29be10cb0afc 130 * Callback method of serial interface.
Inaueadr 0:29be10cb0afc 131 */
Inaueadr 0:29be10cb0afc 132 void SerialServer::transmit() {
Inaueadr 0:29be10cb0afc 133
Inaueadr 0:29be10cb0afc 134 // transmit output
Inaueadr 0:29be10cb0afc 135
Inaueadr 0:29be10cb0afc 136 while (serial.writeable() && (output.size() > 0)) {
Inaueadr 0:29be10cb0afc 137 serial.putc(output[0]);
Inaueadr 0:29be10cb0afc 138 output.erase(0, 1);
Inaueadr 0:29be10cb0afc 139 }
Inaueadr 0:29be10cb0afc 140 }
Inaueadr 0:29be10cb0afc 141