Diff: main.cpp

Revision:

0:89523986a177

Child:

1:a22303e03da3

diff -r 000000000000 -r 89523986a177 main.cpp
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/main.cpp	Thu Feb 26 16:48:02 2015 +0000
@@ -0,0 +1,73 @@
+#include "mbed.h"
+#include "C12832.h"
+#include "Servo.h"
+#include "rtos.h"
+
+C12832 lcd(p5, p7, p6, p8, p11);
+Serial pc(USBTX, USBRX);
+AnalogIn aIn(p17);
+
+float userX, userY;
+float servoOutX, servoOutY;
+float sensorIn;
+
+Servo servoPan(p21);
+Servo servoTilt(p22);
+
+static float clamp(float value, float min, float max){
+    return (value < min)? min : (value > max)? max : value;    
+}
+
+void getUserPosition(void const *name){
+    while(1){
+        pc.scanf("%f,%f", &userX, &userY);
+    }
+}
+
+void getSensorValue(void const *name){
+    while(1){
+        sensorIn = aIn.read();
+        Thread::wait(50);
+    }
+}
+
+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);
+        
+    }
+}
+
+//Sets the servos to the given data
+void setServoValue(void const *name){
+    while(1){
+        
+        //Set the servos to the set values
+        servoPan = servoOutX;
+        servoTilt = servoOutY;
+        
+        Thread::wait(50);
+    }
+}
+ 
+int main(){
+    //Start each of the threads
+    Thread t1(getUserPosition, (void *)"Th 1");
+    Thread t2(getSensorValue, (void *)"Th 2");
+    Thread t3(logic, (void *)"Th 3");
+    Thread t4(setServoValue, (void *) "Th 4");
+    
+    //Stop the main process from dying
+    while(1)wait(10000);
+}
\ No newline at end of file