Diff: main.cpp

Revision:

6:83c4801a027d

Parent:

5:43d5529fbe1e

Child:

7:04ddad10a741

diff -r 43d5529fbe1e -r 83c4801a027d main.cpp
--- a/main.cpp	Tue Mar 04 12:06:27 2014 +0000
+++ b/main.cpp	Tue Mar 04 13:28:04 2014 +0000
@@ -5,12 +5,14 @@
 
 #define ENABLE_1 true
 #define ENABLE_2 true
+#define ENABLE_3 true
 
-Servo myservo(p21);
+#define SENSORS_NUM 3
+
+Servo servo1(p21);
+Servo servo2(p24);
 
 DigitalOut myled(LED1);
-DigitalInOut clk(p23);
-DigitalInOut dat(p22);
 Serial pc(USBTX, USBRX); // tx, rx
 /*
  * 0xFF: Reset command.
@@ -21,32 +23,35 @@
  * 0xF4: Enable device.
  */
 
-int send(uint8_t c);
-int recv();
+//TODO should Iuse sensor1_init? maybe no 255s?
+PS2MS_INIT sensor1_init(p18, p17);
+PS2MS sensor1(p18, p17);
 
-//TODO should Iuse sensor1_init? maybe no 255s?
-PS2MS_INIT sensor1_init(p23, p22);
-PS2MS sensor1(p23, p22);
+PS2MS_INIT sensor2_init(p23, p22);
+PS2MS sensor2(p23, p22);
 
-PS2MS_INIT sensor2_init(p26, p25);
-PS2MS sensor2(p26, p25);
+PS2MS_INIT sensor3_init(p26, p25);
+PS2MS sensor3(p26, p25);
 
 int process_sensor_input(char c, int bytenum, char* bytes, int ind);
 
-int sensorXs[2];
-int sensorYs[2];
-bool sensorToPrint[2];
+int sensorXs[SENSORS_NUM];
+int sensorYs[SENSORS_NUM];
+bool sensorToPrint[SENSORS_NUM];
 
 int main()
 {
 
     float range = 0.00085;
-    float position = 0.5;
+    float position1 = 0.5;
+    float position2 = 0.5;
     float position_adj = 0.03;
     
-    myservo.calibrate(range, 45.0); 
-    myservo = position;
-    printf("position = %.3f, range = +/-%0.5f\n", position, range, position_adj);
+    servo1.calibrate(range, 45.0); 
+    servo2.calibrate(range, 45.0); 
+    servo1 = position1;
+    servo2 = position2;
+    printf("position = %.3f, range = +/-%0.5f\n", position1, range, position_adj);
 
     printf("IMHERE START\n");
     int s1bytenum = 0;
@@ -54,24 +59,33 @@
 
     int s2bytenum = 0;
     char s2bytes[3];
+    
+    int s3bytenum = 0;
+    char s3bytes[3];
 
-    char s1c, s2c;
+    char s1c, s2c, s3c;
     printf("IMHERE GET SENSORS\n");
     s1c = sensor1.getc();
     printf("IMHERE GOT S1\n");
     s2c = sensor2.getc();
     printf("IMHERE GOT S2\n");
-    sensorToPrint[0] = sensorToPrint[1] = false;
+    s3c = sensor2.getc();
+    printf("IMHERE GOT S3\n");
+    sensorToPrint[0] = sensorToPrint[1] = sensorToPrint[2] = false;
     
     while(1) {
         if (pc.readable()) {
             switch(pc.getc()) {
-            case '1': position = 0.0 + position_adj; break;
-            case '2': position = 0.5 + position_adj; break;
-            case '3': position = 1.0 + position_adj; break;
+            case '1': position1 = 0.0 + position_adj; break;
+            case '2': position1 = 0.5 + position_adj; break;
+            case '3': position1 = 1.0 + position_adj; break;
+            case '4': position2 = 0.3 + position_adj; break;
+            case '5': position2 = 0.5 + position_adj; break;
+            case '6': position2 = 0.7 + position_adj; break;
             }
             //printf("position = %.3f, range = +/-%0.5f\n", position, range, position_adj);
-            myservo = position;
+            servo1 = position1;
+            servo2 = position2;
         }
     
         if (ENABLE_1) {
@@ -82,11 +96,16 @@
             s2bytenum = process_sensor_input(s2c, s2bytenum, s2bytes, 1);
             s2c = sensor2.getc();
         }
+        if (ENABLE_3) {
+            s3bytenum = process_sensor_input(s3c, s3bytenum, s3bytes, 2);
+            s3c = sensor3.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;
+        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]);
+            sensorToPrint[0] = sensorToPrint[1] = sensorToPrint[2] = false;
+            sensorXs[0] = sensorYs[0] = sensorXs[1] = sensorYs[1] = sensorXs[2] = sensorYs[2] = 0;
         }
     }
 }