rome2_p6 imported
Dependencies: mbed
SerialServer.cpp@5:957580f33e52, 2018-05-18 (annotated)
- Committer:
- Appalco
- Date:
- Fri May 18 13:54:25 2018 +0000
- Revision:
- 5:957580f33e52
- Parent:
- 0:351a2fb21235
fixed tolerance and wayponts
Who changed what in which revision?
User | Revision | Line number | New 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 |