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@18:306d362d692b, 2013-05-03 (annotated)

Committer:
chrigelburri
Date:
Fri May 03 08:35:29 2013 +0000
Revision:
18:306d362d692b
Child:
19:b2f76b0fe4c8
androidADB successfully linking

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 18:306d362d692b 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 18:306d362d692b 17 void Tokenize(const string& str,
chrigelburri 18:306d362d692b 18 vector<string>& tokens,
chrigelburri 18:306d362d692b 19 const string& delimiters /*= " "*/)
chrigelburri 18:306d362d692b 20 {
chrigelburri 18:306d362d692b 21 // Skip delimiters at beginning.
chrigelburri 18:306d362d692b 22 string::size_type lastPos = str.find_first_not_of(delimiters, 0);
chrigelburri 18:306d362d692b 23 // Find first "non-delimiter".
chrigelburri 18:306d362d692b 24 string::size_type pos = str.find_first_of(delimiters, lastPos);
chrigelburri 18:306d362d692b 25
chrigelburri 18:306d362d692b 26 while (string::npos != pos || string::npos != lastPos) {
chrigelburri 18:306d362d692b 27 // Found a token, add it to the vector.
chrigelburri 18:306d362d692b 28 tokens.push_back(str.substr(lastPos, pos - lastPos));
chrigelburri 18:306d362d692b 29 // Skip delimiters. Note the "not_of"
chrigelburri 18:306d362d692b 30 lastPos = str.find_first_not_of(delimiters, pos);
chrigelburri 18:306d362d692b 31 // Find next "non-delimiter"
chrigelburri 18:306d362d692b 32 pos = str.find_first_of(delimiters, lastPos);
chrigelburri 18:306d362d692b 33 }
chrigelburri 18:306d362d692b 34 }
chrigelburri 18:306d362d692b 35
chrigelburri 18:306d362d692b 36 void adbEventHandler(Connection * connection, adb_eventType event, uint16_t length, uint8_t * data)
chrigelburri 18:306d362d692b 37 {
chrigelburri 18:306d362d692b 38
chrigelburri 18:306d362d692b 39 if (event == ADB_CONNECTION_OPEN) {
chrigelburri 18:306d362d692b 40 androidConnected = true;
chrigelburri 18:306d362d692b 41 //pc.printf("Android Connected\n");
chrigelburri 18:306d362d692b 42 } else if (event == ADB_CONNECTION_CLOSE) {
chrigelburri 18:306d362d692b 43 androidConnected = false;
chrigelburri 18:306d362d692b 44 //pc.printf("Android Disonnected\n");
chrigelburri 18:306d362d692b 45 }
chrigelburri 18:306d362d692b 46
chrigelburri 18:306d362d692b 47 if (event == ADB_CONNECTION_RECEIVE) {
chrigelburri 18:306d362d692b 48 parseMessage(length, data);
chrigelburri 18:306d362d692b 49
chrigelburri 18:306d362d692b 50 }
chrigelburri 18:306d362d692b 51 }
chrigelburri 18:306d362d692b 52
chrigelburri 18:306d362d692b 53 void parseMessage(uint16_t length, uint8_t * data)
chrigelburri 18:306d362d692b 54 {
chrigelburri 18:306d362d692b 55 //received = "[ADB RECV]"; printf("[ADB RECV]:%d %d\r\n",data[0],data[1]);
chrigelburri 18:306d362d692b 56
chrigelburri 18:306d362d692b 57 char str[32];
chrigelburri 18:306d362d692b 58
chrigelburri 18:306d362d692b 59 // convert buffer (unsigned char) to char
chrigelburri 18:306d362d692b 60 sprintf( str, "%s", data);
chrigelburri 18:306d362d692b 61
chrigelburri 18:306d362d692b 62 // new vector of strings
chrigelburri 18:306d362d692b 63 vector<string> tokens;
chrigelburri 18:306d362d692b 64
chrigelburri 18:306d362d692b 65 // tokenize the string with the semicolon separator
chrigelburri 18:306d362d692b 66 Tokenize(str, tokens, ";");
chrigelburri 18:306d362d692b 67 copy(tokens.begin(), tokens.end(), ostream_iterator<string>(cout, ", "));
chrigelburri 18:306d362d692b 68
chrigelburri 18:306d362d692b 69 if(tokens.size() > 2) {
chrigelburri 18:306d362d692b 70
chrigelburri 18:306d362d692b 71 //string to float
chrigelburri 18:306d362d692b 72 androidx = ::atof(tokens.at(0).c_str());
chrigelburri 18:306d362d692b 73 androidy = ::atof(tokens.at(1).c_str());
chrigelburri 18:306d362d692b 74 androidt = ::atof(tokens.at(2).c_str());
chrigelburri 18:306d362d692b 75
chrigelburri 18:306d362d692b 76 //pc.printf("Android x(%d): %f\n\r\n",length,x);
chrigelburri 18:306d362d692b 77 //pc.printf("Android y(%d): %f\n\r\n",length,y);
chrigelburri 18:306d362d692b 78 //pc.printf("Android t(%d): %f\n\r\n",length,t);
chrigelburri 18:306d362d692b 79 } else {
chrigelburri 18:306d362d692b 80 //pc.printf("Android sayys(%d): %s\n\r\n",length,str);
chrigelburri 18:306d362d692b 81 }
chrigelburri 18:306d362d692b 82
chrigelburri 18:306d362d692b 83 }
chrigelburri 18:306d362d692b 84
chrigelburri 18:306d362d692b 85 void connect()
chrigelburri 18:306d362d692b 86 {
chrigelburri 18:306d362d692b 87 ADB::poll();
chrigelburri 18:306d362d692b 88 //pc.printf("connecting...");
chrigelburri 18:306d362d692b 89 char c = 'c';
chrigelburri 18:306d362d692b 90 connection->write(sizeof(c), (unsigned char*)&c);
chrigelburri 18:306d362d692b 91 //wait(0.5);
chrigelburri 18:306d362d692b 92 }
chrigelburri 18:306d362d692b 93
chrigelburri 18:306d362d692b 94 /*int main()
chrigelburri 18:306d362d692b 95 {
chrigelburri 18:306d362d692b 96
chrigelburri 18:306d362d692b 97 pc.baud(460800);
chrigelburri 18:306d362d692b 98
chrigelburri 18:306d362d692b 99 pc.printf("********************* MicroBridge 4568 ********************************\n\r");
chrigelburri 18:306d362d692b 100
chrigelburri 18:306d362d692b 101 // Initialise the ADB subsystem.
chrigelburri 18:306d362d692b 102 ADB::init();
chrigelburri 18:306d362d692b 103
chrigelburri 18:306d362d692b 104 // Open an ADB stream on tcp port 4568. Auto-reconnect
chrigelburri 18:306d362d692b 105 connection = ADB::addConnection("tcp:4568", true, adbEventHandler);
chrigelburri 18:306d362d692b 106
chrigelburri 18:306d362d692b 107 // Connecting to android
chrigelburri 18:306d362d692b 108 while(!(androidConnected)) {
chrigelburri 18:306d362d692b 109 connect();
chrigelburri 18:306d362d692b 110 wait(0.5);
chrigelburri 18:306d362d692b 111 }
chrigelburri 18:306d362d692b 112
chrigelburri 18:306d362d692b 113 pc.printf("connection isOpen\n");
chrigelburri 18:306d362d692b 114
chrigelburri 18:306d362d692b 115 float flt = 0.0;
chrigelburri 18:306d362d692b 116 float flt2 = 0.2;
chrigelburri 18:306d362d692b 117 float flt3 = 1.2;
chrigelburri 18:306d362d692b 118
chrigelburri 18:306d362d692b 119 while(1) {
chrigelburri 18:306d362d692b 120
chrigelburri 18:306d362d692b 121 ADB::poll();
chrigelburri 18:306d362d692b 122
chrigelburri 18:306d362d692b 123 flt = flt - 0.1;
chrigelburri 18:306d362d692b 124 flt2 = flt2 + 0.2;
chrigelburri 18:306d362d692b 125 flt3 = flt3 - 0.05;
chrigelburri 18:306d362d692b 126
chrigelburri 18:306d362d692b 127 char str[32];
chrigelburri 18:306d362d692b 128 sprintf( str, "%f;%f;%f;", flt, flt2, flt3);
chrigelburri 18:306d362d692b 129
chrigelburri 18:306d362d692b 130 pc.printf("Sending: %s\n\r",str);
chrigelburri 18:306d362d692b 131 connection->write(sizeof(str),(unsigned char*)&str);
chrigelburri 18:306d362d692b 132 wait(1);
chrigelburri 18:306d362d692b 133
chrigelburri 18:306d362d692b 134 }
chrigelburri 18:306d362d692b 135 }*/