File content as of revision 4:959812c02a0c:
/**
* 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);
}
//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);
}
}