Code for Sprint 2
Dependencies: C12832 mbed rtos RangeFinder
Diff: main.cpp
- Revision:
- 26:71288f42dbc6
- Parent:
- 23:7d29f2132197
- Parent:
- 25:582c5a0c868c
- Child:
- 27:b1653e9bc81c
diff -r 7d29f2132197 -r 71288f42dbc6 main.cpp --- a/main.cpp Thu Apr 23 14:39:38 2015 +0000 +++ b/main.cpp Thu Apr 23 14:46:50 2015 +0000 @@ -37,14 +37,26 @@ 10 is Time of pulse to send to the rangefinder to trigger a measurement, in microseconds. 5800 is Scaling of the range finder's output pulse from microseconds to metres. 100000 Time to wait for a pulse from the range finder before giving up */ -RangeFinder rf(p26, 10, 5800.0, 100000); +RangeFinder rf(p21, 10, 5800.0, 100000); /* Thread Serial 1 - handles the output data from the control thread, and pass to the servo. @update s1, s2 */ void serial_thread(void const *args) { while (true) { - pc.scanf("%f,%f", &corHoriz, &corVert);// read from serial port the data // temporary Vert1 + + // mutexIn.lock(); + // mutexOut.lock(); + + //if not range finding + //mutex_sonar.lock(); + if(pc.readable()) + pc.scanf("%f,%f", &corHoriz, &corVert);// read from serial port the data + + //mutex_sonar.unlock(); + //mutexIn.unlock(); + //mutexOut.unlock(); + Thread::wait(1000); } } @@ -55,11 +67,19 @@ while (true) { mutex_sonar.lock(); // Display values on the LCD screen - lcd.cls(); // clear the display - lcd.locate(0,1); // the location where you want your charater to be displayed - lcd.printf("Sonar: %0.3f, OutTilt: %0.3f", sonar.read(), outTilt); - lcd.locate(0,9); // the location where you want your charater to be displayed + + // lcd.cls(); // clear the display + lcd.locate(0,0); // the location where you want your charater to be displayed + lcd.printf("differ: %0.3f, OutTilt: %0.3f", differ, outTilt); + + // lcd.cls(); // clear the display + lcd.locate(0,10); // the location where you want your charater to be displayed lcd.printf("Vert: %0.3f, OutVert: %0.3f", corVert, outVert); + + //lcd.cls(); // clear the display + lcd.locate(0,20); // the location where you want your charater to be displayed + lcd.printf("dis: %0.2f\n\r", distance);// Display the distance in meters from the sonar + mutex_sonar.unlock(); Thread::wait(250); mutex_sonar.unlock(); } @@ -67,10 +87,11 @@ /* Thread Control 3 - handles the input data from the sonar sensor, and display on the LCD screen. @update inData */ -void control_thread(void const *args) { +void control_thread(void const *args) +{ while (true) { mutexIn.lock(); - if (corVert > 0 && corVert < 1) { // if corVert is valid (between 0 - 1) then do movements + if (corVert > 0 && corVert < 1) { // if corVert is valid (between 0 - 1) then do movements // moves lamp down by the fraction of the difference from the middle if (corVert >= .5) { outVert = corVert - ((corVert - .5) * sonar); @@ -87,8 +108,8 @@ outHoriz = outHoriz + .05; search = search + .05; } else if (search > outHoriz && search > .1) { - outHoriz = - search = + outHoriz = + search = }*/ } mutexIn.unlock(); @@ -98,7 +119,8 @@ /* Thread Servo 4 - handles the output data from the control thread, and pass to the servo. @update s1, s2 */ -void servo_thread(void const *args) { +void servo_thread(void const *args) +{ while (true) { mutexOut.lock(); tiltServo = outTilt; @@ -109,25 +131,29 @@ } } -/* Thread sonar 5 - handles the sonar values which can be in meter or normailsed value to one */ -void sonar_thread(void const *args) { +/* Thread sonar 5 - handles the sonar values which can be in meter or normailsed value to one */ +void sonar_thread(void const *args) +{ while (true) { mutex_sonar.lock(); distance = rf.read_m(); // read the distance from the sonar sensor in meter norm= distance/3.3; // normalised value from the sonar sensor - printf("dis: %0.2f", distance);// Display the distance in meters from the sonar + // lcd.cls(); // clear the display + // lcd.locate(0,5); // the location where you want your charater to be displayed + printf("dis: %0.2f\n\r", distance);// Display the distance in meters from the sonar mutex_sonar.unlock(); Thread::wait(250); } } -int main() { +int main() +{ Thread thread_1(serial_thread); // Start Serial Thread Thread thread_2(lcd_thread); // Start LCD Thread Thread thread_3(control_thread); // Start Servo Thread Thread thread_4(servo_thread); // Start Servo Thread Thread thread_5(sonar_thread); // Start Servo Thread - while(1) { - wait(1); + while(1) { + Thread::wait(1); } }