Diff: main.cpp

Revision:

28:e04fb7a2a51e

Parent:

27:b1653e9bc81c

Child:

29:e8f7b891b5c1

--- a/main.cpp	Thu Apr 23 16:50:27 2015 +0000
+++ b/main.cpp	Fri Apr 24 10:29:27 2015 +0000
@@ -1,12 +1,11 @@
 #include "mbed.h"
 #include "rtos.h"
-#include "Servo.h"
 #include "C12832.h"
 #include "RangeFinder.h"// header files for sonar sensor
 
-Servo tiltServo(p24);
-Servo panServo(p25);
-Servo vertServo(p23);
+PwmOut tiltServo(p24);
+PwmOut panServo(p25);
+PwmOut vertServo(p23);
 Serial pc(USBTX, USBRX);
 Mutex mutexIn;// protect global variables
 Mutex mutexOut;// protect global variables
@@ -74,12 +73,12 @@
 
         // lcd.cls();          // clear the display
         lcd.locate(0,0);    // the location where you want your charater to be displayed
-        lcd.printf("outTilt: %0.3f", outTilt);
+        lcd.printf("tilt: %0.3f, pan: %0.3f", tiltServo.read(), panServo.read());
 
         // lcd.cls();          // clear the display
         lcd.locate(0,10);    // the location where you want your charater to be displayed
         //lcd.printf("Vert: %0.3f, OutVert: %0.3f", corVert, outVert);
-        lcd.printf("OutHoriz: %0.3f", outHoriz);
+        lcd.printf("vert: %0.3f", vertServo.read());
 
         //lcd.cls();          // clear the display
         lcd.locate(0,20);    // the location where you want your charater to be displayed
@@ -114,14 +113,14 @@
 }
 
 void shooPerson() {
-    outVert = 0.1;
-    outTilt = 0.1;
+    outVert = 0.2;
+    outTilt = 0.2;
     Thread::wait(5000);
-    outTilt = 1;
+    outTilt = 0.8;
     Thread::wait(5000);
-    outTilt = 0.1;
+    outTilt = 0.2;
     Thread::wait(5000);
-    outTilt = 1;
+    outTilt = 0.8;
 }
 
 /* Thread Control 3 - handles the input data from the sonar sensor, and display on the LCD screen.
@@ -154,15 +153,26 @@
     }
 }
 
+float clamp(float min, float max, float scale) {
+    if(scale > 1) {
+        scale = 1;
+    }
+    if(scale < 0) {
+        scale = 0;
+    }
+    return ((max - min) * scale) + min;
+}
+
 /* Thread Servo 4 - handles the output data from the control thread, and pass to the servo.
     @update s1, s2 */
 void servo_thread(void const *args)
 {
     while (true) {
         mutexOut.lock();
-        tiltServo = outTilt;
-        panServo = outHoriz;
-        vertServo = outVert;
+        
+        tiltServo = clamp(0.01, 0.1, outTilt);
+        panServo = clamp(0.01, 0.1, outHoriz);
+        vertServo = clamp(0.01, 0.1, outVert);
         mutexOut.unlock();
         Thread::wait(250);
     }