Diff: main.cpp

Revision:

2:e35627187804

Parent:

1:d290d6a34bef

Child:

3:eb3c3c9587d7

diff -r d290d6a34bef -r e35627187804 main.cpp
--- a/main.cpp	Sun Oct 20 11:35:09 2013 +0000
+++ b/main.cpp	Mon Oct 28 10:28:21 2013 +0000
@@ -2,262 +2,101 @@
 #include "PS2MS_INIT.h"
 #include "mbed.h"
 
+#define ENABLE_1 true
+#define ENABLE_2 true
+
 DigitalOut myled(LED1);
 DigitalInOut clk(p23);
 DigitalInOut dat(p22);
 Serial pc(USBTX, USBRX); // tx, rx
-static const int MAX_RETRY = 1000000;
-    /*
-     * 0xFF: Reset command.
-     * 0xF3: Set sample rate.
-     * 0xF2: Read device type.
-     * 0xE8: Set resolution.
-     * 0xE6: Set scaling.
-     * 0xF4: Enable device.
-     */
+/*
+ * 0xFF: Reset command.
+ * 0xF3: Set sample rate.
+ * 0xF2: Read device type.
+ * 0xE8: Set resolution.
+ * 0xE6: Set scaling.
+ * 0xF4: Enable device.
+ */
 
 int send(uint8_t c);
 int recv();
 
-PS2MS_INIT ps2ms_init(p23, p22);
-PS2MS ps2ms(p23, p22);
+//TODO should Iuse sensor1_init? maybe no 255s?
+PS2MS_INIT sensor1_init(p23, p22);
+PS2MS sensor1(p23, p22);
+
+PS2MS_INIT sensor2_init(p26, p25);
+PS2MS sensor2(p26, p25);
+
+int process_sensor_input(char c, int bytenum, char* bytes, int ind);
+
+int sensorXs[2];
+int sensorYs[2];
+bool sensorToPrint[2];
 
-int main() {
-    int bytenum = 0;
-    char byte1, byte2, byte3;
-    char c;
+int main()
+{
+    int s1bytenum = 0;
+    char s1bytes[3];
+
+    int s2bytenum = 0;
+    char s2bytes[3];
+
+    char s1c, s2c;
+    s1c = sensor1.getc();
+    s2c = sensor2.getc();
+    
+    sensorToPrint[0] = sensorToPrint[1] = false;
+    
     while(1) {
-        //char c = pc.getc();
-        //send(c);
-        c = ps2ms.getc();
-        //TODO: c overflows, c >= 0 is always true, check c < 25? !
-        while(c >= 0) {
-            if (c == 255) {
-                bytenum = 0;
-                while (ps2ms.getc() >= 0) {};
-                break;
-            }
-            if (bytenum % 3 == 0) {
-                byte1 = c;
-                // not byte1, wrong offset, clear remaining buffer
-                if (!((1 << 3) & byte1)) {
-                    //while (ps2ms.getc() >= 0 && c != 255) {};
-                    ps2ms.init_work();
-                    bytenum = 0;
-                    c = ps2ms.getc();
-                    continue;
-                }
-            }
-            else if (bytenum % 3 == 1) {
-                byte2 = c;
-            }
-            else if (bytenum % 3 == 2) {
-                byte3 = c;
-                
-                //TODO: check for overflow
-                if ((1 << 6) & byte1) {
-                    printf("Overflow x!\n\r");
-                }
-                if ((1 << 7) & byte1) {
-                    printf("Overflow y!\n\r");
-                    printf("Byte1 is %d\n\r", byte1);
-                }
-                // check x and y signs
-                int x = ((1 << 4) & byte1) ? int(byte2) - 255 : int(byte2);
-                int y = ((1 << 5) & byte1) ? int(byte3) - 255 : int(byte3);
-                x = byte2 - ((byte1 << 4) & 0x100);
-                y = byte3 - ((byte1 << 3) & 0x100);
-                printf("x=%d   y=%d\n\r", x, y);
-                bytenum = 0;
-                break;
-            }
-            bytenum = (bytenum + 1) % 3;
-            c = ps2ms.getc();
+        if (ENABLE_1) {
+            s1bytenum = process_sensor_input(s1c, s1bytenum, s1bytes, 0);
+            s1c = sensor1.getc();
+        }
+        if (ENABLE_2) {
+            s2bytenum = process_sensor_input(s2c, s2bytenum, s2bytes, 1);
+            s2c = sensor2.getc();
+        }
+        // TODO only prints when both are enabled now
+        if (sensorToPrint[0] || sensorToPrint[1]) {
+            printf("%d : %d %d %d %d\n\r", 2, sensorXs[0], sensorYs[0], sensorXs[1], sensorYs[1]);
+            sensorToPrint[0] = sensorToPrint[1] = false;
+            sensorXs[0] = sensorYs[0] = sensorXs[1] = sensorYs[1] = 0;
         }
     }
 }
-/**
- * Wait a clock down edge.
- *
- * @return true if wait done.
- */
-bool waitClockDownEdge(void) {
-    int cnt;
-    /*
-     * Wait until clock is low.
-     */
-    cnt = 0;
-    while (clk.read() == 0) {
-        cnt++;
-        if (MAX_RETRY < cnt) {
-            return false;
-        }
-        wait_us(1);
-    }
-    /*
-     * Wait until clock is high.
-     */
-    cnt = 0;
-    while (clk.read() == 1) {
-        cnt++;
-        if (MAX_RETRY < cnt) {
-            return false;
-        }
-        wait_us(1);
-    }
-    return true;
-}
 
-/**
- * Wait a clock up level.
- *
- * @return true if wait done.
- */
-bool waitClockUpLevel(void) {
-    int cnt;
-    /*
-     * Wait until clock is low.
-     */
-    cnt = 0;
-    while (clk.read() == 0) {
-        cnt++;
-        if (MAX_RETRY < cnt) {
-            return false;
-        }
-        wait_us(1);
-    }
-    return true;
-}
-
-/**
- * Send a byte data.
- *
- * @param c a character.
- *
- * @return Negative value is a error number.
- */
-int send(uint8_t c) {
-    clk.output();
-    dat.output();
-
-    clk.write(0);
-    wait_us(200);
-
-    dat.write(0);
-    wait_us(10);
-    clk.write(1);
-    wait_us(10);
-
-    clk.input();
-    int parcnt = 0;
-    for (int i = 0; i < 10; i++) {
-        if (!waitClockDownEdge()) {
-            return -1;
-        }
-        if ((0 <= i) && (i <= 7)) {
-            /*
-             * Data bit.
-             */
-            if ((c & (1 << i)) == 0) {
-                dat.write(0);
-            } else {
-                dat.write(1);
-                parcnt++;
-            }
+int process_sensor_input(char c, int bytenum, char* bytes, int ind)
+{
+    if (c == 255) {
+        bytenum = -1;
+    } else if (bytenum % 3 == 0) {
+        bytes[0] = c;
+        if (!((1 << 3) & c)) {
+            // not byte[0] wrong offset, skip c
+            bytenum = -1;
         }
-        if (i == 8) {
-            /*
-             * Parity bit.
-             */
-            if ((parcnt % 2) == 0) {
-                dat.write(1);
-            } else {
-                dat.write(0);
-            }
-        }
-        if (i == 9) {
-            /*
-             * Stop bit.
-             */
-            dat.write(1);
-        }
-    }
-    dat.input();
-
-    /*
-     * Check a ACK.
-     */
-    if (!waitClockDownEdge()) {
-        return -2;
-    }
-    if (dat.read() != 0) {
-        return -3;
-    }
-
-    if (!waitClockUpLevel()) {
-        return -4;
-    }
-
-    return 0;
-}
+    } else if (bytenum % 3 == 1) {
+        bytes[1] = c;
+    } else if (bytenum % 3 == 2) {
+        bytes[2] = c;
 
-/**
- * Receive a byte data.
- *
- * @return return a data. Negative value is a error number.
- */
-int recv(void) {
-    uint8_t c = 0;
-    clk.input();
-    dat.input();
-    int parcnt = 0;
-    for (int i = 0; i < 11; i++) {
-        if (!waitClockDownEdge()) {
-            return -1;
+        //TODO: check for overflow
+        if ((1 << 6) & bytes[0]) {
+            printf("Overflow x!\n\r");
         }
-        if (i == 0) {
-            /*
-             * Start bit.
-             */
-            if (dat.read() != 0) {
-                return -2;
-            }
+        if ((1 << 7) & bytes[0]) {
+            printf("Overflow y!\n\r");
+            printf("Byte1 is %d\n\r", bytes[0]);
         }
-        if ((1 <= i) && (i <= 8)) {
-            /*
-             * Data bit.
-             */
-            if (dat.read() == 0) {
-                c &= ~(1 << (i - 1));
-            } else {
-                c |= (1 << (i - 1));
-                parcnt++;
-            }
-        }
-        if (i == 9) {
-            /*
-             * Parity bit.
-             */
-            if (dat.read() == 0) {
-                if ((parcnt % 2) != 1) {
-                    return -3;
-                }
-            } else {
-                if ((parcnt % 2) != 0) {
-                    return -4;
-                }
-            }
-        }
-        if (i == 10) {
-            /*
-             * Stop bit.
-             */
-            if (dat.read() != 1) {
-                return -5;
-            }
-        }
+        // check x and y signs
+        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;
+        bytenum = -1;
     }
-    return (int)c;
+    return (bytenum + 1) % 3;
 }
-
-