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@19:b2f76b0fe4c8, 2013-05-03 (annotated)

Committer:
chrigelburri
Date:
Fri May 03 13:34:34 2013 +0000
Revision:
19:b2f76b0fe4c8
Parent:
18:306d362d692b
Child:
20:01b233b0e606
asdf

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 18:306d362d692b 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 19:b2f76b0fe4c8 20 return androidx;
chrigelburri 19:b2f76b0fe4c8 21 }
chrigelburri 19:b2f76b0fe4c8 22
chrigelburri 19:b2f76b0fe4c8 23 float getDesiredY()
chrigelburri 19:b2f76b0fe4c8 24 {
chrigelburri 19:b2f76b0fe4c8 25 return androidy;
chrigelburri 19:b2f76b0fe4c8 26 }
chrigelburri 19:b2f76b0fe4c8 27
chrigelburri 19:b2f76b0fe4c8 28 float getDesiredTheta()
chrigelburri 19:b2f76b0fe4c8 29 {
chrigelburri 19:b2f76b0fe4c8 30 return androidt;
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 18:306d362d692b 90
chrigelburri 19:b2f76b0fe4c8 91 printf("Android x(%d): %f\n\r\n",length,androidx);
chrigelburri 19:b2f76b0fe4c8 92 printf("Android y(%d): %f\n\r\n",length,androidy);
chrigelburri 19:b2f76b0fe4c8 93 printf("Android t(%d): %f\n\r\n",length,androidt);
chrigelburri 18:306d362d692b 94 } else {
chrigelburri 18:306d362d692b 95 //pc.printf("Android sayys(%d): %s\n\r\n",length,str);
chrigelburri 18:306d362d692b 96 }
chrigelburri 18:306d362d692b 97
chrigelburri 18:306d362d692b 98 }
chrigelburri 18:306d362d692b 99
chrigelburri 18:306d362d692b 100 void connect()
chrigelburri 18:306d362d692b 101 {
chrigelburri 18:306d362d692b 102 ADB::poll();
chrigelburri 19:b2f76b0fe4c8 103 printf("connecting...");
chrigelburri 18:306d362d692b 104 char c = 'c';
chrigelburri 18:306d362d692b 105 connection->write(sizeof(c), (unsigned char*)&c);
chrigelburri 18:306d362d692b 106 //wait(0.5);
chrigelburri 18:306d362d692b 107 }
chrigelburri 18:306d362d692b 108
chrigelburri 19:b2f76b0fe4c8 109 void init()
chrigelburri 18:306d362d692b 110 {
chrigelburri 18:306d362d692b 111
chrigelburri 19:b2f76b0fe4c8 112 // Initialise the ADB subsystem.
chrigelburri 18:306d362d692b 113 ADB::init();
chrigelburri 18:306d362d692b 114
chrigelburri 18:306d362d692b 115 // Open an ADB stream on tcp port 4568. Auto-reconnect
chrigelburri 18:306d362d692b 116 connection = ADB::addConnection("tcp:4568", true, adbEventHandler);
chrigelburri 18:306d362d692b 117
chrigelburri 18:306d362d692b 118 // Connecting to android
chrigelburri 18:306d362d692b 119 while(!(androidConnected)) {
chrigelburri 18:306d362d692b 120 connect();
chrigelburri 18:306d362d692b 121 wait(0.5);
chrigelburri 18:306d362d692b 122 }
chrigelburri 18:306d362d692b 123
chrigelburri 19:b2f76b0fe4c8 124 }
chrigelburri 19:b2f76b0fe4c8 125
chrigelburri 19:b2f76b0fe4c8 126 void write2Android(char str [32])
chrigelburri 19:b2f76b0fe4c8 127 {
chrigelburri 19:b2f76b0fe4c8 128 connection->write(sizeof(str),(unsigned char*)&str);
chrigelburri 19:b2f76b0fe4c8 129 }
chrigelburri 19:b2f76b0fe4c8 130
chrigelburri 19:b2f76b0fe4c8 131 void writeActualPosition(float x, float y, float t)
chrigelburri 19:b2f76b0fe4c8 132 {
chrigelburri 19:b2f76b0fe4c8 133
chrigelburri 19:b2f76b0fe4c8 134 while(!(androidConnected)) {
chrigelburri 19:b2f76b0fe4c8 135 connect();
chrigelburri 19:b2f76b0fe4c8 136 wait(0.5);
chrigelburri 19:b2f76b0fe4c8 137 }
chrigelburri 19:b2f76b0fe4c8 138
chrigelburri 19:b2f76b0fe4c8 139 char str[32];
chrigelburri 19:b2f76b0fe4c8 140 sprintf( str, "%f;%f;%f;", x, y, t);
chrigelburri 19:b2f76b0fe4c8 141
chrigelburri 19:b2f76b0fe4c8 142 //pc.printf("Sending: %s\n\r",str);
chrigelburri 19:b2f76b0fe4c8 143 connection->write(sizeof(str),(unsigned char*)&str);
chrigelburri 19:b2f76b0fe4c8 144
chrigelburri 19:b2f76b0fe4c8 145 }
chrigelburri 19:b2f76b0fe4c8 146
chrigelburri 19:b2f76b0fe4c8 147 /*
chrigelburri 19:b2f76b0fe4c8 148 int main()
chrigelburri 19:b2f76b0fe4c8 149 {
chrigelburri 19:b2f76b0fe4c8 150
chrigelburri 19:b2f76b0fe4c8 151 pc.baud(460800);
chrigelburri 19:b2f76b0fe4c8 152
chrigelburri 19:b2f76b0fe4c8 153 pc.printf("********************* MicroBridge 4568 ********************************\n\r");
chrigelburri 19:b2f76b0fe4c8 154
chrigelburri 19:b2f76b0fe4c8 155
chrigelburri 19:b2f76b0fe4c8 156
chrigelburri 18:306d362d692b 157 pc.printf("connection isOpen\n");
chrigelburri 18:306d362d692b 158
chrigelburri 18:306d362d692b 159 float flt = 0.0;
chrigelburri 18:306d362d692b 160 float flt2 = 0.2;
chrigelburri 18:306d362d692b 161 float flt3 = 1.2;
chrigelburri 18:306d362d692b 162
chrigelburri 18:306d362d692b 163 while(1) {
chrigelburri 18:306d362d692b 164
chrigelburri 18:306d362d692b 165 ADB::poll();
chrigelburri 18:306d362d692b 166
chrigelburri 18:306d362d692b 167 flt = flt - 0.1;
chrigelburri 18:306d362d692b 168 flt2 = flt2 + 0.2;
chrigelburri 18:306d362d692b 169 flt3 = flt3 - 0.05;
chrigelburri 18:306d362d692b 170
chrigelburri 18:306d362d692b 171 char str[32];
chrigelburri 18:306d362d692b 172 sprintf( str, "%f;%f;%f;", flt, flt2, flt3);
chrigelburri 18:306d362d692b 173
chrigelburri 18:306d362d692b 174 pc.printf("Sending: %s\n\r",str);
chrigelburri 18:306d362d692b 175 connection->write(sizeof(str),(unsigned char*)&str);
chrigelburri 18:306d362d692b 176 wait(1);
chrigelburri 18:306d362d692b 177
chrigelburri 18:306d362d692b 178 }
chrigelburri 19:b2f76b0fe4c8 179 }
chrigelburri 19:b2f76b0fe4c8 180 */