Diff: MicroBridge/androidADB.cpp

Revision:

18:306d362d692b

Child:

19:b2f76b0fe4c8

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/MicroBridge/androidADB.cpp	Fri May 03 08:35:29 2013 +0000
@@ -0,0 +1,135 @@
+#include "androidADB.h"
+
+Connection * connection;
+
+/** @brief Desired position in meters for x-coordinate, given by android */
+float androidx;
+
+/** @brief Desired position in meters for y-coordinate, given by android */
+float androidy;
+
+/** @brief Desired position in degrees for theta, given by android */
+float androidt;
+
+/** @brief Indicates if a ADB connection to a android phone is established */
+boolean androidConnected;
+
+void Tokenize(const string& str,
+              vector<string>& tokens,
+              const string& delimiters /*= " "*/)
+{
+    // Skip delimiters at beginning.
+    string::size_type lastPos = str.find_first_not_of(delimiters, 0);
+    // Find first "non-delimiter".
+    string::size_type pos     = str.find_first_of(delimiters, lastPos);
+
+    while (string::npos != pos || string::npos != lastPos) {
+        // Found a token, add it to the vector.
+        tokens.push_back(str.substr(lastPos, pos - lastPos));
+        // Skip delimiters.  Note the "not_of"
+        lastPos = str.find_first_not_of(delimiters, pos);
+        // Find next "non-delimiter"
+        pos = str.find_first_of(delimiters, lastPos);
+    }
+}
+
+void adbEventHandler(Connection * connection, adb_eventType event, uint16_t length, uint8_t * data)
+{
+
+    if (event == ADB_CONNECTION_OPEN) {
+        androidConnected = true;
+        //pc.printf("Android Connected\n");
+    } else if (event == ADB_CONNECTION_CLOSE) {
+        androidConnected = false;
+        //pc.printf("Android Disonnected\n");
+    }
+
+    if (event == ADB_CONNECTION_RECEIVE) {
+        parseMessage(length, data);
+
+    }
+}
+
+void parseMessage(uint16_t length, uint8_t * data)
+{
+    //received = "[ADB RECV]";    printf("[ADB RECV]:%d   %d\r\n",data[0],data[1]);
+
+    char str[32];
+
+    // convert buffer (unsigned char) to char
+    sprintf( str, "%s", data);
+
+    // new vector of strings
+    vector<string> tokens;
+
+    // tokenize the string with the semicolon separator
+    Tokenize(str, tokens, ";");
+    copy(tokens.begin(), tokens.end(), ostream_iterator<string>(cout, ", "));
+
+    if(tokens.size() > 2) {
+
+        //string to float
+        androidx = ::atof(tokens.at(0).c_str());
+        androidy = ::atof(tokens.at(1).c_str());
+        androidt = ::atof(tokens.at(2).c_str());
+
+        //pc.printf("Android x(%d): %f\n\r\n",length,x);
+        //pc.printf("Android y(%d): %f\n\r\n",length,y);
+        //pc.printf("Android t(%d): %f\n\r\n",length,t);
+    } else {
+        //pc.printf("Android sayys(%d): %s\n\r\n",length,str);
+    }
+
+}
+
+void connect()
+{
+    ADB::poll();
+    //pc.printf("connecting...");
+    char c = 'c';
+    connection->write(sizeof(c), (unsigned char*)&c);
+    //wait(0.5);
+}
+
+/*int main()
+{
+
+    pc.baud(460800);
+
+    pc.printf("********************* MicroBridge 4568 ********************************\n\r");
+
+    // Initialise the ADB subsystem.
+    ADB::init();
+
+    // Open an ADB stream on tcp port 4568. Auto-reconnect
+    connection = ADB::addConnection("tcp:4568", true, adbEventHandler);
+
+    // Connecting to android
+    while(!(androidConnected)) {
+        connect();
+        wait(0.5);
+    }
+
+    pc.printf("connection isOpen\n");
+
+    float flt = 0.0;
+    float flt2 = 0.2;
+    float flt3 = 1.2;
+
+    while(1) {
+
+        ADB::poll();
+
+        flt = flt - 0.1;
+        flt2 = flt2 + 0.2;
+        flt3 = flt3 - 0.05;
+
+        char str[32];
+        sprintf( str, "%f;%f;%f;", flt, flt2, flt3);
+
+        pc.printf("Sending: %s\n\r",str);
+        connection->write(sizeof(str),(unsigned char*)&str);
+        wait(1);
+
+    }
+}*/