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

Committer:
Soldier7
Date:
Thu Apr 23 10:50:12 2015 +0000
Revision:
23:f098ffcd192a
Parent:
22:5f6df7ae19a3
Parent:
18:974430ee2fbb
Child:
24:00a577f64930
Merge Andras's changes

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"
Ali_taher 11:af76305da577 5 #include "RangeFinder.h"// header files for sonar sensor
Argensis 9:e5d24b7a921b 6
Soldier7 16:74733a28eb80 7 Servo tiltServo(p24);
Soldier7 16:74733a28eb80 8 Servo panServo(p25);
Soldier7 0:5edc27224a37 9 Serial pc(USBTX, USBRX);
Argensis 10:ca6f2769964e 10 Servo vertServo(p23);
Soldier7 23:f098ffcd192a 11 Mutex mutexIn;// protect global variables
Soldier7 23:f098ffcd192a 12 Mutex mutexOut;// protect global variables
Ali_taher 11:af76305da577 13 Mutex mutex_sonar;
Soldier7 16:74733a28eb80 14 AnalogIn sonar(p19); // temporary changed to potmeter
Soldier7 23:f098ffcd192a 15 AnalogIn corVert(p20); // temporary changed to potmeter
Soldier7 0:5edc27224a37 16 Mutex mutexIn;
Soldier7 0:5edc27224a37 17 Mutex mutexOut;
Soldier7 0:5edc27224a37 18
Soldier7 8:fe434a018d96 19 // Global variables
Soldier7 17:4730277de7c2 20 float corHoriz = 0.5; // horizontal variable arrives from OpenCV
Soldier7 23:f098ffcd192a 21 float corVert = 0.5; // vertical variable arrives from OpenCV
Soldier7 23:f098ffcd192a 22 float distance = 0.5;// variable holds the distance in meters 0 to 3.3
Ali_taher 11:af76305da577 23 float norm=0; // variable holds the normalised values form the sonar sensor
Soldier7 16:74733a28eb80 24 float outVert; // output to vertical servo
Soldier7 16:74733a28eb80 25 float outTilt; // output to tilt servo
Soldier7 16:74733a28eb80 26 float outHoriz; // output to horizontal servo
Soldier7 0:5edc27224a37 27 C12832 lcd(p5, p7, p6, p8, p11);// lcd is an object from class c12832 initialised by p5,p7....
Argensis 18:974430ee2fbb 28
Soldier7 23:f098ffcd192a 29 /* parallax ultrasound range finder
Ali_taher 11:af76305da577 30 p21 pin the range finder is connected to.
Ali_taher 11:af76305da577 31 10 is Time of pulse to send to the rangefinder to trigger a measurement, in microseconds.
Ali_taher 11:af76305da577 32 5800 is Scaling of the range finder's output pulse from microseconds to metres.
Soldier7 23:f098ffcd192a 33 100000 Time to wait for a pulse from the range finder before giving up */
Soldier7 23:f098ffcd192a 34 RangeFinder rf(p26, 10, 5800.0, 100000);
Ali_taher 11:af76305da577 35
Soldier7 0:5edc27224a37 36 /* Thread Serial 1 - handles the output data from the control thread, and pass to the servo.
Soldier7 0:5edc27224a37 37 @update s1, s2 */
Soldier7 14:f6d4980c48d6 38 void serial_thread(void const *args)
Soldier7 14:f6d4980c48d6 39 {
Soldier7 0:5edc27224a37 40 while (true) {
Soldier7 22:5f6df7ae19a3 41 pc.scanf("%f,%f", &corHoriz, &corVert1);// read from serial port the data // temporary Vert1
Soldier7 0:5edc27224a37 42 }
Soldier7 0:5edc27224a37 43 }
Soldier7 14:f6d4980c48d6 44
Soldier7 0:5edc27224a37 45 /* Thread LCD 2 - handles the input data from the sonar sensor, and display on the LCD screen.
Soldier7 0:5edc27224a37 46 @update inData */
Soldier7 14:f6d4980c48d6 47 void lcd_thread(void const *args)
Soldier7 14:f6d4980c48d6 48 {
Soldier7 0:5edc27224a37 49 while (true) {
Ali_taher 11:af76305da577 50 mutex_sonar.lock();
Soldier7 0:5edc27224a37 51 // Display values on the LCD screen
Soldier7 0:5edc27224a37 52 lcd.cls(); // clear the display
Soldier7 20:5dfebb54cdbd 53 lcd.locate(0,1); // the location where you want your charater to be displayed
Soldier7 22:5f6df7ae19a3 54 lcd.printf("Sonar: %0.3f, OutTilt: %0.3f", sonar.read(), outTilt);
Soldier7 20:5dfebb54cdbd 55 lcd.locate(0,9); // the location where you want your charater to be displayed
Soldier7 20:5dfebb54cdbd 56 lcd.printf("Vert: %0.3f, OutVert: %0.3f", corVert.read(), outVert);
Argensis 9:e5d24b7a921b 57 Thread::wait(250);
Soldier7 0:5edc27224a37 58 }
Soldier7 0:5edc27224a37 59 }
Soldier7 0:5edc27224a37 60
Soldier7 0:5edc27224a37 61 /* Thread Control 3 - handles the input data from the sonar sensor, and display on the LCD screen.
Soldier7 0:5edc27224a37 62 @update inData */
Soldier7 23:f098ffcd192a 63 void control_thread(void const *args) {
Soldier7 0:5edc27224a37 64 while (true) {
Soldier7 0:5edc27224a37 65 mutexIn.lock();
Soldier7 20:5dfebb54cdbd 66 if (corVert > 0 && corVert < 1) { // if corVert is valid (between 0 - 1) then do movements
Soldier7 14:f6d4980c48d6 67 // moves lamp down by the fraction of the difference from the middle
Soldier7 22:5f6df7ae19a3 68 if (corVert >= .5) {
Soldier7 22:5f6df7ae19a3 69 outVert = corVert - ((corVert - .5) * sonar);
Soldier7 22:5f6df7ae19a3 70 } else {
Soldier7 22:5f6df7ae19a3 71 outVert = corVert + ((.5 - corVert) * sonar);
Soldier7 22:5f6df7ae19a3 72 }
Soldier7 22:5f6df7ae19a3 73 outTilt = corVert;
Soldier7 22:5f6df7ae19a3 74 outHoriz = corHoriz;
Soldier7 22:5f6df7ae19a3 75 } else { // Else this is the case when there is no input from the OpenCV
Soldier7 16:74733a28eb80 76 outVert = 1;
Soldier7 20:5dfebb54cdbd 77 outTilt = corVert;
Soldier7 16:74733a28eb80 78 // TODO Pan search code here. (Searching personality.)
Soldier7 22:5f6df7ae19a3 79 /*if (search < outHoriz && search < .9) {
Soldier7 22:5f6df7ae19a3 80 outHoriz = outHoriz + .05;
Soldier7 22:5f6df7ae19a3 81 search = search + .05;
Soldier7 22:5f6df7ae19a3 82 } else if (search > outHoriz && search > .1) {
Soldier7 22:5f6df7ae19a3 83 outHoriz =
Soldier7 22:5f6df7ae19a3 84 search =
Soldier7 22:5f6df7ae19a3 85 }*/
Soldier7 14:f6d4980c48d6 86 }
Soldier7 0:5edc27224a37 87 mutexIn.unlock();
Ali_taher 11:af76305da577 88 Thread::wait(250);
Soldier7 0:5edc27224a37 89 }
Soldier7 0:5edc27224a37 90 }
Soldier7 0:5edc27224a37 91
Soldier7 0:5edc27224a37 92 /* Thread Servo 4 - handles the output data from the control thread, and pass to the servo.
Soldier7 0:5edc27224a37 93 @update s1, s2 */
Soldier7 23:f098ffcd192a 94 void servo_thread(void const *args) {
Soldier7 0:5edc27224a37 95 while (true) {
Soldier7 0:5edc27224a37 96 mutexOut.lock();
Argensis 10:ca6f2769964e 97 tiltServo = outTilt;
Argensis 10:ca6f2769964e 98 panServo = outHoriz;
Argensis 10:ca6f2769964e 99 vertServo = outVert;
Soldier7 0:5edc27224a37 100 mutexOut.unlock();
Ali_taher 11:af76305da577 101 Thread::wait(250);
Soldier7 0:5edc27224a37 102 }
Soldier7 0:5edc27224a37 103 }
Soldier7 0:5edc27224a37 104
Ali_taher 11:af76305da577 105 /* Thread sonar 5 - handles the sonar values which can be in meter or normailsed value to one */
Ali_taher 11:af76305da577 106 void sonar_thread(void const *args) {
Ali_taher 11:af76305da577 107 while (true) {
Ali_taher 11:af76305da577 108 mutex_sonar.lock();
Ali_taher 11:af76305da577 109 distance = rf.read_m(); // read the distance from the sonar sensor in meter
Ali_taher 11:af76305da577 110 norm= distance/3.3; // normalised value from the sonar sensor
Ali_taher 11:af76305da577 111 printf("dis: %0.2f", distance);// Display the distance in meters from the sonar
Ali_taher 11:af76305da577 112 mutex_sonar.unlock();
Ali_taher 11:af76305da577 113 Thread::wait(250);
Ali_taher 11:af76305da577 114 }
Ali_taher 11:af76305da577 115 }
Argensis 18:974430ee2fbb 116
Soldier7 0:5edc27224a37 117 int main() {
Soldier7 0:5edc27224a37 118 Thread thread_1(serial_thread); // Start Serial Thread
Soldier7 0:5edc27224a37 119 Thread thread_2(lcd_thread); // Start LCD Thread
Argensis 10:ca6f2769964e 120 Thread thread_3(control_thread); // Start Servo Thread
Soldier7 0:5edc27224a37 121 Thread thread_4(servo_thread); // Start Servo Thread
Ali_taher 11:af76305da577 122 Thread thread_5(sonar_thread); // Start Servo Thread
Argensis 18:974430ee2fbb 123 while(1) {
Argensis 9:e5d24b7a921b 124 wait(1);
Soldier7 0:5edc27224a37 125 }
Soldier7 0:5edc27224a37 126 }