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@6:dc51d320b369, 2015-03-14 (annotated)
- Committer:
- Soldier7
- Date:
- Sat Mar 14 15:57:23 2015 +0000
- Revision:
- 6:dc51d320b369
- Parent:
- 5:4354b585dcab
Comment added
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" |
Soldier7 | 5:4354b585dcab | 5 | Servo sHoriz(p21); |
Soldier7 | 5:4354b585dcab | 6 | Servo sVert(p22); |
Soldier7 | 0:5edc27224a37 | 7 | Serial pc(USBTX, USBRX); |
Soldier7 | 0:5edc27224a37 | 8 | Mutex mutexIn; |
Soldier7 | 0:5edc27224a37 | 9 | Mutex mutexOut; |
Soldier7 | 0:5edc27224a37 | 10 | |
Soldier7 | 6:dc51d320b369 | 11 | /* Modification changes based on http://lampbotics.blogspot.ie/2015/03/basic-serial-data.html */ |
Soldier7 | 6:dc51d320b369 | 12 | |
Soldier7 | 0:5edc27224a37 | 13 | // Globel variables |
Soldier7 | 5:4354b585dcab | 14 | float corHoriz, corVert, outVert, outHoriz; |
Soldier7 | 0:5edc27224a37 | 15 | C12832 lcd(p5, p7, p6, p8, p11);// lcd is an object from class c12832 initialised by p5,p7.... |
Soldier7 | 0:5edc27224a37 | 16 | |
Soldier7 | 0:5edc27224a37 | 17 | /* Thread Serial 1 - handles the output data from the control thread, and pass to the servo. |
Soldier7 | 0:5edc27224a37 | 18 | @update s1, s2 */ |
Soldier7 | 0:5edc27224a37 | 19 | void serial_thread(void const *args) { |
Soldier7 | 0:5edc27224a37 | 20 | while (true) { |
Soldier7 | 0:5edc27224a37 | 21 | mutexIn.lock(); |
Soldier7 | 5:4354b585dcab | 22 | pc.scanf("%f,%f", &corHoriz, &corVert); |
Soldier7 | 0:5edc27224a37 | 23 | mutexIn.unlock(); |
Soldier7 | 5:4354b585dcab | 24 | Thread::wait(50); |
Soldier7 | 0:5edc27224a37 | 25 | } |
Soldier7 | 0:5edc27224a37 | 26 | } |
Soldier7 | 0:5edc27224a37 | 27 | |
Soldier7 | 5:4354b585dcab | 28 | /* Thread Calc and LCD 2 - handles the input data from the sonar sensor, and display on the LCD screen. |
Soldier7 | 0:5edc27224a37 | 29 | @update inData */ |
Soldier7 | 0:5edc27224a37 | 30 | void lcd_thread(void const *args) { |
Soldier7 | 0:5edc27224a37 | 31 | while (true) { |
Soldier7 | 0:5edc27224a37 | 32 | mutexIn.lock(); |
Soldier7 | 0:5edc27224a37 | 33 | mutexOut.lock(); |
Soldier7 | 5:4354b585dcab | 34 | // Calculate servo movement data arrived from openCV |
Soldier7 | 5:4354b585dcab | 35 | // This case openCV send 5+ if need to increase the value of cordinates |
Soldier7 | 5:4354b585dcab | 36 | outHoriz = outHoriz + ((corHoriz - 5) / 1000); |
Soldier7 | 5:4354b585dcab | 37 | outVert = outVert + ((corVert - 5) / 1000); |
Soldier7 | 0:5edc27224a37 | 38 | // Display values on the LCD screen |
Soldier7 | 0:5edc27224a37 | 39 | lcd.cls(); // clear the display |
Soldier7 | 0:5edc27224a37 | 40 | lcd.locate(0,3); // the location where you want your charater to be displayed |
Soldier7 | 5:4354b585dcab | 41 | lcd.printf("Cor Hor: %.3f, Vert: %.3f", corHoriz, corVert); |
Soldier7 | 5:4354b585dcab | 42 | lcd.printf("Out Hor: %.3f, Vert: %.3f", outHoriz, outVert); |
Soldier7 | 5:4354b585dcab | 43 | lcd.printf("Servo Hor: %f, Vert: %f", sHoriz, sVert); |
Soldier7 | 0:5edc27224a37 | 44 | mutexIn.unlock(); |
Soldier7 | 0:5edc27224a37 | 45 | mutexOut.unlock(); |
Soldier7 | 5:4354b585dcab | 46 | Thread::wait(50); |
Soldier7 | 0:5edc27224a37 | 47 | } |
Soldier7 | 0:5edc27224a37 | 48 | } |
Soldier7 | 0:5edc27224a37 | 49 | |
Soldier7 | 5:4354b585dcab | 50 | /* Thread Servo 3 - handles the output data from the control thread, and pass to the servo. |
Soldier7 | 0:5edc27224a37 | 51 | @update s1, s2 */ |
Soldier7 | 0:5edc27224a37 | 52 | void servo_thread(void const *args) { |
Soldier7 | 0:5edc27224a37 | 53 | while (true) { |
Soldier7 | 0:5edc27224a37 | 54 | mutexOut.lock(); |
Soldier7 | 5:4354b585dcab | 55 | sVert = outVert; |
Soldier7 | 5:4354b585dcab | 56 | sHoriz = outHoriz; |
Soldier7 | 0:5edc27224a37 | 57 | mutexOut.unlock(); |
Soldier7 | 5:4354b585dcab | 58 | Thread::wait(50); |
Soldier7 | 0:5edc27224a37 | 59 | } |
Soldier7 | 0:5edc27224a37 | 60 | } |
Soldier7 | 0:5edc27224a37 | 61 | |
Soldier7 | 0:5edc27224a37 | 62 | int main() { |
Soldier7 | 0:5edc27224a37 | 63 | Thread thread_1(serial_thread); // Start Serial Thread |
Soldier7 | 0:5edc27224a37 | 64 | Thread thread_2(lcd_thread); // Start LCD Thread |
Soldier7 | 5:4354b585dcab | 65 | Thread thread_3(servo_thread); // Start Servo Thread |
Soldier7 | 0:5edc27224a37 | 66 | while(1) { |
Soldier7 | 0:5edc27224a37 | 67 | Thread::wait(10); |
Soldier7 | 0:5edc27224a37 | 68 | } |
Soldier7 | 0:5edc27224a37 | 69 | } |