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

Committer:
Ali_taher
Date:
Thu Apr 23 14:34:02 2015 +0000
Revision:
19:582c5a0c868c
Parent:
18:974430ee2fbb 
Fix RF/Serial interaction

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