Diff: main.cpp

Revision:

8:41b35bda9d48

Parent:

7:04ddad10a741

Child:

9:2c85d7f99a14

--- a/main.cpp	Wed Mar 05 18:53:20 2014 +0000
+++ b/main.cpp	Thu Mar 06 10:38:43 2014 +0000
@@ -8,6 +8,7 @@
 #define ENABLE_3 true
 
 #define SENSORS_NUM 3
+#define BYTES_NUM 3
 
 Servo servo1(p21);
 Servo servo2(p24);
@@ -33,7 +34,7 @@
 PS2MS_INIT sensor3_init(p26, p25);
 PS2MS sensor3(p26, p25);
 
-int process_sensor_input(char c, int bytenum, char* bytes, int ind);
+int process_sensor_input(int c, int bytenum, char* bytes, int ind);
 
 int sensorXs[SENSORS_NUM];
 int sensorYs[SENSORS_NUM];
@@ -57,15 +58,15 @@
 
     printf("IMHERE START\n");
     int s1bytenum = 0;
-    char s1bytes[3];
+    char s1bytes[BYTES_NUM];
 
     int s2bytenum = 0;
-    char s2bytes[3];
+    char s2bytes[BYTES_NUM];
     
     int s3bytenum = 0;
-    char s3bytes[3];
+    char s3bytes[BYTES_NUM];
 
-    char s1c, s2c, s3c;
+    int s1c, s2c, s3c;
     printf("IMHERE GET SENSORS\n");
     s1c = sensor1.getc();
     printf("IMHERE GOT S1\n");
@@ -82,7 +83,7 @@
             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 '3': newPos1 = 0.6 + 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;
@@ -139,28 +140,31 @@
     }
 }
 
-int process_sensor_input(char c, int bytenum, char* bytes, int ind)
+int process_sensor_input(int c, int bytenum, char* bytes, int ind)
 {
-    if (c == 255) {
+    if (c < 0) {
+        //printf("%d: 255\n\r", ind);
         bytenum = -1;
-    } else if (bytenum % 3 == 0) {
+    } else if (bytenum % BYTES_NUM == 0) {
         bytes[0] = c;
-        if (!((1 << 3) & c)) {
+        if (!((c << 5) & 0x100)) {
             // not byte[0] wrong offset, skip c
+            //printf("%d: w %d\n\r", ind, c);
             bytenum = -1;
         }
-    } else if (bytenum % 3 == 1) {
+    } else if (bytenum % BYTES_NUM == 1) {
         bytes[1] = c;
-    } else if (bytenum % 3 == 2) {
+    } else if (bytenum % BYTES_NUM == 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("Overflow x!\n\r");
+            printf("Overflow x! %d\n\r", ind);
             bytenum = -1;
         }
         else if ((1 << 7) & bytes[0]) {
-            //printf("Overflow y!\n\r");
+            printf("Overflow y! %d\n\r", ind);
             //printf("Byte1 is %d\n\r", bytes[0]);
             bytenum = -1;   
         }
@@ -172,9 +176,9 @@
             sensorXs[ind] = x;
             sensorYs[ind] = y;
             sensorToPrint[ind] = true;
-            //printf("To print %d\n\c", ind);
+            //printf("%d        ", ind);
             bytenum = -1;
         }
     }
-    return (bytenum + 1) % 3;
+    return (bytenum + 1) % BYTES_NUM;
 }