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
Fork of autonomous Robot Android by
MicroBridge/androidADB.cpp
- Committer:
- chrigelburri
- Date:
- 2013-05-03
- Revision:
- 20:01b233b0e606
- Parent:
- 19:b2f76b0fe4c8
- Child:
- 21:48248c5b8992
File content as of revision 20:01b233b0e606:
#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; float getDesiredX() { return androidx/1000; } float getDesiredY() { return androidy/1000; } float getDesiredTheta() { return androidt * PI / 180; } 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); } } extern 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; 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()); printf("Android x(%d): %f\n\r\n",length,androidx); printf("Android y(%d): %f\n\r\n",length,androidy); printf("Android t(%d): %f\n\r\n",length,androidt); } else { //pc.printf("Android sayys(%d): %s\n\r\n",length,str); } } void connect() { ADB::poll(); printf("connecting..."); char c = 'c'; connection->write(sizeof(c), (unsigned char*)&c); //wait(0.5); } void init() { // 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); } } void write2Android(char str [32]) { connection->write(sizeof(str),(unsigned char*)&str); } void writeActualPosition(float x, float y, float t, int state_u, int state_l, int state_r, float volt_b) { // reconnect funktioniert trotzdem nicht!? while(!(androidConnected)) { connect(); wait(0.5); } char str[100]; //send to android sprintf( str, "%f;%f;%f;%i;%i;%i;%f;;", x, y, t * 180 / PI, state_u == 0 ? 0 : 1, state_l == 0 ? 0 : 1, state_r == 0 ? 0 : 1, volt_b); connection->write(sizeof(str),(unsigned char*)&str); } /* int main() { pc.baud(460800); pc.printf("********************* MicroBridge 4568 ********************************\n\r"); 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); } } */