File content as of revision 1:d0f6e726b244:

#include "Serializer.h"

Serial pc(USBTX,USBRX);

Serializer::Serializer() {
    serial=new Serial(p28,p27);
    wait(2);
}

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

void Serializer::Stop() {
    if (serial->writeable())
        serial->printf("stop\r");
}

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 speed) {
    if (serial->writeable())
        serial->printf("mogo 1:%i\r", -speed);
}
void Serializer::SetSpeedRight(int speed) {
    if (serial->writeable())
        serial->printf("mogo 2:%i\r", -speed);
}
void Serializer::SetSpeed(int speed) {
    if (serial->writeable())
        serial->printf("mogo 1:%i 2:%i\r", -speed, -speed);
}

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 speed) {
    if (serial->writeable())
        serial->printf("digo 1:%i:%i\r",-dist, speed);
}
void Serializer::DiGoRight(int dist,int speed) {
    if (serial->writeable())
        serial->printf("digo 2:%i:%i\r",-dist, speed);
}
void Serializer::DiGo(int dist,int speed) {
    if (serial->writeable())
        serial->printf("digo 1:%i:%i 2:%i:%i\r",-dist, speed, -dist, speed);
}



//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;
}