Allan Brignoli / Mbed 2 deprecated Rome2_P6

gugus

Dependencies:   mbed

SerialServer.cpp@0:1a0321f1ffbc, 2018-05-18 (annotated)

Committer:
Brignall
Date:
Fri May 18 12:18:21 2018 +0000
Revision:
0:1a0321f1ffbc
lala;

Who changed what in which revision?

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