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@18:974430ee2fbb, 2015-04-20 (annotated)

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