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
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?
User | Revision | Line number | New 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 | } |