Zürcher Eliteeinheit / Mbed 2 deprecated ROME2_P4

Important changes to repositories hosted on mbed.com

Mbed hosted mercurial repositories are deprecated and are due to be permanently deleted in July 2026.

To keep a copy of this software download the repository Zip archive or clone locally using Mercurial.

It is also possible to export all your personal repositories from the account settings page.

Dependencies:   ROME2_P2 mbed

Fork of ROME2_P3 by Zürcher Eliteeinheit

SerialServer.cpp@6:67263dc2c2cf, 2018-04-26 (annotated)

Committer:
matajarb
Date:
Thu Apr 26 12:22:58 2018 +0000
Revision:
6:67263dc2c2cf
s?mme du schluch

Who changed what in which revision?

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