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@12:de4d6a06011b, 2015-04-20 (annotated)

Committer:
Ali_taher
Date:
Mon Apr 20 13:06:40 2015 +0000
Revision:
12:de4d6a06011b
Parent:
11:af76305da577 
adding the sonar reading and servo movements when there is any object within the 3 meters

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
Ali_taher 11:af76305da577 7 Servo tiltServo(p22);
Argensis 9:e5d24b7a921b 8 Servo panServo(p22);
Ali_taher 11:af76305da577 9
Soldier7 0:5edc27224a37 10 Serial pc(USBTX, USBRX);
Argensis 10:ca6f2769964e 11 Servo vertServo(p23);
Ali_taher 11:af76305da577 12 AnalogIn sonar(p19); //not any more used
Ali_taher 11:af76305da577 13
Ali_taher 11:af76305da577 14 Mutex mutexIn;// protect globel variables
Ali_taher 11:af76305da577 15 Mutex mutexOut;// protect globel variables
Ali_taher 11:af76305da577 16 Mutex mutex_sonar;
Soldier7 0:5edc27224a37 17
Argensis 9:e5d24b7a921b 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 8:fe434a018d96 23 float outVert; // rr
Soldier7 8:fe434a018d96 24 float outTilt;
Soldier7 8:fe434a018d96 25 float outHoriz;
Soldier7 0:5edc27224a37 26 C12832 lcd(p5, p7, p6, p8, p11);// lcd is an object from class c12832 initialised by p5,p7....
Soldier7 0:5edc27224a37 27
Ali_taher 11:af76305da577 28 /*parallax ultrasound range finder
Ali_taher 12:de4d6a06011b 29 p23 pin the range finder is connected to.
Ali_taher 11:af76305da577 30 10 is Time of pulse to send to the rangefinder to trigger a measurement, in microseconds.
Ali_taher 11:af76305da577 31 5800 is Scaling of the range finder's output pulse from microseconds to metres.
Ali_taher 11:af76305da577 32 100000 Time to wait for a pulse from the range finder before giving up
Ali_taher 11:af76305da577 33 */
Ali_taher 11:af76305da577 34
Ali_taher 12:de4d6a06011b 35 RangeFinder rf(p23, 10, 5800.0, 100000);
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 0:5edc27224a37 38 void serial_thread(void const *args) {
Soldier7 0:5edc27224a37 39 while (true) {
Ali_taher 12:de4d6a06011b 40 // I have problem in this thread the sonar reading is not correct if this thread is used
Ali_taher 12:de4d6a06011b 41
Argensis 9:e5d24b7a921b 42 pc.scanf("%f,%f", &corHoriz, &corVert);// read from serial port the data
Ali_taher 12:de4d6a06011b 43
Ali_taher 12:de4d6a06011b 44
Soldier7 0:5edc27224a37 45 }
Soldier7 0:5edc27224a37 46 }
Soldier7 0:5edc27224a37 47
Soldier7 0:5edc27224a37 48 /* Thread LCD 2 - handles the input data from the sonar sensor, and display on the LCD screen.
Soldier7 0:5edc27224a37 49 @update inData */
Soldier7 0:5edc27224a37 50 void lcd_thread(void const *args) {
Soldier7 0:5edc27224a37 51 while (true) {
Ali_taher 12:de4d6a06011b 52 mutex_sonar.lock();
Soldier7 0:5edc27224a37 53 // Display values on the LCD screen
Soldier7 0:5edc27224a37 54 lcd.cls(); // clear the display
Soldier7 4:70090f3b1f07 55 lcd.locate(0,5); // the location where you want your charater to be displayed
Argensis 9:e5d24b7a921b 56 lcd.printf("Hor: %0.3f", corHoriz);
Ali_taher 11:af76305da577 57 lcd.locate(0,10); // the location where you want your charater to be displayed
Ali_taher 11:af76305da577 58 lcd.printf("Ver: %0.3f", corVert);
Ali_taher 3:3d53799c2f18 59 lcd.locate(0,20); // the location where you want your charater to be displayed
Ali_taher 11:af76305da577 60 lcd.printf("dis: %0.2f", distance);// Display the distance in meters from the sonar
Ali_taher 11:af76305da577 61 mutex_sonar.unlock();
Argensis 9:e5d24b7a921b 62 Thread::wait(250);
Soldier7 0:5edc27224a37 63 }
Soldier7 0:5edc27224a37 64 }
Soldier7 0:5edc27224a37 65
Soldier7 0:5edc27224a37 66 /* Thread Control 3 - handles the input data from the sonar sensor, and display on the LCD screen.
Soldier7 0:5edc27224a37 67 @update inData */
Soldier7 0:5edc27224a37 68 void control_thread(void const *args) {
Soldier7 0:5edc27224a37 69 while (true) {
Soldier7 0:5edc27224a37 70 mutexIn.lock();
Ali_taher 11:af76305da577 71
Soldier7 8:fe434a018d96 72 float differ;
Soldier7 8:fe434a018d96 73 differ = exp(corVert + sonar * outTilt) / (1 + exp(corVert + sonar * outTilt));
Soldier7 7:24d62ef1ed34 74 if (corVert > .5) { // check if face is below the half of the camera view
Soldier7 8:fe434a018d96 75 if (outTilt > .5) { // check if lamp head is facing down
Soldier7 8:fe434a018d96 76 // moves lamp down by the fraction of the difference from the middle
Argensis 10:ca6f2769964e 77 }
Argensis 10:ca6f2769964e 78 }
Soldier7 0:5edc27224a37 79 mutexIn.unlock();
Ali_taher 11:af76305da577 80 Thread::wait(250);
Soldier7 0:5edc27224a37 81 }
Soldier7 0:5edc27224a37 82 }
Soldier7 0:5edc27224a37 83
Soldier7 0:5edc27224a37 84 /* Thread Servo 4 - handles the output data from the control thread, and pass to the servo.
Soldier7 0:5edc27224a37 85 @update s1, s2 */
Soldier7 0:5edc27224a37 86 void servo_thread(void const *args) {
Soldier7 0:5edc27224a37 87 while (true) {
Ali_taher 12:de4d6a06011b 88 // mutex_sonar.lock();
Soldier7 0:5edc27224a37 89 mutexOut.lock();
Argensis 10:ca6f2769964e 90 tiltServo = outTilt;
Argensis 10:ca6f2769964e 91 panServo = outHoriz;
Argensis 10:ca6f2769964e 92 vertServo = outVert;
Ali_taher 12:de4d6a06011b 93
Ali_taher 12:de4d6a06011b 94 /* if the distance more than 3 meters the servo will mover full range CW and CCW*/
Ali_taher 12:de4d6a06011b 95 for(int i=0; i<100; i++) {
Ali_taher 12:de4d6a06011b 96 if (distance>=2)
Ali_taher 12:de4d6a06011b 97 {
Ali_taher 12:de4d6a06011b 98 tiltServo = i/100.0;
Ali_taher 12:de4d6a06011b 99 Thread::wait(25);
Ali_taher 12:de4d6a06011b 100 }
Ali_taher 12:de4d6a06011b 101 }
Ali_taher 12:de4d6a06011b 102 for(int i=100; i>0; i--) {
Ali_taher 12:de4d6a06011b 103 if (distance>=2)
Ali_taher 12:de4d6a06011b 104 {
Ali_taher 12:de4d6a06011b 105 tiltServo = i/100.0;
Ali_taher 12:de4d6a06011b 106 Thread:: wait(25);
Ali_taher 12:de4d6a06011b 107 }
Ali_taher 12:de4d6a06011b 108 }
Ali_taher 12:de4d6a06011b 109 tiltServo.position(0.0);
Ali_taher 12:de4d6a06011b 110 // mutex_sonar.unlock();
Soldier7 0:5edc27224a37 111 mutexOut.unlock();
Ali_taher 11:af76305da577 112 Thread::wait(250);
Soldier7 0:5edc27224a37 113 }
Soldier7 0:5edc27224a37 114 }
Ali_taher 12:de4d6a06011b 115
Ali_taher 11:af76305da577 116 /* Thread sonar 5 - handles the sonar values which can be in meter or normailsed value to one */
Ali_taher 11:af76305da577 117 void sonar_thread(void const *args) {
Ali_taher 12:de4d6a06011b 118 while (1) {
Ali_taher 12:de4d6a06011b 119
Ali_taher 11:af76305da577 120 }
Ali_taher 12:de4d6a06011b 121 }
Soldier7 0:5edc27224a37 122 int main() {
Ali_taher 12:de4d6a06011b 123 // Thread thread_1(serial_thread); // Start Serial Thread
Ali_taher 12:de4d6a06011b 124 Thread thread_2(lcd_thread); // Start LCD Thread
Ali_taher 12:de4d6a06011b 125
Argensis 10:ca6f2769964e 126 Thread thread_3(control_thread); // Start Servo Thread
Soldier7 0:5edc27224a37 127 Thread thread_4(servo_thread); // Start Servo Thread
Ali_taher 11:af76305da577 128 Thread thread_5(sonar_thread); // Start Servo Thread
Soldier7 0:5edc27224a37 129 while(1) {
Ali_taher 12:de4d6a06011b 130 mutex_sonar.lock();
Ali_taher 12:de4d6a06011b 131 distance = rf.read_m(); // read the distance from the sonar sensor in meter
Ali_taher 12:de4d6a06011b 132 norm= distance/3.3; // normalised value from the sonar sensor
Ali_taher 12:de4d6a06011b 133 mutex_sonar.unlock();
Ali_taher 12:de4d6a06011b 134 Thread::wait(25);
Soldier7 0:5edc27224a37 135 }
Soldier7 0:5edc27224a37 136 }