Diff: main.cpp

Revision:

10:37e7c46837dc

Parent:

9:2c85d7f99a14

Child:

11:f5dcf8811a4e

diff -r 2c85d7f99a14 -r 37e7c46837dc main.cpp
--- a/main.cpp	Thu Mar 06 18:09:37 2014 +0000
+++ b/main.cpp	Mon Mar 10 20:43:52 2014 +0000
@@ -3,10 +3,6 @@
 #include "mbed.h"
 #include "Servo.h"
 
-#define ENABLE_1 true
-#define ENABLE_2 true
-#define ENABLE_3 true
-
 #define SENSORS_NUM 3
 #define BYTES_NUM 3
 
@@ -16,8 +12,8 @@
 
 #define MAX_REPLY_ERRORS 3
 
-Servo servo1(p21);
-Servo servo2(p24);
+Servo servoYaw(p21);
+Servo servoPitch(p24);
 
 DigitalOut myled(LED1);
 Serial pc(USBTX, USBRX); // tx, rx
@@ -31,194 +27,331 @@
  */
 
 //TODO should Iuse sensor1_init? maybe no 255s?
-PS2MS_INIT sensor1_init(p18, p17);
-PS2MS sensor1(p18, p17);
+PS2MS* sensor[3];
+PS2MS_INIT* sensor_init[3];
 
-PS2MS_INIT sensor2_init(p23, p22);
-PS2MS sensor2(p23, p22);
-
-PS2MS_INIT sensor3_init(p26, p25);
-PS2MS sensor3(p26, p25);
 
 int process_sensor_input(int c, int bytenum, char* bytes, int ind);
-bool processACKReply(int reply, int ind);
+bool processACKReply(int ind);
 void sendError(int ind);
+bool getPacket(int ind);
+bool getMovementPacket(int ind);
+void sendResend(int ind);
+void sendError(int ind);
+int sendCommand(int ind, char command);
+void processSerial();
 
 int sensorXs[SENSORS_NUM];
 int sensorYs[SENSORS_NUM];
 bool sensorToPrint[SENSORS_NUM];
 
+char last_command[3];
+
 bool expectingAck1 = false, expectingAck2 = false, expectingAck3 = false;
-int consecutiveOverflows[3];
 
 int replyErrors[3];
 
+static const int SERVO_YAW = 0;
+static const int SERVO_PITCH = 1;
+float servoPos[2];
+float servoAdj[2] = {0.03, 0.03};
+bool mCommandCompleted[2] = {true, true};
+
 int main()
 {
+    sensor_init[0] = new PS2MS_INIT(p18, p17);
+    sensor_init[1] = new PS2MS_INIT(p23, p22);
+    sensor_init[2] = new PS2MS_INIT(p26, p25);
+    sensor[0] = new PS2MS(p18, p17);
+    sensor[1] = new PS2MS(p23, p22);
+    sensor[2] = new PS2MS(p26, p25);
+
+    //TODO: receive all pending packets here
 
     float range = 0.00085;
     float position1 = 0.5;
     float position2 = 0.5;
-    float position_adj = 0.03;
-    
-    float newPos1 = position1, newPos2 = position2;
-    
-    consecutiveOverflows[0] = consecutiveOverflows[1] = consecutiveOverflows[2] = 0;
+
+    servoPos[SERVO_YAW] = position1 + servoAdj[SERVO_YAW];
+    servoPos[SERVO_PITCH] = position2 + servoAdj[SERVO_PITCH];
+
     replyErrors[0] = replyErrors[1] = replyErrors[2] = 0;
-    
-    servo1.calibrate(range, 45.0); 
-    servo2.calibrate(range, 45.0); 
-    servo1 = position1 + position_adj;
-    servo2 = position2 + position_adj;
-    printf("position = %.3f, range = +/-%0.5f\n", position1, range, position_adj);
+
+    servoYaw.calibrate(range, 45.0);
+    servoPitch.calibrate(range, 45.0);
+    servoYaw = servoPos[SERVO_YAW];
+    servoPitch = servoPos[SERVO_PITCH];
+    printf("position = %.3f, range = +/-%0.5f\n", position1, range);
 
     printf("IMHERE START\n");
-    int s1bytenum = 0;
-    char s1bytes[BYTES_NUM];
 
-    int s2bytenum = 0;
-    char s2bytes[BYTES_NUM];
-    
-    int s3bytenum = 0;
-    char s3bytes[BYTES_NUM];
-
-    int s1c, s2c, s3c;
-    printf("IMHERE GET SENSORS\n");
-    s1c = sensor1.getc();
-    printf("IMHERE GOT S1\n");
-    s2c = sensor2.getc();
-    printf("IMHERE GOT S2\n");
-    s3c = sensor2.getc();
-    printf("IMHERE GOT S3\n");
     sensorToPrint[0] = sensorToPrint[1] = sensorToPrint[2] = false;
     int dir;
     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 = 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;
+            processSerial();
+        }
+
+        if (abs(position1 - servoPos[SERVO_YAW]) > servoSpeed) {
+            dir = position1 < servoPos[SERVO_YAW] ? 1 : -1;
+            position1 += servoSpeed * dir;
+            servoYaw = position1;
+        } else {
+            if (mCommandCompleted[SERVO_YAW] == false) {
+                printf("Command completed\n\r");
+                mCommandCompleted[SERVO_YAW] = true;
             }
-            //printf("position = %.3f, range = +/-%0.5f\n", position, range, position_adj);
-            //dir = position1 < newPos1 ? 1 : -1;
-            /*printf("IMHERE %f %f %d\n", position1, newPos1, dir);
-            for (float i = position1; abs(i - newPos1) > servoSpeed; i += servoSpeed * dir) {
-                servo1 = i;
-                printf("%f %f\n", i, newPos1);
+            position1 = servoPos[SERVO_YAW];
+        }
+
+        if (abs(position2 - servoPos[SERVO_PITCH]) > servoSpeed) {
+            dir = position2 < servoPos[SERVO_PITCH] ? 1 : -1;
+            position2 += servoSpeed * dir;
+            servoPitch = position2;
+        } else {
+            if (mCommandCompleted[SERVO_PITCH] == false) {
+                printf("Command completed\n\r");
+                mCommandCompleted[SERVO_PITCH] = true;
             }
-            position1 = newPos1;*/
-            //servo1 = position1;
-            //servo2 = position2;
+            position2 = servoPos[SERVO_PITCH];
         }
-        
-        if (abs(position1 - newPos1) > servoSpeed) {
-            dir = position1 < newPos1 ? 1 : -1;
-            position1 += servoSpeed * dir;
-            servo1 = position1;
-        }
-        else {
-            position1 = newPos1;
-        }
-        
-        if (abs(position2 - newPos2) > servoSpeed) {
-            dir = position2 < newPos2 ? 1 : -1;
-            position2 += servoSpeed * dir;
-            servo2 = position2;
-        }
-        else {
-            position2 = newPos2;
-        }
-        
+        int res;
         if (!awaitingPackets) {
-            sensor1_init.send('\xEB');
+            //TODO: check for errors on send
+            res = sendCommand(0, '\xEB');
+
+            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;
-            sensor2_init.send('\xEB');
+            res = sendCommand(1, '\xEB');
+            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;
-            sensor3_init.send('\xEB');
+            res = sendCommand(2, '\xEB');
+            if (res) {
+                if (DEBUG) {
+                    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;
         }
-        
-        if (ENABLE_1) {
-            
-            s1c = sensor1.getc();
-            if (expectingAck1) {
-                expectingAck1 = processACKReply(s1c, 1);
-            }
-            else {
-                s1bytenum = process_sensor_input(s1c, s1bytenum, s1bytes, 0);
-            }
-        }
-        if (ENABLE_2) {
-            s2c = sensor2.getc();
-            if (expectingAck2) {
-                expectingAck2 = processACKReply(s2c, 2);
-            }
-            else {
-                s2bytenum = process_sensor_input(s2c, s2bytenum, s2bytes, 1);
-            }
-        }
-        if (ENABLE_3) {
-            
-            s3c = sensor3.getc();
-            if (expectingAck3) {
-                expectingAck3 = processACKReply(s3c, 3);
-            }
-            else {
-                s3bytenum = process_sensor_input(s3c, s3bytenum, s3bytes, 2);
-            }
+
+        if (expectingAck1) {
+            expectingAck1 = !getPacket(0);
+        } else if (expectingAck2) {
+            expectingAck2 = !getPacket(1);
+        } else if (expectingAck3) {
+            expectingAck3 = !getPacket(2);
         }
         // TODO only prints when both are enabled now
         if (sensorToPrint[0] && sensorToPrint[1] && sensorToPrint[2]) {
-            printf("%d : %d %d %d %d %d %d\n\r", SENSORS_NUM, sensorXs[0], sensorYs[0], sensorXs[1], sensorYs[1],
-                sensorXs[2], sensorYs[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],
+                       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;
         }
+    }\
+    /*
+    TODO: Deallocate those somewhere
+
+    delete[] sensor_init[0];
+    delete[] sensor_init[1];
+    delete[] sensor_init[2];
+    delete[] sensor[0];
+    delete[] sensor[1];
+    delete[] sensor[2];
+    */
+}
+
+#define MAX_ANGLE_STR_SIZE 100
+char rotateAngleStr[MAX_ANGLE_STR_SIZE];
+
+void processSerial()
+{
+    char c = pc.getc();
+    if (c <= '9' && c >= '0') {
+        int servoNum = int(c - '0');
+        if (servoNum == SERVO_YAW || servoNum == SERVO_PITCH) {
+             pc.gets(rotateAngleStr, MAX_ANGLE_STR_SIZE);
+             
+             double rotateAngleNum = atoi(rotateAngleStr);
+             
+             if (rotateAngleNum > 90 || rotateAngleNum < -90) {
+                 return;
+             }
+             rotateAngleNum = 0.5 + rotateAngleNum / 90.;
+             servoPos[servoNum] = rotateAngleNum + servoAdj[servoNum];
+             mCommandCompleted[servoNum] = 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;
+int sendCommand(int ind, char command)
+{
+    int res;
+    last_command[ind] = command;
+    //res = sensor_init[ind]->send(command);
+    res = sensor[ind]->sendCommand(command);
+    //__enable_irq();
+    if (res) {
+        if (DEBUG) printf("error sending command %#x to %d\n\r", command, ind);
+    }
+    return res;
+}
+
+bool getPacket(int ind)
+{
+    bool successful = processACKReply(ind);
+    if (!successful) {
+        sendResend(ind);
+        successful = processACKReply(ind);
+        if (!successful) {
+            printf("DUNNO WHAT TO DO ACK Reply %d\n\r", ind);
+            //wait(1);
+            sendCommand(ind, '\xEB');
+            return getPacket(ind);
         }
     }
-    else if (reply == '\xFA') {
-        if (DEBUG) printf("%d: ACK ", ind);
+    successful = getMovementPacket(ind);
+
+    if (!successful) {
+        printf("DISCARDING PACKET %d\n\r", ind);
+        if (DEBUG) {
+            sendCommand(ind, '\xEB');
+            //wait(1);
+        }
         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;
 }
 
+void sendError(int ind)
+{
+    sendCommand(ind, '\xFC');
+}
+
+void sendResend(int ind)
+{
+    sendCommand(ind, '\xFE');
+}
+
+bool getMovementPacket(int ind)
+{
+    char bytes[3];
+    int c;
+    for (int i = 0; i < 3; ++i) {
+        c = sensor[ind]->getc();
+        if (c < 0) {
+            //printf("%d: 255\n\r", ind);
+            return false;
+        } else if (i == 0) {
+            bytes[0] = c;
+            if (!((c << 5) & 0x100)) {
+                // not byte[0] wrong offset, skip e
+                if (DEBUG) printf("%d: w %d ", ind, c);
+                c = sensor[ind]->getc();
+                while (c >= 0) {
+                    printf("%d ", c);
+                    c = sensor[ind]->getc();
+                }
+                printf("\n\r");
+                return false;
+            }
+        } else if (i == 1) {
+            bytes[1] = c;
+        } else if (i == 2) {
+            bytes[2] = c;
+            //printf("%d - %d %d %d\n\r", ind, bytes[0], bytes[1], bytes[2]);
+
+            //TODO: check for overflow
+            if ((1 << 6) & bytes[0]) {
+                //printf("%d: Overflow x %d %d %d!\n\r", ind, bytes[0], bytes[1], bytes[2]);
+                return false;
+            } else if ((1 << 7) & bytes[0]) {
+                printf("%d: Overflow y %d %d %d!\n\r", ind, bytes[0], bytes[1], bytes[2]);
+                return false;
+            }
+            // check x and y signs
+            else {
+                int x = bytes[1] - ((bytes[0] << 4) & 0x100);
+                int y = bytes[2] - ((bytes[0] << 3) & 0x100);
+                //printf("%s: x = %d   y = %d\n\r", id, x, y);
+                sensorXs[ind] = x;
+                sensorYs[ind] = y;
+                sensorToPrint[ind] = true;
+                //printf("%d        ", ind);
+            }
+        }
+    }
+    return true;
+}
+
+bool processACKReply(int ind)
+{
+    int reply = sensor[ind]->getc();
+    if (reply < 0) {
+        if (DEBUG) printf("%d: Error %d", ind, reply);
+        return false;
+    } else if (reply == '\xFA') {
+        //if (DEBUG) printf("%d: ACK ", ind);
+        return true;
+    } else if (reply == '\xEB') {
+        if (DEBUG) printf("%d: READ_DATA ", ind);
+        //TODO: inf loop possible here cause recursion
+        return processACKReply(ind);
+    } else if (reply == '\xFE') {
+        if (last_command[ind] == '\xFE') {
+            if (DEBUG) printf("%d: REPEATING_COMMAND BUT REPEAT_COMMAND!", ind, last_command[ind]);
+            //wait(1);
+            sendCommand(ind, '\xEB');
+            return processACKReply(ind);
+        }
+        // repeat command
+        if (DEBUG) printf("%d: REPEATING_COMMAND %#x", ind, last_command[ind]);
+        //wait(1);
+        sendCommand(ind, last_command[ind]);
+        return processACKReply(ind);
+    } else if (reply == '\xF2') {
+        // get device ID??
+        if (DEBUG) printf("%d: GET_DEVICE_ID %#x", ind, reply);
+        //wait(1);
+        return processACKReply(ind);
+    } else {
+        if (DEBUG) printf("%d: Unexpected ACK Reply %d\n\r", ind, reply);
+        //wait(1);
+        return processACKReply(ind);
+    }
+}
+/*
 int process_sensor_input(int c, int bytenum, char* bytes, int ind)
 {
     if (c < 0) {
@@ -243,18 +376,15 @@
             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 {
+            } else {
                 consecutiveOverflows[ind] = 0;
             }
             bytenum = -1;
-        }
-        else if ((1 << 7) & bytes[0]) {
+        } else if ((1 << 7) & bytes[0]) {
             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 {
+            } else {
                 consecutiveOverflows[ind] = 0;
             }
             bytenum = -1;
@@ -273,7 +403,8 @@
     }
     return (bytenum + 1) % BYTES_NUM;
 }
-
+*/
+/*
 void sendError(int ind)
 {
     switch (ind) {
@@ -290,4 +421,4 @@
             expectingAck3 = true;
             break;
     }
-}
\ No newline at end of file
+}*/
\ No newline at end of file