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@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?

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 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 }