TeamSurface / Libs

Nim leo niiiim

SerialServer.cpp@0:da791f233257, 2018-05-11 (annotated)

Committer:
Kiwicjam
Date:
Fri May 11 12:21:19 2018 +0000
Revision:
0:da791f233257
start of rome2 p5;

Who changed what in which revision?

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