sonar sensor is working in the version of code but the reading are not correct
Dependencies: C12832 Pulse RangeFinder Servo mbed rtos
Fork of Team_Sprint2 by
main.cpp
- Committer:
- Soldier7
- Date:
- 2015-04-13
- Revision:
- 15:4ef3d9835b13
- Parent:
- 10:ca6f2769964e
- Parent:
- 14:f6d4980c48d6
- Child:
- 16:74733a28eb80
File content as of revision 15:4ef3d9835b13:
#include "mbed.h" #include "rtos.h" #include "Servo.h" #include "C12832.h" Servo tiltServo(p21); Servo panServo(p22); Serial pc(USBTX, USBRX); Servo vertServo(p23); AnalogIn sonar(p19); 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; float outVert; // float outTilt; float outHoriz; C12832 lcd(p5, p7, p6, p8, p11);// lcd is an object from class c12832 initialised by p5,p7.... /* 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 } } /* 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) { // 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,20); // the location where you want your charater to be displayed lcd.printf("Ver: %0.3f", corVert); 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) { while (true) { mutexIn.lock(); float differ; differ = exp(corVert + sonar * outTilt) / (1 + exp(corVert + sonar * outTilt)); if (corVert > .45) { // check if face is below the half of the camera view if (outTilt > .45) { // check if lamp head is facing down // moves lamp down by the fraction of the difference from the middle outVert = outVert + outTilt = outTilt + } else if (outTilt < .55) { // check if lamp head is facing up // } } else if (corVert < .55) { // moves lamp up by the fraction of the difference from the middle } mutexIn.unlock(); Thread::wait(25); } mutexIn.unlock(); Thread::wait(25); } } /* 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(200); } } 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 while(1) { wait(1); } }