Diff: Serializer/Serializer.cpp

Revision:

0:ee786e500a3c

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/Serializer/Serializer.cpp	Fri Dec 17 18:27:33 2010 +0000
@@ -0,0 +1,330 @@
+/**
+ * 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);
+
+    led1=1;
+    if (serial->writeable())
+        serial->printf("fw\r");
+    wait(.25);
+    led2=1;
+    while (serial->readable()) {
+        c1=serial->getc();
+    }
+    if (c1!='>')
+        for (int i=0;i<5;i++) {
+            led1=led2=led3=led4=1;
+            wait(0.1);
+            led1=led2=led3=led4=0;
+            wait(0.1);
+        }
+    wait(.25);
+    led3=1;
+    wait(.25);
+    led4=1;
+ //   SetVPID(1,0,0,100);
+    wait(0.25);
+    led1=0;
+    wait(.25);
+    ClearCount();
+    led2=0;
+    wait(.25);
+    led3=0;
+    wait(.25);
+    led4=0;
+    _lPWM=_rPWM=0;
+    _isBusy=0;
+    _irqIsBusy=0;
+//    serial->attach(this, &Serializer::InterruptHandler);
+
+    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) {
+    if (inPsec>MAX_SPEED)inPsec=MAX_SPEED;
+    if (inPsec<-MAX_SPEED)inPsec=-MAX_SPEED;
+    inPsec*=-1;
+    if (serial->writeable())
+        serial->printf("mogo 1:%i 2:%i\r", inPsec-50, 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(_isBusy)
+        return;
+    _isBusy=1;
+*/
+    if (serial->writeable())
+        serial->printf("digo 1:%i:%i\r",dist*PULSES_PER_INCH, inPsec);
+}
+void Serializer::DiGoRight(int dist,int inPsec) {
+/*     if(_isBusy)
+        return;
+    _isBusy=1;
+*/
+    if (serial->writeable())
+        serial->printf("digo 2:%i:%i\r",dist*PULSES_PER_INCH, inPsec);
+}
+void Serializer::DiGo(int dist,int inPsec) {
+/*    if(_isBusy)
+        return;
+    _isBusy=1;
+*/
+    if (serial->writeable()) {
+        serial->printf("digo 1:%i:%i 2:%i:%i\r", \
+                       -dist*PULSES_PER_INCH,   \
+                       inPsec,                  \
+                       -dist*PULSES_PER_INCH,   \
+                       inPsec);
+
+    }
+}
+
+int Serializer::IsBusy() {
+
+    if (_isBusy&&serial->writeable()) {
+        serial->printf("pids\r");
+        wait(0.01);
+    }
+    return _isBusy;
+}
+/*
+      char tmp=0;
+        if (_isBusy) {
+            if (serial->writeable()) {
+                serial->printf("pids\r");
+            }
+            wait(0.1);
+            while (serial->readable()) {
+                tmp=serial->getc();
+                pcc.putc(tmp);
+                if (tmp=='1')
+                    _isBusy=1;
+                else if (tmp=='0')
+                    _isBusy=0;
+            }
+        }
+        pcc.printf("IsBusy = %i\n\r",_isBusy);
+        return _isBusy;
+*/
+
+
+
+void Serializer::TurnLeft(int deg) {
+
+}
+
+void Serializer::TurnRight(int deg) {
+
+
+}
+void Serializer::PivetLeft(int deg) {
+
+    Stop();
+    deg=deg*PIVET_ADJUSTMENT;
+    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;
+    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);
+}
+
+void Serializer::SetPWM(int pwm){
+    _rPWM=-pwm;
+    _lPWM=-pwm;
+    if (serial->writeable())
+        serial->printf("pwm 1:%i 2:%i\r",_lPWM, _rPWM);      
+ }
+
+void Serializer::SetPWM(int lPwm, int rPwm){
+    _rPWM=-rPwm;
+    _lPWM=-lPwm;
+    if (serial->writeable())
+        serial->printf("pwm 1:%i 2:%i\r",_lPWM, _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();
+}
+float Serializer::GetDistanceLeft() {
+    return float(GetCountLeft()*PULSE_DISTANCE);
+}
+float Serializer::GetDistanceRight() {
+    return float(GetCountRight()*PULSE_DISTANCE);
+}
+float Serializer::GetDistance() {
+    return float(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;
+    char bsy=_isBusy;
+    while (serial->readable()) {
+        c=serial->getc();
+        if (!_isBusy)
+            continue;
+
+        if (c=='1')
+            bsy=1;
+        else if (c=='0')
+            bsy=0;
+    }
+    if (_isBusy&&!bsy) _isBusy=0;
+}