Code for Sprint 2
Dependencies: C12832 mbed rtos RangeFinder
main.cpp
- Committer:
- Soldier7
- Date:
- 2015-04-23
- Revision:
- 20:5f6df7ae19a3
- Parent:
- 19:32270b453137
- Child:
- 21:f098ffcd192a
File content as of revision 20:5f6df7ae19a3:
#include "mbed.h" #include "rtos.h" #include "Servo.h" #include "C12832.h" Servo tiltServo(p24); Servo panServo(p25); Serial pc(USBTX, USBRX); Servo vertServo(p23); AnalogIn sonar(p19); // temporary changed to potmeter AnalogIn corVert(p20); // temporary Mutex mutexIn; Mutex mutexOut; // Global variables float corHoriz = 0.5; // horizontal variable arrives from OpenCV //float corVert = 0.5; // vertical variable arrives from OpenCV float corVert1 = 0.5; // temporary float distance = 0.5; // distance from the sonar sensor float outVert; // output to vertical servo float outTilt; // output to tilt servo float outHoriz; // output to horizontal servo float search = .9; // set current position of pan servo to find the movement direction 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) { pc.scanf("%f,%f", &corHoriz, &corVert1);// read from serial port the data // temporary Vert1 } } /* Thread 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) { // 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.printf("Vert: %0.3f, OutVert: %0.3f", corVert.read(), outVert); //lcd.locate(0,18); // the location where you want your charater to be displayed //lcd.printf("Sonar: %0.3f", sonar.read()); Thread::wait(250); } } /* 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) { while (true) { mutexIn.lock(); 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); } else { outVert = corVert + ((.5 - corVert) * sonar); } outTilt = corVert; outHoriz = corHoriz; } else { // Else this is the case when there is no input from the OpenCV outVert = 1; outTilt = corVert; // TODO Pan search code here. (Searching personality.) /*if (search < outHoriz && search < .9) { outHoriz = outHoriz + .05; search = search + .05; } else if (search > outHoriz && search > .1) { outHoriz = search = }*/ } mutexIn.unlock(); Thread::wait(25); } } /* 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) { while (true) { mutexOut.lock(); tiltServo = outTilt; panServo = outHoriz; vertServo = outVert; mutexOut.unlock(); Thread::wait(200); } } 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 while(1) { wait(1); } }