Diff: Serializer.cpp

Revision:

1:d0f6e726b244

Parent:

0:a6113b3c872b

Child:

2:0a3c369d0076

--- a/Serializer.cpp	Thu Nov 25 00:10:20 2010 +0000
+++ b/Serializer.cpp	Fri Nov 26 21:00:50 2010 +0000
@@ -1,114 +1,98 @@
-#include "Serializer.h"
-
-Serial pc(USBTX,USBRX);
-
-Serializer::Serializer() {
-    serial=new Serial(p28,p27);
-    wait(2);
-
-    isBusy=0;
-    serial->attach(this, &Serializer::InterruptHandler);
-}
-
-Serializer::~Serializer() {
-    delete serial;
-}
-
-void Serializer::Reset() {
-//    if (serial->writeable())
-//        serial->printf("reset\r");
-}
-int Serializer::IsBusy() {
-    if (serial->writeable())
-        serial->printf("pids\r");
-    return isBusy;
-
-}
-void Serializer::Stop() {
-    if (serial->writeable())
-        serial->printf("stop\r");
-}
-
-int Serializer::GetCountLeft() {
-    return 0;
-}
-int Serializer::GetCountRight() {
-    return 0;
-}
-
-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 a,int b,int c,int d) {
-}
-void Serializer::SetDPID(int a,int b,int,int) {
-}
-
-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);
-}
-
-
-void Serializer::InterruptHandler() {
-    serial->getc();
- /*   pc.putc(serial->getc());
-        isBusy=1;
-        int isReady=0;
-        int len=0;
-        while (serial->readable()) {
-            rxBuffer[len++]=serial->getc();
-            if (len>96)break;
-        }
-        for (int i=0;i<len;i++) {
-            if ((rxBuffer[i]=='A')&&(rxBuffer[i+1]=='C')&&(rxBuffer[i+2]=='K')) {
-                if (i&&(rxBuffer[i-1]=='N'))
-                    response=NEGATIVE;
-                else
-                    response=WILCO;
-                continue;
-            }
-            if (rxBuffer[i]=='>'){
-                isReady=1;
-                break;
-            }
-
-        }
-
-
-        isBusy=0;
-        return;
-     */
-}
+#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;
+}