Diff: main.cpp.orig

Revision:

26:71288f42dbc6

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/main.cpp.orig	Thu Apr 23 14:46:50 2015 +0000
@@ -0,0 +1,133 @@
+#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);
+Serial pc(USBTX, USBRX);
+Mutex mutexIn;// protect global variables
+Mutex mutexOut;// protect global variables
+Mutex mutex_sonar;
+AnalogIn sonar(p17); // correct is p17. TEMPORARY changed to potmeter p19
+//AnalogIn corVert(p20); // TO REMOVE. Temporary changed to potmeter
+
+// Global variables
+float corHoriz = 0.5; // horizontal variable arrives from OpenCV
+float corVert = 0.5; // vertical variable arrives from OpenCV
+float distance = 0.5;// variable holds the distance in meters 0 to 3.3
+float norm=0;      // variable holds the normalised values form the sonar sensor
+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....
+
+typedef enum {
+    searching,
+    tracking,
+    retreating
+} LampState;
+
+LampState lamp;
+
+/* parallax ultrasound range finder
+p21 pin the range finder is connected to.
+10 is Time of pulse to send to the rangefinder to trigger a measurement, in microseconds.
+5800 is   Scaling of the range finder's output pulse from microseconds to metres.
+100000 Time to wait for a pulse from the range finder before giving up */
+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)
+{
+    while (true) {
+        pc.scanf("%f,%f", &corHoriz, &corVert);// read from serial port the data // temporary Vert1
+    }
+}
+
+/* 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)
+{
+    while (true) {
+        mutex_sonar.lock();
+        // Display values on the LCD screen
+        lcd.cls();          // clear the display
+        lcd.locate(0,1);    // the location where you want your charater to be displayed
+        lcd.printf("Sonar: %0.3f, OutTilt: %0.3f", sonar.read(), outTilt);
+        lcd.locate(0,9);    // the location where you want your charater to be displayed
+        lcd.printf("Vert: %0.3f, OutVert: %0.3f", corVert, outVert);
+        Thread::wait(250);
+        mutex_sonar.unlock();
+    }
+}
+
+/* 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) {
+    while (true) {
+        mutexIn.lock();
+        if (corVert > 0 && corVert < 1) { // if corVert is valid (between 0 - 1) then do movements                
+            // moves lamp down by the fraction of the difference from the middle
+            if (corVert >= .5) {
+                outVert = corVert - ((corVert - .5) * sonar);
+            } else {
+                outVert = corVert + ((.5 - corVert) * sonar);
+            }
+            outTilt = corVert;
+            outHoriz = corHoriz;
+        } else { // Else this is the case when there is no input from the OpenCV
+            outVert = 1;
+            outTilt = corVert;
+            // TODO Pan search code here. (Searching personality.)
+            /*if (search < outHoriz && search < .9) {
+                outHoriz = outHoriz + .05;
+                search = search + .05;
+            } else if (search > outHoriz && search > .1) {
+                outHoriz = 
+                search = 
+            }*/
+        }
+        mutexIn.unlock();
+        Thread::wait(250);
+    }
+}
+
+/* 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;
+        mutexOut.unlock();
+        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) {
+        mutex_sonar.lock();
+        distance = rf.read_m(); // read the distance from the sonar sensor in meter
+        norm= distance/3.3;     // normalised value from the sonar sensor
+        printf("dis: %0.2f", distance);// Display the distance in meters from the sonar
+        mutex_sonar.unlock();
+        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) {     
+        wait(1);
+    }
+}