Ali Alshaqaq / Team_Sprint2

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 WIT_EmbOS_Gr1

main.cpp@14:f6d4980c48d6, 2015-04-13 (annotated)

Committer:
Soldier7
Date:
Mon Apr 13 13:03:16 2015 +0000
Revision:
14:f6d4980c48d6
Parent:
8:fe434a018d96
Child:
15:4ef3d9835b13
Unfinished changes on tilt and up servo movement.

Who changed what in which revision?

UserRevisionLine numberNew contents of line
Soldier7 0:5edc27224a37 1 #include "mbed.h"
Soldier7 0:5edc27224a37 2 #include "rtos.h"
Soldier7 0:5edc27224a37 3 #include "Servo.h"
Soldier7 0:5edc27224a37 4 #include "C12832.h"
Soldier7 8:fe434a018d96 5 Servo sPan(p21);
Soldier7 7:24d62ef1ed34 6 Servo sTilt(p22);
Soldier7 0:5edc27224a37 7 Serial pc(USBTX, USBRX);
Soldier7 8:fe434a018d96 8 Servo sVert(p23);
Soldier7 14:f6d4980c48d6 9 AnalogIn distance(p19);
Soldier7 0:5edc27224a37 10 Mutex mutexIn;
Soldier7 0:5edc27224a37 11 Mutex mutexOut;
Soldier7 0:5edc27224a37 12
Soldier7 8:fe434a018d96 13 // Global variables
Soldier7 8:fe434a018d96 14 float corHoriz; // horizontal variable arrives from OpenCV
Soldier7 8:fe434a018d96 15 float corVert; // vertical variable arrives from OpenCV
Soldier7 8:fe434a018d96 16 float distance; // distance measured by the sonar
Soldier7 14:f6d4980c48d6 17 float outVert; //
Soldier7 8:fe434a018d96 18 float outTilt;
Soldier7 8:fe434a018d96 19 float outHoriz;
Soldier7 0:5edc27224a37 20 C12832 lcd(p5, p7, p6, p8, p11);// lcd is an object from class c12832 initialised by p5,p7....
Soldier7 0:5edc27224a37 21
Soldier7 0:5edc27224a37 22 /* Thread Serial 1 - handles the output data from the control thread, and pass to the servo.
Soldier7 0:5edc27224a37 23 @update s1, s2 */
Soldier7 14:f6d4980c48d6 24 void serial_thread(void const *args)
Soldier7 14:f6d4980c48d6 25 {
Soldier7 0:5edc27224a37 26 while (true) {
Soldier7 0:5edc27224a37 27 mutexIn.lock();
Soldier7 14:f6d4980c48d6 28 // pc.gets(cordinates,4);
Soldier7 14:f6d4980c48d6 29 // cordinates = pc.putc(pc.getc());
Soldier7 14:f6d4980c48d6 30 pc.scanf("%s \n %s \n ",corHoriz,corVert);// read from serial port the data
Soldier7 0:5edc27224a37 31 mutexIn.unlock();
Soldier7 0:5edc27224a37 32 Thread::wait(200);
Soldier7 0:5edc27224a37 33 }
Soldier7 0:5edc27224a37 34 }
Soldier7 14:f6d4980c48d6 35
Soldier7 0:5edc27224a37 36 /* Thread LCD 2 - handles the input data from the sonar sensor, and display on the LCD screen.
Soldier7 0:5edc27224a37 37 @update inData */
Soldier7 14:f6d4980c48d6 38 void lcd_thread(void const *args)
Soldier7 14:f6d4980c48d6 39 {
Soldier7 0:5edc27224a37 40 while (true) {
Soldier7 0:5edc27224a37 41 mutexIn.lock();
Soldier7 0:5edc27224a37 42 mutexOut.lock();
Soldier7 0:5edc27224a37 43 // Display values on the LCD screen
Soldier7 0:5edc27224a37 44 lcd.cls(); // clear the display
Soldier7 4:70090f3b1f07 45 lcd.locate(0,5); // the location where you want your charater to be displayed
Soldier7 4:70090f3b1f07 46 lcd.printf("Hor:%s",corHoriz);
Ali_taher 3:3d53799c2f18 47 lcd.locate(0,20); // the location where you want your charater to be displayed
Soldier7 4:70090f3b1f07 48 lcd.printf("Ver%s", corVert);
Soldier7 0:5edc27224a37 49 mutexIn.unlock();
Soldier7 0:5edc27224a37 50 mutexOut.unlock();
Soldier7 0:5edc27224a37 51 Thread::wait(25);
Soldier7 0:5edc27224a37 52 }
Soldier7 0:5edc27224a37 53 }
Soldier7 0:5edc27224a37 54
Soldier7 0:5edc27224a37 55 /* Thread Control 3 - handles the input data from the sonar sensor, and display on the LCD screen.
Soldier7 0:5edc27224a37 56 @update inData */
Soldier7 14:f6d4980c48d6 57 void control_thread(void const *args)
Soldier7 14:f6d4980c48d6 58 {
Soldier7 0:5edc27224a37 59 while (true) {
Soldier7 0:5edc27224a37 60 mutexIn.lock();
Soldier7 8:fe434a018d96 61 float differ;
Soldier7 8:fe434a018d96 62 differ = exp(corVert + sonar * outTilt) / (1 + exp(corVert + sonar * outTilt));
Soldier7 14:f6d4980c48d6 63 if (corVert > .45) { // check if face is below the half of the camera view
Soldier7 14:f6d4980c48d6 64 if (outTilt > .45) { // check if lamp head is facing down
Soldier7 14:f6d4980c48d6 65 // moves lamp down by the fraction of the difference from the middle
Soldier7 14:f6d4980c48d6 66 outVert = outVert +
Soldier7 14:f6d4980c48d6 67 outTilt = outTilt +
Soldier7 14:f6d4980c48d6 68 } else if (outTilt < .55) { // check if lamp head is facing up
Soldier7 14:f6d4980c48d6 69 //
Soldier7 14:f6d4980c48d6 70 }
Soldier7 14:f6d4980c48d6 71
Soldier7 14:f6d4980c48d6 72 } else if (corVert < .55) {
Soldier7 14:f6d4980c48d6 73 // moves lamp up by the fraction of the difference from the middle
Soldier7 14:f6d4980c48d6 74 }
Soldier7 0:5edc27224a37 75 }
Soldier7 14:f6d4980c48d6 76 mutexIn.unlock();
Soldier7 14:f6d4980c48d6 77 Thread::wait(25);
Soldier7 14:f6d4980c48d6 78 }
Soldier7 0:5edc27224a37 79 }
Soldier7 0:5edc27224a37 80
Soldier7 0:5edc27224a37 81 /* Thread Servo 4 - handles the output data from the control thread, and pass to the servo.
Soldier7 0:5edc27224a37 82 @update s1, s2 */
Soldier7 14:f6d4980c48d6 83 void servo_thread(void const *args)
Soldier7 14:f6d4980c48d6 84 {
Soldier7 0:5edc27224a37 85 while (true) {
Soldier7 0:5edc27224a37 86 mutexOut.lock();
Soldier7 8:fe434a018d96 87 sTi1t = outTilt;
Soldier7 8:fe434a018d96 88 sPan = outHoriz;
Soldier7 8:fe434a018d96 89 sVert = outVert;
Soldier7 0:5edc27224a37 90 mutexOut.unlock();
Soldier7 0:5edc27224a37 91 Thread::wait(200);
Soldier7 0:5edc27224a37 92 }
Soldier7 0:5edc27224a37 93 }
Soldier7 0:5edc27224a37 94
Soldier7 14:f6d4980c48d6 95 int main()
Soldier7 14:f6d4980c48d6 96 {
Soldier7 0:5edc27224a37 97 Thread thread_1(serial_thread); // Start Serial Thread
Soldier7 0:5edc27224a37 98 Thread thread_2(lcd_thread); // Start LCD Thread
Soldier7 0:5edc27224a37 99 Thread thread_3(control_thread); // Start Control Thread
Soldier7 0:5edc27224a37 100 Thread thread_4(servo_thread); // Start Servo Thread
Soldier7 0:5edc27224a37 101 while(1) {
Soldier7 0:5edc27224a37 102 Thread::wait(10);
Soldier7 0:5edc27224a37 103 }
Soldier7 0:5edc27224a37 104 }