Diff: main.cpp

Revision:

1:a22303e03da3

Parent:

0:89523986a177

Child:

2:b1e22f332443

--- a/main.cpp	Thu Feb 26 16:48:02 2015 +0000
+++ b/main.cpp	Mon Mar 02 14:46:00 2015 +0000
@@ -14,13 +14,44 @@
 Servo servoPan(p21);
 Servo servoTilt(p22);
 
+typedef struct {
+    float x;
+    float y;
+} Position;
+
+Mail<Position, 16> rrInput;
+
 static float clamp(float value, float min, float max){
     return (value < min)? min : (value > max)? max : value;    
 }
 
+bool withinDelta(float a, float b, float delta) {
+    return fabs(a - b) < delta;
+}
+
 void getUserPosition(void const *name){
+    float x;
+    float y;
+    float lastX;
+    float lastY;
+    pc.scanf("%f,%f", &x, &y);
+    lastX = x;
+    lastY = y;
+    bool firstTime = true;
     while(1){
-        pc.scanf("%f,%f", &userX, &userY);
+        pc.scanf("%f,%f", &x, &y);
+        if(firstTime || !withinDelta(x, lastX, 0.02) || !withinDelta(y, lastY, 0.02)) {
+            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.
+                pos->x = x;
+                pos->y = y;
+                rrInput.put(pos);
+            }
+            lastX = x;
+            lastY = y;
+        }
+
     }
 }
 
@@ -33,19 +64,24 @@
 
 void logic(void const *name){
     while(1){
-        
-        servoOutX = clamp(userX, 0, 1);
-        servoOutY = clamp(userY, 0, 1);
-        
-        //Displays debug data
-        lcd.cls();
-        lcd.locate(0,0);
-        lcd.printf("InX: %f, OutX: %f", userX, servoOutX);
-        lcd.locate(0,15);
-        lcd.printf("InY: %f, OutY: %f", userY, servoOutY);
-        
-        Thread::wait(50);
-        
+        osEvent evt = rrInput.get();
+        if(evt.status == osEventMail) {
+            Position* reading = (Position*)evt.value.p;
+            lcd.cls();
+            lcd.locate(0, 0);
+            servoOutX = clamp(reading->x, 0, 1);
+            servoOutY = clamp(reading->y, 0, 1);
+            
+            //Displays debug data
+            lcd.cls();
+            lcd.locate(0,0);
+            lcd.printf("InX: %.3f, OutX: %.3f", reading->x, servoOutX);
+            lcd.locate(0,15);
+            lcd.printf("InY: %.3f, OutY: %.3f", reading->y, servoOutY);
+            
+            rrInput.free(reading);
+            Thread::wait(50);
+        }
     }
 }