androidADB.cpp

00001 #include "androidADB.h"
00002 
00003 Connection * connection;
00004 
00005 /** @brief Desired position in meters for x-coordinate, given by android */
00006 float androidx;
00007 
00008 /** @brief Desired position in meters for y-coordinate, given by android */
00009 float androidy;
00010 
00011 /** @brief Desired position in degrees for theta, given by android */
00012 float androidt;
00013 
00014 /** @brief Indicates if a ADB connection to a android phone is established */
00015 boolean androidConnected;
00016 
00017 float getDesiredX()
00018 {
00019     return androidx/1000;
00020 }
00021 
00022 float getDesiredY()
00023 {
00024     return androidy/1000;
00025 }
00026 
00027 float getDesiredTheta()
00028 {
00029     return androidt * PI / 180;
00030 }
00031 
00032 void setDesiredTheta(float t)
00033 {
00034     androidt = t;
00035 }
00036 
00037 void Tokenize(const string& str,
00038               vector<string>& tokens,
00039               const string& delimiters /*= " "*/)
00040 {
00041     // Skip delimiters at beginning.
00042     string::size_type lastPos = str.find_first_not_of(delimiters, 0);
00043     // Find first "non-delimiter".
00044     string::size_type pos     = str.find_first_of(delimiters, lastPos);
00045 
00046     while (string::npos != pos || string::npos != lastPos) {
00047         // Found a token, add it to the vector.
00048         tokens.push_back(str.substr(lastPos, pos - lastPos));
00049         // Skip delimiters.  Note the "not_of"
00050         lastPos = str.find_first_not_of(delimiters, pos);
00051         // Find next "non-delimiter"
00052         pos = str.find_first_of(delimiters, lastPos);
00053     }
00054 }
00055 
00056 extern void adbEventHandler(Connection * connection, adb_eventType event, uint16_t length, uint8_t * data)
00057 {
00058 
00059     if (event == ADB_CONNECTION_OPEN) {
00060         androidConnected = true;
00061     } else if (event == ADB_CONNECTION_CLOSE) {
00062         androidConnected = false;
00063     }
00064 
00065     if (event == ADB_CONNECTION_RECEIVE) {
00066         parseMessage(length, data);
00067 
00068     }
00069 }
00070 
00071 void parseMessage(uint16_t length, uint8_t * data)
00072 {
00073     char str[32];
00074 
00075     // convert buffer (unsigned char) to char
00076     sprintf( str, "%s", data);
00077 
00078     // new vector of strings
00079     vector<string> tokens;
00080 
00081     // tokenize the string with the semicolon separator
00082     Tokenize(str, tokens, ";");
00083     copy(tokens.begin(), tokens.end(), ostream_iterator<string>(cout, ", "));
00084 
00085     if(tokens.size() > 2) {
00086 
00087         //string to float
00088         androidx = ::atof(tokens.at(0).c_str());
00089         androidy = ::atof(tokens.at(1).c_str());
00090         androidt = ::atof(tokens.at(2).c_str());
00091 
00092     } else {}
00093 
00094 }
00095 
00096 void connect()
00097 {
00098     ADB::poll();
00099     char c = 'c';
00100     connection->write(sizeof(c), (unsigned char*)&c);
00101 }
00102 
00103 void init()
00104 {
00105 
00106     // Initialise the ADB subsystem.
00107     ADB::init();
00108 
00109     // Open an ADB stream on tcp port 4568. Auto-reconnect
00110     connection = ADB::addConnection("tcp:4568", true, adbEventHandler);
00111 
00112     // Connecting to android
00113     while(!(androidConnected)) {
00114         connect();
00115         wait(0.5);
00116     }
00117 
00118 }
00119 
00120 void writeActualPosition(float x, float y, float t, int state_u, int state_l, int state_r, float volt_b)
00121 {
00122     // reconnect funktioniert trotzdem nicht!?
00123     while(!(androidConnected)) {
00124         connect();
00125         wait(0.5);
00126     }
00127 
00128     char str[100];
00129 
00130     //send to android
00131     sprintf( str, "%f;%f;%f;%i;%i;%i;%f;;", x,
00132              y,
00133              t * 180 / PI,
00134              state_u == 0 ? 0 : 1,
00135              state_l == 0 ? 0 : 1,
00136              state_r == 0 ? 0 : 1,
00137              volt_b);
00138 
00139     connection->write(sizeof(str),(unsigned char*)&str);
00140 
00141 }