Diff: Serializer.cpp

Revision:

1:9a7e6c978adb

Parent:

0:d2dcb47dc8e9

Child:

2:9e0d3f159ec3

--- a/Serializer.cpp	Sun Dec 12 23:49:02 2010 +0000
+++ b/Serializer.cpp	Mon Dec 13 04:26:04 2010 +0000
@@ -18,7 +18,6 @@
     DigitalOut led3(LED3);
     DigitalOut led4(LED4);
 
-    commandInProgress=1;
     led1=1;
     if (serial->writeable())
         serial->printf("fw\r");
@@ -29,7 +28,7 @@
         pcc.putc(c1);
     }
     if (c1!='>')
-        while (1) {
+        for (int i=0;i<5;i++) {
             led1=led2=led3=led4=1;
             wait(0.5);
             led1=led2=led3=led4=0;
@@ -50,9 +49,9 @@
     wait(.25);
     led4=0;
     _lPWM=_rPWM=0;
-
+    _isBusy=0;
+    _irqIsBusy=0;
 //    serial->attach(this, &Serializer::InterruptHandler);
-    commandInProgress=0;
 
     return 0;
 }
@@ -119,24 +118,66 @@
 }
 
 void Serializer::DiGoLeft(int dist,int inPsec) {
+/*    if(_isBusy)
+        return;
+    _isBusy=1;
+*/
     if (serial->writeable())
-        serial->printf("digo 1:%i:%i\r",-dist, inPsec);
+        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);
+        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:%f:%f 2:%f:%f\r", \
+        serial->printf("digo 1:%i:%i 2:%i:%i\r", \
                        -dist*PULSES_PER_INCH,   \
-                       inPsec,  \
+                       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) {
 
 }
@@ -147,8 +188,10 @@
 }
 void Serializer::PivetLeft(int deg) {
 
-    this->Stop();
-    deg=deg*PIVET_ADJUSTMENT + PIVET_ADJUSTMENT*10;
+    Stop();
+    pcc.printf("deg = %i\t",deg);
+    deg=deg*PIVET_ADJUSTMENT;
+    //pcc.printf("adj = %i\n\r",deg);
     wait(0.1);
     if (serial->writeable())
         serial->printf("digo 1:%i:%i 2:%i:%i\r",deg,PIVET_SPEED,-deg,PIVET_SPEED);
@@ -156,7 +199,7 @@
 }
 void Serializer::PivetRight(int deg) {
     this->Stop();
-    deg=deg*PIVET_ADJUSTMENT + PIVET_ADJUSTMENT*10;
+    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);
@@ -179,15 +222,15 @@
 int Serializer::GetCountLeft() {
     if (serial->writeable()) {
         serial->printf("getenc 1\r");
-//        pcc.printf("Sendreq\t");
+        //        pcc.printf("Sendreq\t");
     }
     wait(0.01);
     int ret=0;
     char c=0;
     while (serial->readable()) {
-//        pcc.printf("Reading");
+        //        pcc.printf("Reading");
         c=serial->getc();
-//        pcc.putc(c);
+        //        pcc.putc(c);
         if (c>='0'&&c<='9') {
             c-='0';
             ret*=10;
@@ -200,15 +243,15 @@
 int Serializer::GetCountRight() {
     if (serial->writeable()) {
         serial->printf("getenc 2\r");
-//        pcc.printf("Sendreq\t");
+        //        pcc.printf("Sendreq\t");
     }
     wait(0.01);
     int ret=0;
     char c=0;
     while (serial->readable()) {
-//        pcc.printf("Reading");
+        //        pcc.printf("Reading");
         c=serial->getc();
-//        pcc.putc(c);
+        //        pcc.putc(c);
         if (c>='0'&&c<='9') {
             c-='0';
             ret*=10;
@@ -262,11 +305,18 @@
 
 
 void Serializer::InterruptHandler() {
-    char c='0';
+    char c=0;
+    char bsy=_isBusy;
     while (serial->readable()) {
         c=serial->getc();
-        if (c=='>')
-            commandInProgress=0;
-        pcc.putc(c);
+        if (!_isBusy)
+            continue;
+
+        if (c=='1')
+            bsy=1;
+        else if (c=='0')
+            bsy=0;
     }
-}
\ No newline at end of file
+   pcc.putc(_isBusy+'0');
+    if (_isBusy&&!bsy) _isBusy=0;
+}