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@22:5f6df7ae19a3, 2015-04-23 (annotated)

Committer:
Soldier7
Date:
Thu Apr 23 10:40:09 2015 +0000
Revision:
22:5f6df7ae19a3
Parent:
21:32270b453137
Child:
23:f098ffcd192a
Working code for synchronize the tilt and up-down servos based on distance gathered by the sonar sensor.; As the distance is closer the tilt servo stays as closer to the middle.

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
Soldier7 16:74733a28eb80 6 Servo tiltServo(p24);
Soldier7 16:74733a28eb80 7 Servo panServo(p25);
Soldier7 0:5edc27224a37 8 Serial pc(USBTX, USBRX);
Argensis 10:ca6f2769964e 9 Servo vertServo(p23);
Soldier7 17:4730277de7c2 10
Soldier7 16:74733a28eb80 11 AnalogIn sonar(p19); // temporary changed to potmeter
Soldier7 20:5dfebb54cdbd 12 AnalogIn corVert(p20); // temporary
Soldier7 0:5edc27224a37 13 Mutex mutexIn;
Soldier7 0:5edc27224a37 14 Mutex mutexOut;
Soldier7 0:5edc27224a37 15
Soldier7 8:fe434a018d96 16 // Global variables
Soldier7 17:4730277de7c2 17 float corHoriz = 0.5; // horizontal variable arrives from OpenCV
Soldier7 20:5dfebb54cdbd 18 //float corVert = 0.5; // vertical variable arrives from OpenCV
Soldier7 20:5dfebb54cdbd 19 float corVert1 = 0.5; // temporary
Soldier7 17:4730277de7c2 20 float distance = 0.5; // distance from the sonar sensor
Soldier7 16:74733a28eb80 21 float outVert; // output to vertical servo
Soldier7 16:74733a28eb80 22 float outTilt; // output to tilt servo
Soldier7 16:74733a28eb80 23 float outHoriz; // output to horizontal servo
Soldier7 22:5f6df7ae19a3 24 float search = .9; // set current position of pan servo to find the movement direction
Soldier7 16:74733a28eb80 25 C12832 lcd(p5, p7, p6, p8, p11);// lcd is an object from class c12832 initialised by p5, p7....
Soldier7 0:5edc27224a37 26
Soldier7 0:5edc27224a37 27 /* Thread Serial 1 - handles the output data from the control thread, and pass to the servo.
Soldier7 0:5edc27224a37 28 @update s1, s2 */
Soldier7 14:f6d4980c48d6 29 void serial_thread(void const *args)
Soldier7 14:f6d4980c48d6 30 {
Soldier7 0:5edc27224a37 31 while (true) {
Soldier7 22:5f6df7ae19a3 32 pc.scanf("%f,%f", &corHoriz, &corVert1);// read from serial port the data // temporary Vert1
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 // Display values on the LCD screen
Soldier7 0:5edc27224a37 42 lcd.cls(); // clear the display
Soldier7 20:5dfebb54cdbd 43 lcd.locate(0,1); // the location where you want your charater to be displayed
Soldier7 22:5f6df7ae19a3 44 lcd.printf("Sonar: %0.3f, OutTilt: %0.3f", sonar.read(), outTilt);
Soldier7 20:5dfebb54cdbd 45 lcd.locate(0,9); // the location where you want your charater to be displayed
Soldier7 20:5dfebb54cdbd 46 lcd.printf("Vert: %0.3f, OutVert: %0.3f", corVert.read(), outVert);
Soldier7 20:5dfebb54cdbd 47 //lcd.locate(0,18); // the location where you want your charater to be displayed
Soldier7 20:5dfebb54cdbd 48 //lcd.printf("Sonar: %0.3f", sonar.read());
Argensis 9:e5d24b7a921b 49 Thread::wait(250);
Soldier7 0:5edc27224a37 50 }
Soldier7 0:5edc27224a37 51 }
Soldier7 0:5edc27224a37 52
Soldier7 0:5edc27224a37 53 /* Thread Control 3 - handles the input data from the sonar sensor, and display on the LCD screen.
Soldier7 0:5edc27224a37 54 @update inData */
Soldier7 14:f6d4980c48d6 55 void control_thread(void const *args)
Soldier7 14:f6d4980c48d6 56 {
Soldier7 0:5edc27224a37 57 while (true) {
Soldier7 0:5edc27224a37 58 mutexIn.lock();
Soldier7 20:5dfebb54cdbd 59 if (corVert > 0 && corVert < 1) { // if corVert is valid (between 0 - 1) then do movements
Soldier7 14:f6d4980c48d6 60 // moves lamp down by the fraction of the difference from the middle
Soldier7 22:5f6df7ae19a3 61 if (corVert >= .5) {
Soldier7 22:5f6df7ae19a3 62 outVert = corVert - ((corVert - .5) * sonar);
Soldier7 22:5f6df7ae19a3 63 } else {
Soldier7 22:5f6df7ae19a3 64 outVert = corVert + ((.5 - corVert) * sonar);
Soldier7 22:5f6df7ae19a3 65 }
Soldier7 22:5f6df7ae19a3 66 outTilt = corVert;
Soldier7 22:5f6df7ae19a3 67 outHoriz = corHoriz;
Soldier7 22:5f6df7ae19a3 68 } else { // Else this is the case when there is no input from the OpenCV
Soldier7 16:74733a28eb80 69 outVert = 1;
Soldier7 20:5dfebb54cdbd 70 outTilt = corVert;
Soldier7 16:74733a28eb80 71 // TODO Pan search code here. (Searching personality.)
Soldier7 22:5f6df7ae19a3 72 /*if (search < outHoriz && search < .9) {
Soldier7 22:5f6df7ae19a3 73 outHoriz = outHoriz + .05;
Soldier7 22:5f6df7ae19a3 74 search = search + .05;
Soldier7 22:5f6df7ae19a3 75 } else if (search > outHoriz && search > .1) {
Soldier7 22:5f6df7ae19a3 76 outHoriz =
Soldier7 22:5f6df7ae19a3 77 search =
Soldier7 22:5f6df7ae19a3 78 }*/
Soldier7 14:f6d4980c48d6 79 }
Soldier7 0:5edc27224a37 80 mutexIn.unlock();
Soldier7 0:5edc27224a37 81 Thread::wait(25);
Soldier7 0:5edc27224a37 82 }
Soldier7 0:5edc27224a37 83 }
Soldier7 0:5edc27224a37 84
Soldier7 0:5edc27224a37 85 /* Thread Servo 4 - handles the output data from the control thread, and pass to the servo.
Soldier7 0:5edc27224a37 86 @update s1, s2 */
Soldier7 14:f6d4980c48d6 87 void servo_thread(void const *args)
Soldier7 14:f6d4980c48d6 88 {
Soldier7 0:5edc27224a37 89 while (true) {
Soldier7 0:5edc27224a37 90 mutexOut.lock();
Argensis 10:ca6f2769964e 91 tiltServo = outTilt;
Argensis 10:ca6f2769964e 92 panServo = outHoriz;
Argensis 10:ca6f2769964e 93 vertServo = outVert;
Soldier7 0:5edc27224a37 94 mutexOut.unlock();
Soldier7 0:5edc27224a37 95 Thread::wait(200);
Soldier7 0:5edc27224a37 96 }
Soldier7 0:5edc27224a37 97 }
Soldier7 0:5edc27224a37 98
Soldier7 14:f6d4980c48d6 99 int main()
Soldier7 14:f6d4980c48d6 100 {
Soldier7 0:5edc27224a37 101 Thread thread_1(serial_thread); // Start Serial Thread
Soldier7 0:5edc27224a37 102 Thread thread_2(lcd_thread); // Start LCD Thread
Argensis 10:ca6f2769964e 103 Thread thread_3(control_thread); // Start Servo Thread
Soldier7 0:5edc27224a37 104 Thread thread_4(servo_thread); // Start Servo Thread
Soldier7 0:5edc27224a37 105 while(1) {
Argensis 9:e5d24b7a921b 106 wait(1);
Soldier7 0:5edc27224a37 107 }
Soldier7 0:5edc27224a37 108 }