Code for Sprint 2
Dependencies: C12832 mbed rtos RangeFinder
Diff: main.cpp
- Revision:
- 10:ca6f2769964e
- Parent:
- 8:fe434a018d96
- Parent:
- 9:e5d24b7a921b
- Child:
- 13:4ef3d9835b13
- Child:
- 16:af76305da577
diff -r fe434a018d96 -r ca6f2769964e main.cpp --- a/main.cpp Mon Apr 13 12:43:43 2015 +0000 +++ b/main.cpp Mon Apr 13 13:01:02 2015 +0000 @@ -2,11 +2,12 @@ #include "rtos.h" #include "Servo.h" #include "C12832.h" -Servo sPan(p21); -Servo sTilt(p22); + +Servo tiltServo(p21); +Servo panServo(p22); Serial pc(USBTX, USBRX); -Servo sVert(p23); -Sonar distance(p24); +Servo vertServo(p23); +AnalogIn sonar(p19); Mutex mutexIn; Mutex mutexOut; @@ -14,9 +15,9 @@ AnalogIn p2(p20); // Global variables -float corHoriz; // horizontal variable arrives from OpenCV -float corVert; // vertical variable arrives from OpenCV -float distance; // distance measured by the sonar +float corHoriz = 0; // horizontal variable arrives from OpenCV +float corVert = 0; // vertical variable arrives from OpenCV +float distance = 0; float outVert; // rr float outTilt; float outHoriz; @@ -26,12 +27,7 @@ @update s1, s2 */ void serial_thread(void const *args) { while (true) { - mutexIn.lock(); - // pc.gets(cordinates,4); - // cordinates = pc.putc(pc.getc()); - pc.scanf("%s \n %s \n ",corHoriz,corVert);// read from serial port the data - mutexIn.unlock(); - Thread::wait(200); + pc.scanf("%f,%f", &corHoriz, &corVert);// read from serial port the data } } @@ -39,17 +35,13 @@ @update inData */ void lcd_thread(void const *args) { while (true) { - mutexIn.lock(); - mutexOut.lock(); // Display values on the LCD screen lcd.cls(); // clear the display lcd.locate(0,5); // the location where you want your charater to be displayed - lcd.printf("Hor:%s",corHoriz); + lcd.printf("Hor: %0.3f", corHoriz); lcd.locate(0,20); // the location where you want your charater to be displayed - lcd.printf("Ver%s", corVert); - mutexIn.unlock(); - mutexOut.unlock(); - Thread::wait(25); + lcd.printf("Ver: %0.3f", corVert); + Thread::wait(250); } } @@ -63,6 +55,8 @@ if (corVert > .5) { // check if face is below the half of the camera view if (outTilt > .5) { // check if lamp head is facing down // moves lamp down by the fraction of the difference from the middle + } + } mutexIn.unlock(); Thread::wait(25); } @@ -73,9 +67,9 @@ void servo_thread(void const *args) { while (true) { mutexOut.lock(); - sTi1t = outTilt; - sPan = outHoriz; - sVert = outVert; + tiltServo = outTilt; + panServo = outHoriz; + vertServo = outVert; mutexOut.unlock(); Thread::wait(200); } @@ -84,9 +78,9 @@ int main() { Thread thread_1(serial_thread); // Start Serial Thread Thread thread_2(lcd_thread); // Start LCD Thread - Thread thread_3(control_thread); // Start Control Thread + Thread thread_3(control_thread); // Start Servo Thread Thread thread_4(servo_thread); // Start Servo Thread while(1) { - Thread::wait(10); + wait(1); } }