File content as of revision 21:48248c5b8992:

#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 /*= (2 * PI) + 1*/;

/** @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);
    }

}
//////// Brachts dies noch?????????
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);

    }
}
*/