Code for Sprint 2
Dependencies: C12832 mbed rtos RangeFinder
main.cpp
- Committer:
- Soldier7
- Date:
- 2015-03-14
- Revision:
- 6:dc51d320b369
- Parent:
- 5:4354b585dcab
File content as of revision 6:dc51d320b369:
#include "mbed.h" #include "rtos.h" #include "Servo.h" #include "C12832.h" Servo sHoriz(p21); Servo sVert(p22); Serial pc(USBTX, USBRX); Mutex mutexIn; Mutex mutexOut; /* Modification changes based on http://lampbotics.blogspot.ie/2015/03/basic-serial-data.html */ // Globel variables float corHoriz, corVert, outVert, outHoriz; C12832 lcd(p5, p7, p6, p8, p11);// lcd is an object from class c12832 initialised by p5,p7.... /* 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) { mutexIn.lock(); pc.scanf("%f,%f", &corHoriz, &corVert); mutexIn.unlock(); Thread::wait(50); } } /* Thread Calc and LCD 2 - handles the input data from the sonar sensor, and display on the LCD screen. @update inData */ void lcd_thread(void const *args) { while (true) { mutexIn.lock(); mutexOut.lock(); // Calculate servo movement data arrived from openCV // This case openCV send 5+ if need to increase the value of cordinates outHoriz = outHoriz + ((corHoriz - 5) / 1000); outVert = outVert + ((corVert - 5) / 1000); // Display values on the LCD screen lcd.cls(); // clear the display lcd.locate(0,3); // the location where you want your charater to be displayed lcd.printf("Cor Hor: %.3f, Vert: %.3f", corHoriz, corVert); lcd.printf("Out Hor: %.3f, Vert: %.3f", outHoriz, outVert); lcd.printf("Servo Hor: %f, Vert: %f", sHoriz, sVert); mutexIn.unlock(); mutexOut.unlock(); Thread::wait(50); } } /* Thread Servo 3 - handles the output data from the control thread, and pass to the servo. @update s1, s2 */ void servo_thread(void const *args) { while (true) { mutexOut.lock(); sVert = outVert; sHoriz = outHoriz; mutexOut.unlock(); Thread::wait(50); } } int main() { Thread thread_1(serial_thread); // Start Serial Thread Thread thread_2(lcd_thread); // Start LCD Thread Thread thread_3(servo_thread); // Start Servo Thread while(1) { Thread::wait(10); } }