Diff: main.cpp

Revision:

18:974430ee2fbb

Parent:

11:af76305da577

Parent:

17:4730277de7c2

Child:

19:582c5a0c868c

Child:

23:f098ffcd192a

--- a/main.cpp	Fri Apr 17 10:12:20 2015 +0000
+++ b/main.cpp	Mon Apr 20 13:31:33 2015 +0000
@@ -4,26 +4,30 @@
 #include "C12832.h"
 #include "RangeFinder.h"// header files for sonar sensor
 
-Servo tiltServo(p22);
-Servo panServo(p22);
 
+Servo tiltServo(p24);
+Servo panServo(p25);
 Serial pc(USBTX, USBRX);
 Servo vertServo(p23);
-AnalogIn sonar(p19); //not any more used
 
 Mutex mutexIn;// protect globel variables
 Mutex mutexOut;// protect globel variables
 Mutex mutex_sonar;
+AnalogIn sonar(p19); // temporary changed to potmeter
+Mutex mutexIn;
+Mutex mutexOut;
 
 // Global variables
 float corHoriz = 0; // horizontal variable arrives from OpenCV
 float corVert = 0; // vertical variable arrives from OpenCV
 float distance = 0;// variable holds the distance in meters 0 to 3.3
 float norm=0;      // variable holds the normalised values form the sonar sensor
-float outVert; // rr
-float outTilt;
-float outHoriz;
+float outVert; // output to vertical servo
+float outTilt; // output to tilt servo
+float outHoriz; // output to horizontal servo
 C12832 lcd(p5, p7, p6, p8, p11);// lcd is an object from class c12832 initialised by p5,p7....
+float differ; // temporary global. Can be local.
+
 
 /*parallax ultrasound range finder
 p21 pin the range finder is connected to.
@@ -35,42 +39,54 @@
 RangeFinder rf(p26, 10, 5800.0, 100000);
 /* Thread Serial 1 - handles the output data from the control thread, and pass to the servo.
     @update s1, s2 */
-void serial_thread(void const *args) {
+void serial_thread(void const *args)
+{
     while (true) {
         pc.scanf("%f,%f", &corHoriz, &corVert);// read from serial port the data
     }
 }
- 
+
 /* Thread LCD 2 - handles the input data from the sonar sensor, and display on the LCD screen.
     @update inData */
-void lcd_thread(void const *args) {
+void lcd_thread(void const *args)
+{
     while (true) {
         mutex_sonar.lock();
         // Display values on the LCD screen
         lcd.cls();          // clear the display
         lcd.locate(0,5);    // the location where you want your charater to be displayed
-        lcd.printf("Hor: %0.3f", corHoriz);
-        lcd.locate(0,10);    // the location where you want your charater to be displayed
-        lcd.printf("Ver: %0.3f", corVert);
+
+        lcd.printf("differ: %0.3f, OutTilt: %0.3f", differ, outTilt);
         lcd.locate(0,20);    // the location where you want your charater to be displayed
-        lcd.printf("dis: %0.2f", distance);// Display the distance in meters from the sonar
-        mutex_sonar.unlock();
+        lcd.printf("Vert: %0.3f, OutVert: %0.3f", corVert, outVert);
         Thread::wait(250);
     }
 }
 
 /* Thread Control 3 - handles the input data from the sonar sensor, and display on the LCD screen.
     @update inData */
-void control_thread(void const *args) {
+void control_thread(void const *args)
+{
     while (true) {
         mutexIn.lock();
-        
-        float differ;
-        differ = exp(corVert + sonar * outTilt) / (1 + exp(corVert + sonar * outTilt));
-        if (corVert > .5) { // check if face is below the half of the camera view
-            if (outTilt > .5) { // check if lamp head is facing down
-                // moves lamp down by the fraction of the difference from the middle
-            }
+        //float differ;
+        differ = exp(corVert + sonar * (outTilt / 20)) / (1 + exp(corVert + sonar * (outTilt / 20)));
+        if (corVert > 0.1 && corVert < 0.9) { // if corVert is valid (between 0 - 1) then do movements                
+            // moves lamp down by the fraction of the difference from the middle
+            outVert = corVert * (1 - differ);
+            // tilt down by the fraction of the difference from the middle
+            outTilt = corVert * differ;  /*
+        } else if (0 < corVert < 0.3) {
+            outVert = 0;
+            outTilt = corVert * .9; // Follow the low movement only with the tilt servo. 
+                                           // (.9 is the correction of the side of the screen at OpenCV)
+        } else if (corVert > 0.7 && corVert < 1) {
+            outVert = 1;
+            outTilt = corVert * 1.1; */
+        } else { // Else this is the case when there is no input from the OpenCV
+            outVert = .5;
+            outTilt = corVert * differ;
+            // TODO Pan search code here. (Searching personality.)
         }
         mutexIn.unlock();
         Thread::wait(250);
@@ -79,7 +95,8 @@
 
 /* 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) {
+void servo_thread(void const *args)
+{
     while (true) {
         mutexOut.lock();
         tiltServo = outTilt;
@@ -89,6 +106,7 @@
         Thread::wait(250);
     }
 }
+
 /* Thread sonar 5 - handles the sonar values which can be in meter or normailsed value to one */ 
 void sonar_thread(void const *args) {
     while (true) {
@@ -100,14 +118,14 @@
         Thread::wait(250);
     }
 }
+
 int main() {
     Thread thread_1(serial_thread); // Start Serial Thread
     Thread thread_2(lcd_thread); // Start LCD Thread
     Thread thread_3(control_thread); // Start Servo Thread
     Thread thread_4(servo_thread); // Start Servo Thread
     Thread thread_5(sonar_thread); // Start Servo Thread
-    while(1) {
-       
+    while(1) {     
         wait(1);
     }
 }