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@21:48248c5b8992, 2013-05-17 (annotated)

Committer:
chrigelburri
Date:
Fri May 17 06:39:44 2013 +0000
Revision:
21:48248c5b8992
Parent:
20:01b233b0e606
Child:
27:a13ede88e75f
vor dem einstellen des gr?nen kaktus :-)

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