Diff: main.cpp

Revision:

12:c9d0b1ff36f2

Parent:

11:f5dcf8811a4e

Child:

13:582064cfb9b2

diff -r f5dcf8811a4e -r c9d0b1ff36f2 main.cpp
--- a/main.cpp	Wed Mar 12 10:35:47 2014 +0000
+++ b/main.cpp	Thu Mar 13 12:07:48 2014 +0000
@@ -15,7 +15,6 @@
 Servo servoYaw(p21);
 Servo servoPitch(p24);
 
-DigitalOut myled(LED1);
 Serial pc(USBTX, USBRX); // tx, rx
 /*
  * 0xFF: Reset command.
@@ -28,8 +27,6 @@
 
 //TODO should Iuse sensor1_init? maybe no 255s?
 PS2MS* sensor[3];
-PS2MS_INIT* sensor_init[3];
-
 
 int process_sensor_input(int c, int bytenum, char* bytes, int ind);
 bool processACKReply(int ind);
@@ -61,22 +58,63 @@
 
 float servoSpeed = 1 * SERVO_SPEED_ADJ;
 
+Timer timer;
+
+int mDiscardedCount[3] = {0, 0, 0};
+const int MAX_DISCARDED = 3;
+
+PinName sensorPorts[3][2] = {
+    {p18, p17}, // sensor 0
+    {p15, p14}, // sensor 1
+    {p26, p25} // sensor 2
+};
+
+void initializeSensor(int ind) {
+    PS2MS_INIT s1(sensorPorts[ind][0], sensorPorts[ind][1]);
+    printf("SENSOR_INIT % DONE on ports %d and %d\n\r", ind, sensorPorts[ind][0], sensorPorts[ind][1]);
+    sensor[ind] = new PS2MS(sensorPorts[ind][0], sensorPorts[ind][1]);
+}
+
+bool notTooFarOff() {
+    int maxAbs1 = abs(sensorXs[0]) > abs(sensorYs[0]) ? abs(sensorXs[0]) : abs(sensorYs[0]);
+    int maxAbs2 = abs(sensorXs[1]) > abs(sensorYs[1]) ? abs(sensorXs[1]) : abs(sensorYs[1]);
+    int maxAbs3 = abs(sensorXs[2]) > abs(sensorYs[2]) ? abs(sensorXs[2]) : abs(sensorYs[1]);
+    
+    if (maxAbs1 == 0) maxAbs1 = 1;
+    if (maxAbs2 == 0) maxAbs2 = 1;
+    if (maxAbs3 == 0) maxAbs3 = 1;
+    
+    if (maxAbs1 > maxAbs2 * 20) {
+        return false;
+    }
+    
+    if (maxAbs2 > maxAbs3 * 20) {
+        return false;
+    }
+    
+    if (maxAbs3 > maxAbs1 * 20) {
+        return false;
+    }
+    
+    return true;
+}
+
+//TODO switch to Tickers instead of checking these in a while loop, much cleaner
+long lastTickMs;
+long lastSendMs;
+const int TICK_EVERY_MS = 10;
+const int SEND_EVERY_MS = 30;
+
 int main()
 {
     printf("MAIN START\n\r");
-    sensor_init[0] = new PS2MS_INIT(p18, p17);
-    printf("SENSOR_INIT 0 DONE\n\r");
-    sensor_init[1] = new PS2MS_INIT(p23, p22);
-    printf("SENSOR_INIT 1 DONE\n\r");
-    sensor_init[2] = new PS2MS_INIT(p26, p25);
-    printf("SENSOR_INIT 2 DONE\n\r");
-    printf("SENSOR_INIT DONE\n\r");
-    sensor[0] = new PS2MS(p18, p17);
-    sensor[1] = new PS2MS(p23, p22);
-    sensor[2] = new PS2MS(p26, p25);
-    printf("SENSOR DONE\n\r");
+    initializeSensor(0);
+    initializeSensor(1);
+    initializeSensor(2);
     //TODO: receive all pending packets here
 
+    timer.start();
+
     float range = 0.00085;
     float position1 = 0.5;
     float position2 = 0.5;
@@ -95,7 +133,13 @@
     int dir;
 
     bool awaitingPackets = false;
-
+    
+    lastTickMs = 0;
+    lastSendMs = 0;
+    
+    int mTooFarOffFor = 0;
+    const int MAX_TOO_FAR_OFF_COUNT = 2;
+    
     while(1) {
         if (pc.readable()) {
             processSerial();
@@ -107,7 +151,7 @@
             servoYaw = position1;
         } else {
             if (mCommandCompleted[SERVO_YAW] == false) {
-                printf("Command completed\n\r");
+                printf("Command completed %d\n\r", timer.read_ms());
                 mCommandCompleted[SERVO_YAW] = true;
             }
             position1 = servoPos[SERVO_YAW];
@@ -119,51 +163,56 @@
             servoPitch = position2;
         } else {
             if (mCommandCompleted[SERVO_PITCH] == false) {
-                printf("Command completed\n\r");
+                printf("Command completed %d\n\r", timer.read_ms());
                 mCommandCompleted[SERVO_PITCH] = true;
             }
             position2 = servoPos[SERVO_PITCH];
         }
-        int res;
-        if (!awaitingPackets) {
-            //TODO: check for errors on send
-            res = sendCommand(0, '\xEB');
-
-            if (res) {
-                if (DEBUG) {
-                    printf("%d: send error %d\n\r", 0, res);
-                }
+        
+        if (lastSendMs + SEND_EVERY_MS <= timer.read_ms()) {
+            //printf("SEND %d\n\r", timer.read_ms());
+            lastSendMs = timer.read_ms();
+            int res;
+            if (!awaitingPackets) {
+                //TODO: check for errors on send
                 res = sendCommand(0, '\xEB');
-                if (DEBUG) {
-                    printf("%d: two failed sends %d\n\r", 0, res);
+    
+                if (res) {
+                    if (DEBUG) {
+                        printf("%d: send error %d\n\r", 0, res);
+                    }
+                    res = sendCommand(0, '\xEB');
+                    if (DEBUG) {
+                        printf("%d: two failed sends %d\n\r", 0, res);
+                    }
                 }
-            }
-            expectingAck1 = true;
-            res = sendCommand(1, '\xEB');
-            if (res) {
-                if (DEBUG) {
-                    printf("%d: send error %d\n\r", 1, res);
-                }
+                expectingAck1 = true;
                 res = sendCommand(1, '\xEB');
-                if (DEBUG) {
-                    printf("%d: two failed sends %d\n\r", 1, res);
+                if (res) {
+                    if (DEBUG) {
+                        printf("%d: send error %d\n\r", 1, res);
+                    }
+                    res = sendCommand(1, '\xEB');
+                    if (DEBUG) {
+                        printf("%d: two failed sends %d\n\r", 1, res);
+                    }
                 }
-            }
-            expectingAck2 = true;
-            res = sendCommand(2, '\xEB');
-            if (res) {
-                if (DEBUG) {
-                    printf("%d: send error %d\n\r", 2, res);
-                }
+                expectingAck2 = true;
                 res = sendCommand(2, '\xEB');
                 if (res) {
                     if (DEBUG) {
-                        printf("%d: two failed sends %d\n\r", 2, res);
+                        printf("%d: send error %d\n\r", 2, res);
+                    }
+                    res = sendCommand(2, '\xEB');
+                    if (res) {
+                        if (DEBUG) {
+                            printf("%d: two failed sends %d\n\r", 2, res);
+                        }
                     }
                 }
+                expectingAck3 = true;
+                awaitingPackets = true;
             }
-            expectingAck3 = true;
-            awaitingPackets = true;
         }
 
         if (expectingAck1) {
@@ -177,14 +226,37 @@
         if (sensorToPrint[0] && sensorToPrint[1] && sensorToPrint[2]) {
             if ((sensorXs[0] | sensorYs[0] | sensorXs[1] | sensorYs[1] | sensorXs[2] | sensorYs[2]) ) {
                 // some of the velocities are not 0
-                printf("%d : %d %d %d %d %d %d\n\r", SENSORS_NUM, sensorXs[0], sensorYs[0], sensorXs[1], sensorYs[1],
+                if (notTooFarOff()) {
+                    printf("%d : %d %d %d %d %d %d\n\r", SENSORS_NUM, sensorXs[0], sensorYs[0], sensorXs[1], sensorYs[1],
                        sensorXs[2], sensorYs[2]);
+                }
+                else {
+                    mTooFarOffFor++;
+                    if (mTooFarOffFor < MAX_TOO_FAR_OFF_COUNT) {
+                        printf("Too far off %d : %d %d %d %d %d %d\n\r", SENSORS_NUM, sensorXs[0], sensorYs[0], sensorXs[1], sensorYs[1],
+                           sensorXs[2], sensorYs[2]);
+                        sendResend(0); expectingAck1 = true;
+                        sendResend(1); expectingAck2 = true;
+                        sendResend(2); expectingAck3 = true;
+                        continue;
+                    }
+                    else {
+                        printf("Ignoring a TooFarOff packet %d : %d %d %d %d %d %d\n\r", SENSORS_NUM, sensorXs[0], sensorYs[0], sensorXs[1], sensorYs[1],
+                           sensorXs[2], sensorYs[2]);
+                    }
+                }
             }
+            mTooFarOffFor = 0;
             sensorToPrint[0] = sensorToPrint[1] = sensorToPrint[2] = false;
             sensorXs[0] = sensorYs[0] = sensorXs[1] = sensorYs[1] = sensorXs[2] = sensorYs[2] = 0;
             awaitingPackets = false;
         }
-    }\
+        while (lastTickMs + TICK_EVERY_MS > timer.read_ms()) {
+            wait_ms(1);
+        }
+        lastTickMs = timer.read_ms();
+        //printf("TICK %d\n\r", timer.read_ms());
+    }
     /*
     TODO: Deallocate those somewhere
 
@@ -198,7 +270,6 @@
 }
 
 #define MAX_ANGLE_STR_SIZE 100
-char rotateAngleStr[MAX_ANGLE_STR_SIZE];
 
 void processSerial()
 {
@@ -206,16 +277,17 @@
     if (c <= '9' && c >= '0') {
         int servoNum = int(c - '0');
         if (servoNum == SERVO_YAW || servoNum == SERVO_PITCH) {
+             char rotateAngleStr[MAX_ANGLE_STR_SIZE];
              pc.gets(rotateAngleStr, MAX_ANGLE_STR_SIZE);
              
              double rotateAngleNum = atoi(rotateAngleStr);
-             
              if (rotateAngleNum > 90 || rotateAngleNum < -90) {
                  return;
              }
              rotateAngleNum = 0.5 + rotateAngleNum / 180.;
              servoPos[servoNum] = rotateAngleNum + servoAdj[servoNum];
              mCommandCompleted[servoNum] = false;
+             printf("Command started %d\n\r", timer.read_ms());
         }
     }
     else if (c == 's') {
@@ -257,12 +329,16 @@
 
     if (!successful) {
         printf("DISCARDING PACKET %d\n\r", ind);
-        if (DEBUG) {
-            sendCommand(ind, '\xEB');
-            //wait(1);
+        mDiscardedCount[ind]++;
+        if (mDiscardedCount[ind] > MAX_DISCARDED) {
+            initializeSensor(ind);
         }
+        //if (DEBUG) {
+        sendCommand(ind, '\xEB');
+        //}
         return false;
     }
+    mDiscardedCount[ind] = 0;
     return true;
 }
 
@@ -289,7 +365,7 @@
             bytes[0] = c;
             if (!((c << 5) & 0x100)) {
                 // not byte[0] wrong offset, skip e
-                if (DEBUG) printf("%d: w %d ", ind, c);
+                printf("%d: w %d ", ind, c);
                 c = sensor[ind]->getc();
                 while (c >= 0) {
                     printf("%d ", c);