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@15:4ef3d9835b13, 2015-04-13 (annotated)

Committer:
Soldier7
Date:
Mon Apr 13 13:19:24 2015 +0000
Revision:
15:4ef3d9835b13
Parent:
10:ca6f2769964e
Parent:
14:f6d4980c48d6
Child:
16:74733a28eb80
An other merge with changes to the servos

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