File content as of revision 0:d2dcb47dc8e9:

/**
 * Includes
 */
#include "Serializer.h"

Serial pcc(USBTX, USBRX);

Serializer::Serializer() {
    serial=new Serial(p28,p27);
    serial->baud(BAUD);
    Initialize();
}

int Serializer::Initialize() {
    char c1=0;
    DigitalOut led1(LED1);
    DigitalOut led2(LED2);
    DigitalOut led3(LED3);
    DigitalOut led4(LED4);

    commandInProgress=1;
    led1=1;
    if (serial->writeable())
        serial->printf("fw\r");
    wait(.25);
    led2=1;
    while (serial->readable()) {
        c1=serial->getc();
        pcc.putc(c1);
    }
    if (c1!='>')
        while (1) {
            led1=led2=led3=led4=1;
            wait(0.5);
            led1=led2=led3=led4=0;
            wait(0.5);
        }
    wait(.25);
    led3=1;
    wait(.25);
    led4=1;
    SetVPID(10,0,5,10);
    wait(0.25);
    led1=0;
    wait(.25);
    ClearCount();
    led2=0;
    wait(.25);
    led3=0;
    wait(.25);
    led4=0;
    _lPWM=_rPWM=0;

//    serial->attach(this, &Serializer::InterruptHandler);
    commandInProgress=0;

    return 0;
}

Serializer::~Serializer() {
    delete serial;
}

void Serializer::Stop() {
    if (serial->writeable())
        serial->printf("stop\r");
    leftSpeed=rightSpeed=0;
//    pc.printf("Stop\r\n");
}

void Serializer::ClearCountLeft() {
    if (serial->writeable())
        serial->printf("clrenc 1\r");
}
void Serializer::ClearCountRight() {
    if (serial->writeable())
        serial->printf("clrenc 2\r");
}

void Serializer::ClearCount() {
    if (serial->writeable())
        serial->printf("clrenc 1 2\r");
}

void Serializer::SetSpeedLeft(int inPsec) {
    if (inPsec>MAX_SPEED)inPsec=MAX_SPEED;
    if (inPsec<-MAX_SPEED)inPsec=-MAX_SPEED;
    if (serial->writeable())
        serial->printf("mogo 1:%i\r", -inPsec);
    leftSpeed=inPsec;
}
void Serializer::SetSpeedRight(int inPsec) {
    if (inPsec>MAX_SPEED)inPsec=MAX_SPEED;
    if (inPsec<-MAX_SPEED)inPsec=-MAX_SPEED;
    if (serial->writeable())
        serial->printf("mogo 2:%i\r", -inPsec);
    rightSpeed=inPsec;
}

void Serializer::SetSpeed(int inPsec) {
    pcc.printf("inSpeed %i\n\r",inPsec);
    if (inPsec>MAX_SPEED)inPsec=MAX_SPEED;
    if (inPsec<-MAX_SPEED)inPsec=-MAX_SPEED;
    inPsec*=-1;
    pcc.printf("mogo 1:%i\n\r",inPsec);
    if (serial->writeable())
        serial->printf("mogo 1:%i 2:%i\r", inPsec, inPsec);
    leftSpeed=rightSpeed=inPsec;
}

void Serializer::SetVPID(int p,int i,int d,int l) {
    if (serial->writeable())
        serial->printf("vpid %i:%i:%i:%i\r", p,i,d,l);
}

void Serializer::SetDPID(int p,int i,int d,int a) {
    if (serial->writeable())
        serial->printf("dpid %i:%i:%i:%i\r", p,i,d,a);
}

void Serializer::DiGoLeft(int dist,int inPsec) {
    if (serial->writeable())
        serial->printf("digo 1:%i:%i\r",-dist, inPsec);
}
void Serializer::DiGoRight(int dist,int inPsec) {
    if (serial->writeable())
        serial->printf("digo 2:%i:%i\r",-dist*PULSES_PER_INCH, inPsec);
}
void Serializer::DiGo(int dist,int inPsec) {
    if (serial->writeable()) {
        serial->printf("digo 1:%f:%f 2:%f:%f\r", \
                       -dist*PULSES_PER_INCH,   \
                       inPsec,  \
                       -dist*PULSES_PER_INCH,   \
                       inPsec);

    }
}

void Serializer::TurnLeft(int deg) {

}

void Serializer::TurnRight(int deg) {


}
void Serializer::PivetLeft(int deg) {

    this->Stop();
    deg=deg*PIVET_ADJUSTMENT + PIVET_ADJUSTMENT*10;
    wait(0.1);
    if (serial->writeable())
        serial->printf("digo 1:%i:%i 2:%i:%i\r",deg,PIVET_SPEED,-deg,PIVET_SPEED);

}
void Serializer::PivetRight(int deg) {
    this->Stop();
    deg=deg*PIVET_ADJUSTMENT + PIVET_ADJUSTMENT*10;
    wait(0.1);
    if (serial->writeable())
        serial->printf("digo 1:%i:%i 2:%i:%i\r",-deg,PIVET_SPEED,deg,PIVET_SPEED);

}

void Serializer::SetLeftPWM(int pwm) {
    _lPWM=-pwm;
    if (serial->writeable())
        serial->printf("pwm 1:%i\r",_lPWM);

}

void Serializer::SetRightPWM(int pwm) {
    _rPWM=-pwm;
    if (serial->writeable())
        serial->printf("pwm 2:%i\r",_rPWM);
}

int Serializer::GetCountLeft() {
    if (serial->writeable()) {
        serial->printf("getenc 1\r");
//        pcc.printf("Sendreq\t");
    }
    wait(0.01);
    int ret=0;
    char c=0;
    while (serial->readable()) {
//        pcc.printf("Reading");
        c=serial->getc();
//        pcc.putc(c);
        if (c>='0'&&c<='9') {
            c-='0';
            ret*=10;
            ret+=c;
        }
    }
    return ret;
}

int Serializer::GetCountRight() {
    if (serial->writeable()) {
        serial->printf("getenc 2\r");
//        pcc.printf("Sendreq\t");
    }
    wait(0.01);
    int ret=0;
    char c=0;
    while (serial->readable()) {
//        pcc.printf("Reading");
        c=serial->getc();
//        pcc.putc(c);
        if (c>='0'&&c<='9') {
            c-='0';
            ret*=10;
            ret+=c;
        }
    }
    return ret;
}
int Serializer::GetCount() {
    return GetCountLeft();
}
int Serializer::GetDistanceLeft() {
    return GetCountLeft()*PULSE_DISTANCE;
}
int Serializer::GetDistanceRight() {
    return GetCountRight()*PULSE_DISTANCE;
}
int Serializer::GetDistance() {
    return GetCountLeft()*PULSE_DISTANCE;
}


//unused and untested
int Serializer::GetReply() {
    int returnValue=0;
    char c=0;
    char readyToReturn=0;
    char ack=0;
    char nack=0;
    wait(0.0001);
    while (serial->readable()) {
        c=serial->getc();
        if (readyToReturn)continue;
        if (c=='N'&&nack==0)nack=1;
        else nack=0;
        if (c=='A'&&ack==0)ack++;
        else ack=nack=0;
        if (c=='C'&&ack==1)ack++;
        else ack=nack=0;
        if (c=='K'&&ack==2)ack++;
        else ack=nack=0;
        if (ack==3)
            if (nack==1)
                returnValue=1;
            else
                returnValue=0;
        if (c=='>')readyToReturn=1;
    }
    return returnValue;
}


void Serializer::InterruptHandler() {
    char c='0';
    while (serial->readable()) {
        c=serial->getc();
        if (c=='>')
            commandInProgress=0;
        pcc.putc(c);
    }
}