Code for Sprint 2
Dependencies: C12832 mbed rtos RangeFinder
Diff: main.cpp
- Revision:
- 17:974430ee2fbb
- Parent:
- 16:af76305da577
- Parent:
- 15:4730277de7c2
- Child:
- 21:f098ffcd192a
- Child:
- 25:582c5a0c868c
--- 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); } }