File content as of revision 14:6a45a9f940a8:

#include "android.h"
//#define PI                    3.141592654f
using namespace std;

AdkTerm::AdkTerm(RobotControl* robotControl,
                 state_t* s,
                 float period)
    : AndroidAccessory(INBL,
                       OUTL,
                       "ARM",
                       "mbed",
                       "mbed Terminal",
                       "0.1",
                       "http://www.mbed.org",
                       "0000000012345678"),
    Task(period)
{
    this->robotControl = robotControl;
    this->period = period;
    this->s = s;
};

void AdkTerm::setupDevice()
{
    for (int i = 0; i<OUTL; i++) {
        buffer[i] = 0;
    }
    bcount = 0;
}

float AdkTerm::getDesiredX()
{
    return desiredX/1000;
}

float AdkTerm::getDesiredY()
{
    return desiredY/1000;
}

float AdkTerm::getDesiredTheta()
{
    return desiredTheta * PI / 180;
}


void AdkTerm::run()
{

    u8* wbuf = _writebuff;

    // floats to string
    char str[32];
    
    //send to android
    sprintf( str, "%f;%f;%f;%i;%i;%i;%f", robotControl->getxActualPosition(),
             robotControl->getyActualPosition(),
             robotControl->getActualTheta(),
             s->state&STATE_UNDER == 0 ? 0 : 1,
             s->state&STATE_LEFT == 0 ? 0 : 1,
             s->state&STATE_RIGHT == 0 ? 0 : 1,
             s->voltageBattery);

    //copy to wbuf
    strcpy( (char*) wbuf, str );

    this->write(wbuf,32);

}

void AdkTerm::resetDevice()
{
    for (int i = 0; i<OUTL; i++) {
        buffer[i] = 0;
    }
    bcount = 0;
}

void AdkTerm::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);
    }
}

int AdkTerm::callbackRead(u8 *buf, int len)
{
    // convert buffer (unsigned char) to string
    std::string str(reinterpret_cast<char*>(buf), len);

    // 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
        desiredX = ::atof(tokens.at(0).c_str());
        desiredY = ::atof(tokens.at(1).c_str());
        desiredTheta = ::atof(tokens.at(2).c_str());
    }

    return 0;
}

int AdkTerm::callbackWrite()
{
    ind = false;
    return 0;
}

void AdkTerm::serialIRQ()
{
    //buffer[bcount] = pc.getc();


    if (buffer[bcount] == '\n' || buffer[bcount] == '\r') {
        u8* wbuf = _writebuff;
        for (int i = 0; i<OUTL; i++) {
            wbuf[i] = buffer[i];
            buffer[i] = 0;
        }
        //pc.printf("Sending: %s\n\r",wbuf);
        ind = true;
        this->write(wbuf,bcount);
        bcount = 0;
    } else {
        if (buffer[bcount] != 0x08 && buffer[bcount] != 0x7F ) {
            bcount++;
            if (bcount == OUTL) {
                bcount = 0;
            }
        } else {
            bcount--;
        }
    }

}