Zürcher Eliteeinheit / Mbed 2 deprecated ROME2_P6

rome2_p6 imported

Dependencies:   mbed

SerialServer.cpp@0:351a2fb21235, 2018-05-18 (annotated)

Committer:
Appalco
Date:
Fri May 18 12:05:32 2018 +0000
Revision:
0:351a2fb21235
p6 imported;

Who changed what in which revision?

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