Diff: main.cpp

Revision:

9:2c85d7f99a14

Parent:

8:41b35bda9d48

Child:

10:37e7c46837dc

--- a/main.cpp	Thu Mar 06 10:38:43 2014 +0000
+++ b/main.cpp	Thu Mar 06 18:09:37 2014 +0000
@@ -10,6 +10,12 @@
 #define SENSORS_NUM 3
 #define BYTES_NUM 3
 
+#define DEBUG false
+
+#define MAX_OVERFLOWS 3
+
+#define MAX_REPLY_ERRORS 3
+
 Servo servo1(p21);
 Servo servo2(p24);
 
@@ -35,11 +41,18 @@
 PS2MS sensor3(p26, p25);
 
 int process_sensor_input(int c, int bytenum, char* bytes, int ind);
+bool processACKReply(int reply, int ind);
+void sendError(int ind);
 
 int sensorXs[SENSORS_NUM];
 int sensorYs[SENSORS_NUM];
 bool sensorToPrint[SENSORS_NUM];
 
+bool expectingAck1 = false, expectingAck2 = false, expectingAck3 = false;
+int consecutiveOverflows[3];
+
+int replyErrors[3];
+
 int main()
 {
 
@@ -50,10 +63,13 @@
     
     float newPos1 = position1, newPos2 = position2;
     
+    consecutiveOverflows[0] = consecutiveOverflows[1] = consecutiveOverflows[2] = 0;
+    replyErrors[0] = replyErrors[1] = replyErrors[2] = 0;
+    
     servo1.calibrate(range, 45.0); 
     servo2.calibrate(range, 45.0); 
-    servo1 = position1;
-    servo2 = position2;
+    servo1 = position1 + position_adj;
+    servo2 = position2 + position_adj;
     printf("position = %.3f, range = +/-%0.5f\n", position1, range, position_adj);
 
     printf("IMHERE START\n");
@@ -76,14 +92,18 @@
     printf("IMHERE GOT S3\n");
     sensorToPrint[0] = sensorToPrint[1] = sensorToPrint[2] = false;
     int dir;
-    float servoSpeed = 0.001;
+    float servoSpeed = 0.002;
+    
+    int recv;
+    
+    bool awaitingPackets = false;
     
     while(1) {
         if (pc.readable()) {
             switch(pc.getc()) {
             case '1': newPos1 = 0.0 + position_adj; break; //position1 = 0.0 + position_adj; break;
             case '2': newPos1 = 0.5 + position_adj; break; //position1 = 0.5 + position_adj; break;
-            case '3': newPos1 = 0.6 + position_adj; break; //position1 = 1.0 + position_adj; break;
+            case '3': newPos1 = 1.0 + position_adj; break; //position1 = 1.0 + position_adj; break;
             case '4': newPos2 = 0.3 + position_adj; break;
             case '5': newPos2 = 0.5 + position_adj; break;
             case '6': newPos2 = 0.7 + position_adj; break;
@@ -117,18 +137,45 @@
         else {
             position2 = newPos2;
         }
-    
+        
+        if (!awaitingPackets) {
+            sensor1_init.send('\xEB');
+            expectingAck1 = true;
+            sensor2_init.send('\xEB');
+            expectingAck2 = true;
+            sensor3_init.send('\xEB');
+            expectingAck3 = true;
+            awaitingPackets = true;
+        }
+        
         if (ENABLE_1) {
-            s1bytenum = process_sensor_input(s1c, s1bytenum, s1bytes, 0);
+            
             s1c = sensor1.getc();
+            if (expectingAck1) {
+                expectingAck1 = processACKReply(s1c, 1);
+            }
+            else {
+                s1bytenum = process_sensor_input(s1c, s1bytenum, s1bytes, 0);
+            }
         }
         if (ENABLE_2) {
-            s2bytenum = process_sensor_input(s2c, s2bytenum, s2bytes, 1);
             s2c = sensor2.getc();
+            if (expectingAck2) {
+                expectingAck2 = processACKReply(s2c, 2);
+            }
+            else {
+                s2bytenum = process_sensor_input(s2c, s2bytenum, s2bytes, 1);
+            }
         }
         if (ENABLE_3) {
-            s3bytenum = process_sensor_input(s3c, s3bytenum, s3bytes, 2);
+            
             s3c = sensor3.getc();
+            if (expectingAck3) {
+                expectingAck3 = processACKReply(s3c, 3);
+            }
+            else {
+                s3bytenum = process_sensor_input(s3c, s3bytenum, s3bytes, 2);
+            }
         }
         // TODO only prints when both are enabled now
         if (sensorToPrint[0] && sensorToPrint[1] && sensorToPrint[2]) {
@@ -136,8 +183,40 @@
                 sensorXs[2], sensorYs[2]);
             sensorToPrint[0] = sensorToPrint[1] = sensorToPrint[2] = false;
             sensorXs[0] = sensorYs[0] = sensorXs[1] = sensorYs[1] = sensorXs[2] = sensorYs[2] = 0;
+            awaitingPackets = false;
+        }
+    }
+}
+
+bool processACKReply(int reply, int ind) {
+    if (reply < 0) {
+        if (DEBUG) printf("%d: Error %d - %d", ind, reply, replyErrors[ind]);
+        replyErrors[ind]++;
+        if (replyErrors[ind] > MAX_REPLY_ERRORS) {
+            if (DEBUG) printf("%d: MAX ERRORS REACHED %d", ind, reply);
+            replyErrors[ind] = 0;
+            return false;
         }
     }
+    else if (reply == '\xFA') {
+        if (DEBUG) printf("%d: ACK ", ind);
+        return false;
+    }
+    else if (reply == '\xEB') {
+        if (DEBUG) printf("%d: READ_DATA ", ind);
+    }
+    else if (reply == '\xFE') {
+        if (DEBUG) printf("%d: RESEND received\n\r", ind);
+        return false;
+    }
+    else if (reply == '\xF2') {
+        if (DEBUG) printf("%d: GET_DEVICE_ID received\n\r", ind);
+        return false;
+    }
+    else {
+        if (DEBUG) printf("%d: Unexpected ACK Reply %d\n\r", ind, reply);
+    }
+    return true;
 }
 
 int process_sensor_input(int c, int bytenum, char* bytes, int ind)
@@ -149,8 +228,9 @@
         bytes[0] = c;
         if (!((c << 5) & 0x100)) {
             // not byte[0] wrong offset, skip c
-            //printf("%d: w %d\n\r", ind, c);
+            if (DEBUG) printf("%d: w %d\n\r", ind, c);
             bytenum = -1;
+            sendError(ind);
         }
     } else if (bytenum % BYTES_NUM == 1) {
         bytes[1] = c;
@@ -160,13 +240,24 @@
 
         //TODO: check for overflow
         if ((1 << 6) & bytes[0]) {
-            printf("Overflow x! %d\n\r", ind);
+            printf("%d: Overflow x %d %d %d - %d!\n\r", ind, bytes[0], bytes[1], bytes[2], consecutiveOverflows[ind]);
+            if (consecutiveOverflows[ind]++ < MAX_OVERFLOWS) {
+                sendError(ind);
+            }
+            else {
+                consecutiveOverflows[ind] = 0;
+            }
             bytenum = -1;
         }
         else if ((1 << 7) & bytes[0]) {
-            printf("Overflow y! %d\n\r", ind);
-            //printf("Byte1 is %d\n\r", bytes[0]);
-            bytenum = -1;   
+            printf("%d: Overflow y %d %d %d - %d!\n\r", ind, bytes[0], bytes[1], bytes[2], consecutiveOverflows[ind]);
+            if (consecutiveOverflows[ind]++ < MAX_OVERFLOWS) {
+                sendError(ind);
+            }
+            else {
+                consecutiveOverflows[ind] = 0;
+            }
+            bytenum = -1;
         }
         // check x and y signs
         else {
@@ -182,3 +273,21 @@
     }
     return (bytenum + 1) % BYTES_NUM;
 }
+
+void sendError(int ind)
+{
+    switch (ind) {
+        case 0:
+            sensor1_init.send('\xFE');
+            expectingAck1 = true;
+            break;
+        case 1:
+            sensor2_init.send('\xFE');
+            expectingAck2 = true;
+            break;
+        case 2:
+            sensor3_init.send('\xFE');
+            expectingAck3 = true;
+            break;
+    }
+}
\ No newline at end of file