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@5:4354b585dcab, 2015-03-13 (annotated)

Committer:
Soldier7
Date:
Fri Mar 13 22:51:36 2015 +0000
Revision:
5:4354b585dcab
Parent:
0:5edc27224a37
Child:
6:dc51d320b369
Try to display openCV 0 to 9 - increase or decrease data.

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