Revision 2:b1e22f332443, committed 2015-03-09

Comitter:

Qoramas

Date:

Mon Mar 09 13:37:45 2015 +0000

Parent:

1:a22303e03da3

Commit message:

Add Sonar back

Changed in this revision

--- a/main.cpp	Mon Mar 02 14:46:00 2015 +0000
+++ b/main.cpp	Mon Mar 09 13:37:45 2015 +0000
@@ -13,6 +13,7 @@
 
 Servo servoPan(p21);
 Servo servoTilt(p22);
+Servo servoStick(p23);
 
 typedef struct {
     float x;
@@ -40,7 +41,8 @@
     bool firstTime = true;
     while(1){
         pc.scanf("%f,%f", &x, &y);
-        if(firstTime || !withinDelta(x, lastX, 0.02) || !withinDelta(y, lastY, 0.02)) {
+        //if(firstTime || !withinDelta(x, lastX, 0.02) || !withinDelta(y, lastY, 0.02)) {
+        if(true) {
             firstTime = false;
             Position* pos = rrInput.alloc();
             if(pos != NULL) { // Can't have nmore than 16 items in the queue, alloc() returns null if queue is full.
@@ -51,7 +53,7 @@
             lastX = x;
             lastY = y;
         }
-
+        Thread::wait(50);
     }
 }
 
@@ -75,9 +77,11 @@
             //Displays debug data
             lcd.cls();
             lcd.locate(0,0);
-            lcd.printf("InX: %.3f, OutX: %.3f", reading->x, servoOutX);
+            /*lcd.printf("InX: %.3f, OutX: %.3f", reading->x, servoOutX);
             lcd.locate(0,15);
             lcd.printf("InY: %.3f, OutY: %.3f", reading->y, servoOutY);
+            lcd.locate(0,30);*/
+            lcd.printf("%f", sensorIn);
             
             rrInput.free(reading);
             Thread::wait(50);
@@ -93,6 +97,8 @@
         servoPan = servoOutX;
         servoTilt = servoOutY;
         
+        servoStick = 1 - clamp(((sensorIn -  0.01)/0.06),0,1);;
+        
         Thread::wait(50);
     }
 }