Christian Burri / Mbed 2 deprecated autonomous Robot Android

This program is for an autonomous robot for the competition at the Hochschule Luzern. http://cruisingcrepe.wordpress.com/ We are one of the 32 teams. http://cruisingcrepe.wordpress.com/ 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: http://www.dis.uniroma1.it/~labrob/pub/papers/Ramsete01.pdf

Dependencies:   mbed

Fork of autonomous Robot Android by Christian Burri

MicroBridge/androidADB.cpp@27:a13ede88e75f, 2013-05-20 (annotated)

Committer:
chrigelburri
Date:
Mon May 20 11:51:33 2013 +0000
Revision:
27:a13ede88e75f
Parent:
21:48248c5b8992
Child:
32:767044a3e421
mit led methode

Who changed what in which revision?

UserRevisionLine numberNew contents of line
chrigelburri 18:306d362d692b 1 #include "androidADB.h"
chrigelburri 18:306d362d692b 2
chrigelburri 19:b2f76b0fe4c8 3
chrigelburri 18:306d362d692b 4 Connection * connection;
chrigelburri 18:306d362d692b 5
chrigelburri 18:306d362d692b 6 /** @brief Desired position in meters for x-coordinate, given by android */
chrigelburri 18:306d362d692b 7 float androidx;
chrigelburri 18:306d362d692b 8
chrigelburri 18:306d362d692b 9 /** @brief Desired position in meters for y-coordinate, given by android */
chrigelburri 18:306d362d692b 10 float androidy;
chrigelburri 18:306d362d692b 11
chrigelburri 18:306d362d692b 12 /** @brief Desired position in degrees for theta, given by android */
chrigelburri 27:a13ede88e75f 13 float androidt;
chrigelburri 18:306d362d692b 14
chrigelburri 18:306d362d692b 15 /** @brief Indicates if a ADB connection to a android phone is established */
chrigelburri 18:306d362d692b 16 boolean androidConnected;
chrigelburri 18:306d362d692b 17
chrigelburri 19:b2f76b0fe4c8 18 float getDesiredX()
chrigelburri 19:b2f76b0fe4c8 19 {
chrigelburri 20:01b233b0e606 20 return androidx/1000;
chrigelburri 19:b2f76b0fe4c8 21 }
chrigelburri 19:b2f76b0fe4c8 22
chrigelburri 19:b2f76b0fe4c8 23 float getDesiredY()
chrigelburri 19:b2f76b0fe4c8 24 {
chrigelburri 20:01b233b0e606 25 return androidy/1000;
chrigelburri 19:b2f76b0fe4c8 26 }
chrigelburri 19:b2f76b0fe4c8 27
chrigelburri 19:b2f76b0fe4c8 28 float getDesiredTheta()
chrigelburri 19:b2f76b0fe4c8 29 {
chrigelburri 20:01b233b0e606 30 return androidt * PI / 180;
chrigelburri 19:b2f76b0fe4c8 31 }
chrigelburri 27:a13ede88e75f 32
chrigelburri 27:a13ede88e75f 33 void setDesiredTheta(float t)
chrigelburri 27:a13ede88e75f 34 {
chrigelburri 27:a13ede88e75f 35 androidt = t;
chrigelburri 27:a13ede88e75f 36 }
chrigelburri 27:a13ede88e75f 37
chrigelburri 27:a13ede88e75f 38
chrigelburri 18:306d362d692b 39 void Tokenize(const string& str,
chrigelburri 18:306d362d692b 40 vector<string>& tokens,
chrigelburri 18:306d362d692b 41 const string& delimiters /*= " "*/)
chrigelburri 18:306d362d692b 42 {
chrigelburri 18:306d362d692b 43 // Skip delimiters at beginning.
chrigelburri 18:306d362d692b 44 string::size_type lastPos = str.find_first_not_of(delimiters, 0);
chrigelburri 18:306d362d692b 45 // Find first "non-delimiter".
chrigelburri 18:306d362d692b 46 string::size_type pos = str.find_first_of(delimiters, lastPos);
chrigelburri 18:306d362d692b 47
chrigelburri 18:306d362d692b 48 while (string::npos != pos || string::npos != lastPos) {
chrigelburri 18:306d362d692b 49 // Found a token, add it to the vector.
chrigelburri 18:306d362d692b 50 tokens.push_back(str.substr(lastPos, pos - lastPos));
chrigelburri 18:306d362d692b 51 // Skip delimiters. Note the "not_of"
chrigelburri 18:306d362d692b 52 lastPos = str.find_first_not_of(delimiters, pos);
chrigelburri 18:306d362d692b 53 // Find next "non-delimiter"
chrigelburri 18:306d362d692b 54 pos = str.find_first_of(delimiters, lastPos);
chrigelburri 18:306d362d692b 55 }
chrigelburri 18:306d362d692b 56 }
chrigelburri 18:306d362d692b 57
chrigelburri 19:b2f76b0fe4c8 58 extern void adbEventHandler(Connection * connection, adb_eventType event, uint16_t length, uint8_t * data)
chrigelburri 18:306d362d692b 59 {
chrigelburri 18:306d362d692b 60
chrigelburri 18:306d362d692b 61 if (event == ADB_CONNECTION_OPEN) {
chrigelburri 18:306d362d692b 62 androidConnected = true;
chrigelburri 18:306d362d692b 63 //pc.printf("Android Connected\n");
chrigelburri 18:306d362d692b 64 } else if (event == ADB_CONNECTION_CLOSE) {
chrigelburri 18:306d362d692b 65 androidConnected = false;
chrigelburri 19:b2f76b0fe4c8 66 printf("Android Disonnected\n");
chrigelburri 18:306d362d692b 67 }
chrigelburri 18:306d362d692b 68
chrigelburri 18:306d362d692b 69 if (event == ADB_CONNECTION_RECEIVE) {
chrigelburri 18:306d362d692b 70 parseMessage(length, data);
chrigelburri 18:306d362d692b 71
chrigelburri 18:306d362d692b 72 }
chrigelburri 18:306d362d692b 73 }
chrigelburri 18:306d362d692b 74
chrigelburri 18:306d362d692b 75 void parseMessage(uint16_t length, uint8_t * data)
chrigelburri 18:306d362d692b 76 {
chrigelburri 18:306d362d692b 77 //received = "[ADB RECV]"; printf("[ADB RECV]:%d %d\r\n",data[0],data[1]);
chrigelburri 18:306d362d692b 78
chrigelburri 18:306d362d692b 79 char str[32];
chrigelburri 18:306d362d692b 80
chrigelburri 18:306d362d692b 81 // convert buffer (unsigned char) to char
chrigelburri 18:306d362d692b 82 sprintf( str, "%s", data);
chrigelburri 18:306d362d692b 83
chrigelburri 18:306d362d692b 84 // new vector of strings
chrigelburri 18:306d362d692b 85 vector<string> tokens;
chrigelburri 18:306d362d692b 86
chrigelburri 18:306d362d692b 87 // tokenize the string with the semicolon separator
chrigelburri 18:306d362d692b 88 Tokenize(str, tokens, ";");
chrigelburri 18:306d362d692b 89 copy(tokens.begin(), tokens.end(), ostream_iterator<string>(cout, ", "));
chrigelburri 18:306d362d692b 90
chrigelburri 18:306d362d692b 91 if(tokens.size() > 2) {
chrigelburri 18:306d362d692b 92
chrigelburri 18:306d362d692b 93 //string to float
chrigelburri 18:306d362d692b 94 androidx = ::atof(tokens.at(0).c_str());
chrigelburri 18:306d362d692b 95 androidy = ::atof(tokens.at(1).c_str());
chrigelburri 18:306d362d692b 96 androidt = ::atof(tokens.at(2).c_str());
chrigelburri 21:48248c5b8992 97
chrigelburri 18:306d362d692b 98
chrigelburri 19:b2f76b0fe4c8 99 printf("Android x(%d): %f\n\r\n",length,androidx);
chrigelburri 19:b2f76b0fe4c8 100 printf("Android y(%d): %f\n\r\n",length,androidy);
chrigelburri 19:b2f76b0fe4c8 101 printf("Android t(%d): %f\n\r\n",length,androidt);
chrigelburri 18:306d362d692b 102 } else {
chrigelburri 18:306d362d692b 103 //pc.printf("Android sayys(%d): %s\n\r\n",length,str);
chrigelburri 18:306d362d692b 104 }
chrigelburri 18:306d362d692b 105
chrigelburri 18:306d362d692b 106 }
chrigelburri 18:306d362d692b 107
chrigelburri 18:306d362d692b 108 void connect()
chrigelburri 18:306d362d692b 109 {
chrigelburri 18:306d362d692b 110 ADB::poll();
chrigelburri 19:b2f76b0fe4c8 111 printf("connecting...");
chrigelburri 18:306d362d692b 112 char c = 'c';
chrigelburri 18:306d362d692b 113 connection->write(sizeof(c), (unsigned char*)&c);
chrigelburri 18:306d362d692b 114 //wait(0.5);
chrigelburri 18:306d362d692b 115 }
chrigelburri 18:306d362d692b 116
chrigelburri 19:b2f76b0fe4c8 117 void init()
chrigelburri 18:306d362d692b 118 {
chrigelburri 18:306d362d692b 119
chrigelburri 19:b2f76b0fe4c8 120 // Initialise the ADB subsystem.
chrigelburri 18:306d362d692b 121 ADB::init();
chrigelburri 18:306d362d692b 122
chrigelburri 18:306d362d692b 123 // Open an ADB stream on tcp port 4568. Auto-reconnect
chrigelburri 18:306d362d692b 124 connection = ADB::addConnection("tcp:4568", true, adbEventHandler);
chrigelburri 18:306d362d692b 125
chrigelburri 18:306d362d692b 126 // Connecting to android
chrigelburri 18:306d362d692b 127 while(!(androidConnected)) {
chrigelburri 18:306d362d692b 128 connect();
chrigelburri 18:306d362d692b 129 wait(0.5);
chrigelburri 18:306d362d692b 130 }
chrigelburri 18:306d362d692b 131
chrigelburri 19:b2f76b0fe4c8 132 }
chrigelburri 21:48248c5b8992 133 //////// Brachts dies noch?????????
chrigelburri 19:b2f76b0fe4c8 134 void write2Android(char str [32])
chrigelburri 19:b2f76b0fe4c8 135 {
chrigelburri 19:b2f76b0fe4c8 136 connection->write(sizeof(str),(unsigned char*)&str);
chrigelburri 19:b2f76b0fe4c8 137 }
chrigelburri 19:b2f76b0fe4c8 138
chrigelburri 20:01b233b0e606 139 void writeActualPosition(float x, float y, float t, int state_u, int state_l, int state_r, float volt_b)
chrigelburri 19:b2f76b0fe4c8 140 {
chrigelburri 20:01b233b0e606 141 // reconnect funktioniert trotzdem nicht!?
chrigelburri 19:b2f76b0fe4c8 142 while(!(androidConnected)) {
chrigelburri 19:b2f76b0fe4c8 143 connect();
chrigelburri 19:b2f76b0fe4c8 144 wait(0.5);
chrigelburri 19:b2f76b0fe4c8 145 }
chrigelburri 19:b2f76b0fe4c8 146
chrigelburri 20:01b233b0e606 147 char str[100];
chrigelburri 20:01b233b0e606 148
chrigelburri 20:01b233b0e606 149 //send to android
chrigelburri 20:01b233b0e606 150 sprintf( str, "%f;%f;%f;%i;%i;%i;%f;;", x,
chrigelburri 20:01b233b0e606 151 y,
chrigelburri 20:01b233b0e606 152 t * 180 / PI,
chrigelburri 20:01b233b0e606 153 state_u == 0 ? 0 : 1,
chrigelburri 20:01b233b0e606 154 state_l == 0 ? 0 : 1,
chrigelburri 20:01b233b0e606 155 state_r == 0 ? 0 : 1,
chrigelburri 20:01b233b0e606 156 volt_b);
chrigelburri 19:b2f76b0fe4c8 157
chrigelburri 19:b2f76b0fe4c8 158 connection->write(sizeof(str),(unsigned char*)&str);
chrigelburri 19:b2f76b0fe4c8 159
chrigelburri 19:b2f76b0fe4c8 160 }
chrigelburri 19:b2f76b0fe4c8 161
chrigelburri 19:b2f76b0fe4c8 162 /*
chrigelburri 19:b2f76b0fe4c8 163 int main()
chrigelburri 19:b2f76b0fe4c8 164 {
chrigelburri 19:b2f76b0fe4c8 165
chrigelburri 19:b2f76b0fe4c8 166 pc.baud(460800);
chrigelburri 19:b2f76b0fe4c8 167
chrigelburri 19:b2f76b0fe4c8 168 pc.printf("********************* MicroBridge 4568 ********************************\n\r");
chrigelburri 19:b2f76b0fe4c8 169
chrigelburri 19:b2f76b0fe4c8 170
chrigelburri 19:b2f76b0fe4c8 171
chrigelburri 18:306d362d692b 172 pc.printf("connection isOpen\n");
chrigelburri 18:306d362d692b 173
chrigelburri 18:306d362d692b 174 float flt = 0.0;
chrigelburri 18:306d362d692b 175 float flt2 = 0.2;
chrigelburri 18:306d362d692b 176 float flt3 = 1.2;
chrigelburri 18:306d362d692b 177
chrigelburri 18:306d362d692b 178 while(1) {
chrigelburri 18:306d362d692b 179
chrigelburri 18:306d362d692b 180 ADB::poll();
chrigelburri 18:306d362d692b 181
chrigelburri 18:306d362d692b 182 flt = flt - 0.1;
chrigelburri 18:306d362d692b 183 flt2 = flt2 + 0.2;
chrigelburri 18:306d362d692b 184 flt3 = flt3 - 0.05;
chrigelburri 18:306d362d692b 185
chrigelburri 18:306d362d692b 186 char str[32];
chrigelburri 18:306d362d692b 187 sprintf( str, "%f;%f;%f;", flt, flt2, flt3);
chrigelburri 18:306d362d692b 188
chrigelburri 18:306d362d692b 189 pc.printf("Sending: %s\n\r",str);
chrigelburri 18:306d362d692b 190 connection->write(sizeof(str),(unsigned char*)&str);
chrigelburri 18:306d362d692b 191 wait(1);
chrigelburri 18:306d362d692b 192
chrigelburri 18:306d362d692b 193 }
chrigelburri 19:b2f76b0fe4c8 194 }
chrigelburri 19:b2f76b0fe4c8 195 */