Christian Burri / Mbed 2 deprecated autonomousRobotAndroid

This program is for an autonomous robot for the competition at the Hochschule Luzern. We are one of the 32 teams. <a href="http://cruisingcrepe.wordpress.com/">http://cruisingcrepe.wordpress.com/</a> The postition control is based on this Documentation: Control of Wheeled Mobile Robots: An Experimental Overview from Alessandro De Luca, Giuseppe Oriolo, Marilena Vendittelli. For more information see here: <a href="http://www.dis.uniroma1.it/~labrob/pub/papers/Ramsete01.pdf">http://www.dis.uniroma1.it/~labrob/pub/papers/Ramsete01.pdf</a>

Dependencies:   mbed

Fork of autonomousRobotAndroid by Christian Burri

MicroBridge/androidADB.cpp@39:a4fd6206da89, 2013-06-10 (annotated)

Committer:
chrigelburri
Date:
Mon Jun 10 14:40:37 2013 +0000
Revision:
39:a4fd6206da89
Parent:
33:ac39982fd3b2 
V1.0

Who changed what in which revision?

UserRevisionLine numberNew contents of line
chrigelburri 18:306d362d692b 1 #include "androidADB.h"
chrigelburri 18:306d362d692b 2
chrigelburri 18:306d362d692b 3 Connection * connection;
chrigelburri 18:306d362d692b 4
chrigelburri 18:306d362d692b 5 /** @brief Desired position in meters for x-coordinate, given by android */
chrigelburri 18:306d362d692b 6 float androidx;
chrigelburri 18:306d362d692b 7
chrigelburri 18:306d362d692b 8 /** @brief Desired position in meters for y-coordinate, given by android */
chrigelburri 18:306d362d692b 9 float androidy;
chrigelburri 18:306d362d692b 10
chrigelburri 18:306d362d692b 11 /** @brief Desired position in degrees for theta, given by android */
chrigelburri 28:b3e195e80439 12 float androidt;
chrigelburri 18:306d362d692b 13
chrigelburri 18:306d362d692b 14 /** @brief Indicates if a ADB connection to a android phone is established */
chrigelburri 18:306d362d692b 15 boolean androidConnected;
chrigelburri 18:306d362d692b 16
chrigelburri 19:b2f76b0fe4c8 17 float getDesiredX()
chrigelburri 19:b2f76b0fe4c8 18 {
chrigelburri 20:01b233b0e606 19 return androidx/1000;
chrigelburri 19:b2f76b0fe4c8 20 }
chrigelburri 19:b2f76b0fe4c8 21
chrigelburri 19:b2f76b0fe4c8 22 float getDesiredY()
chrigelburri 19:b2f76b0fe4c8 23 {
chrigelburri 20:01b233b0e606 24 return androidy/1000;
chrigelburri 19:b2f76b0fe4c8 25 }
chrigelburri 19:b2f76b0fe4c8 26
chrigelburri 19:b2f76b0fe4c8 27 float getDesiredTheta()
chrigelburri 19:b2f76b0fe4c8 28 {
chrigelburri 20:01b233b0e606 29 return androidt * PI / 180;
chrigelburri 19:b2f76b0fe4c8 30 }
chrigelburri 28:b3e195e80439 31
chrigelburri 28:b3e195e80439 32 void setDesiredTheta(float t)
chrigelburri 28:b3e195e80439 33 {
chrigelburri 28:b3e195e80439 34 androidt = t;
chrigelburri 28:b3e195e80439 35 }
chrigelburri 28:b3e195e80439 36
chrigelburri 18:306d362d692b 37 void Tokenize(const string& str,
chrigelburri 18:306d362d692b 38 vector<string>& tokens,
chrigelburri 18:306d362d692b 39 const string& delimiters /*= " "*/)
chrigelburri 18:306d362d692b 40 {
chrigelburri 18:306d362d692b 41 // Skip delimiters at beginning.
chrigelburri 18:306d362d692b 42 string::size_type lastPos = str.find_first_not_of(delimiters, 0);
chrigelburri 18:306d362d692b 43 // Find first "non-delimiter".
chrigelburri 18:306d362d692b 44 string::size_type pos = str.find_first_of(delimiters, lastPos);
chrigelburri 18:306d362d692b 45
chrigelburri 18:306d362d692b 46 while (string::npos != pos || string::npos != lastPos) {
chrigelburri 18:306d362d692b 47 // Found a token, add it to the vector.
chrigelburri 18:306d362d692b 48 tokens.push_back(str.substr(lastPos, pos - lastPos));
chrigelburri 18:306d362d692b 49 // Skip delimiters. Note the "not_of"
chrigelburri 18:306d362d692b 50 lastPos = str.find_first_not_of(delimiters, pos);
chrigelburri 18:306d362d692b 51 // Find next "non-delimiter"
chrigelburri 18:306d362d692b 52 pos = str.find_first_of(delimiters, lastPos);
chrigelburri 18:306d362d692b 53 }
chrigelburri 18:306d362d692b 54 }
chrigelburri 18:306d362d692b 55
chrigelburri 19:b2f76b0fe4c8 56 extern void adbEventHandler(Connection * connection, adb_eventType event, uint16_t length, uint8_t * data)
chrigelburri 18:306d362d692b 57 {
chrigelburri 18:306d362d692b 58
chrigelburri 18:306d362d692b 59 if (event == ADB_CONNECTION_OPEN) {
chrigelburri 18:306d362d692b 60 androidConnected = true;
chrigelburri 18:306d362d692b 61 } else if (event == ADB_CONNECTION_CLOSE) {
chrigelburri 18:306d362d692b 62 androidConnected = false;
chrigelburri 18:306d362d692b 63 }
chrigelburri 18:306d362d692b 64
chrigelburri 18:306d362d692b 65 if (event == ADB_CONNECTION_RECEIVE) {
chrigelburri 18:306d362d692b 66 parseMessage(length, data);
chrigelburri 18:306d362d692b 67
chrigelburri 18:306d362d692b 68 }
chrigelburri 18:306d362d692b 69 }
chrigelburri 18:306d362d692b 70
chrigelburri 18:306d362d692b 71 void parseMessage(uint16_t length, uint8_t * data)
chrigelburri 18:306d362d692b 72 {
chrigelburri 18:306d362d692b 73 char str[32];
chrigelburri 18:306d362d692b 74
chrigelburri 18:306d362d692b 75 // convert buffer (unsigned char) to char
chrigelburri 18:306d362d692b 76 sprintf( str, "%s", data);
chrigelburri 18:306d362d692b 77
chrigelburri 18:306d362d692b 78 // new vector of strings
chrigelburri 18:306d362d692b 79 vector<string> tokens;
chrigelburri 18:306d362d692b 80
chrigelburri 18:306d362d692b 81 // tokenize the string with the semicolon separator
chrigelburri 18:306d362d692b 82 Tokenize(str, tokens, ";");
chrigelburri 18:306d362d692b 83 copy(tokens.begin(), tokens.end(), ostream_iterator<string>(cout, ", "));
chrigelburri 18:306d362d692b 84
chrigelburri 18:306d362d692b 85 if(tokens.size() > 2) {
chrigelburri 18:306d362d692b 86
chrigelburri 18:306d362d692b 87 //string to float
chrigelburri 18:306d362d692b 88 androidx = ::atof(tokens.at(0).c_str());
chrigelburri 18:306d362d692b 89 androidy = ::atof(tokens.at(1).c_str());
chrigelburri 18:306d362d692b 90 androidt = ::atof(tokens.at(2).c_str());
chrigelburri 18:306d362d692b 91
chrigelburri 32:767044a3e421 92 } else {}
chrigelburri 18:306d362d692b 93
chrigelburri 18:306d362d692b 94 }
chrigelburri 18:306d362d692b 95
chrigelburri 18:306d362d692b 96 void connect()
chrigelburri 18:306d362d692b 97 {
chrigelburri 18:306d362d692b 98 ADB::poll();
chrigelburri 18:306d362d692b 99 char c = 'c';
chrigelburri 18:306d362d692b 100 connection->write(sizeof(c), (unsigned char*)&c);
chrigelburri 18:306d362d692b 101 }
chrigelburri 18:306d362d692b 102
chrigelburri 19:b2f76b0fe4c8 103 void init()
chrigelburri 18:306d362d692b 104 {
chrigelburri 18:306d362d692b 105
chrigelburri 32:767044a3e421 106 // Initialise the ADB subsystem.
chrigelburri 18:306d362d692b 107 ADB::init();
chrigelburri 18:306d362d692b 108
chrigelburri 18:306d362d692b 109 // Open an ADB stream on tcp port 4568. Auto-reconnect
chrigelburri 18:306d362d692b 110 connection = ADB::addConnection("tcp:4568", true, adbEventHandler);
chrigelburri 18:306d362d692b 111
chrigelburri 18:306d362d692b 112 // Connecting to android
chrigelburri 18:306d362d692b 113 while(!(androidConnected)) {
chrigelburri 18:306d362d692b 114 connect();
chrigelburri 18:306d362d692b 115 wait(0.5);
chrigelburri 18:306d362d692b 116 }
chrigelburri 18:306d362d692b 117
chrigelburri 19:b2f76b0fe4c8 118 }
chrigelburri 32:767044a3e421 119
chrigelburri 20:01b233b0e606 120 void writeActualPosition(float x, float y, float t, int state_u, int state_l, int state_r, float volt_b)
chrigelburri 19:b2f76b0fe4c8 121 {
chrigelburri 20:01b233b0e606 122 // reconnect funktioniert trotzdem nicht!?
chrigelburri 19:b2f76b0fe4c8 123 while(!(androidConnected)) {
chrigelburri 19:b2f76b0fe4c8 124 connect();
chrigelburri 19:b2f76b0fe4c8 125 wait(0.5);
chrigelburri 19:b2f76b0fe4c8 126 }
chrigelburri 19:b2f76b0fe4c8 127
chrigelburri 20:01b233b0e606 128 char str[100];
chrigelburri 32:767044a3e421 129
chrigelburri 20:01b233b0e606 130 //send to android
chrigelburri 20:01b233b0e606 131 sprintf( str, "%f;%f;%f;%i;%i;%i;%f;;", x,
chrigelburri 20:01b233b0e606 132 y,
chrigelburri 20:01b233b0e606 133 t * 180 / PI,
chrigelburri 20:01b233b0e606 134 state_u == 0 ? 0 : 1,
chrigelburri 20:01b233b0e606 135 state_l == 0 ? 0 : 1,
chrigelburri 20:01b233b0e606 136 state_r == 0 ? 0 : 1,
chrigelburri 20:01b233b0e606 137 volt_b);
chrigelburri 19:b2f76b0fe4c8 138
chrigelburri 19:b2f76b0fe4c8 139 connection->write(sizeof(str),(unsigned char*)&str);
chrigelburri 19:b2f76b0fe4c8 140
chrigelburri 32:767044a3e421 141 }