Diff: main.cpp

Revision:

7:04ddad10a741

Parent:

6:83c4801a027d

Child:

8:41b35bda9d48

--- a/main.cpp	Tue Mar 04 13:28:04 2014 +0000
+++ b/main.cpp	Wed Mar 05 18:53:20 2014 +0000
@@ -47,6 +47,8 @@
     float position2 = 0.5;
     float position_adj = 0.03;
     
+    float newPos1 = position1, newPos2 = position2;
+    
     servo1.calibrate(range, 45.0); 
     servo2.calibrate(range, 45.0); 
     servo1 = position1;
@@ -72,21 +74,48 @@
     s3c = sensor2.getc();
     printf("IMHERE GOT S3\n");
     sensorToPrint[0] = sensorToPrint[1] = sensorToPrint[2] = false;
+    int dir;
+    float servoSpeed = 0.001;
     
     while(1) {
         if (pc.readable()) {
             switch(pc.getc()) {
-            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;
+            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;
             }
             //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 = newPos1;*/
+            //servo1 = position1;
+            //servo2 = position2;
+        }
+        
+        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;
+        }
     
         if (ENABLE_1) {
             s1bytenum = process_sensor_input(s1c, s1bytenum, s1bytes, 0);
@@ -143,6 +172,7 @@
             sensorXs[ind] = x;
             sensorYs[ind] = y;
             sensorToPrint[ind] = true;
+            //printf("To print %d\n\c", ind);
             bytenum = -1;
         }
     }