Code for Sprint 2
Dependencies: C12832 mbed rtos RangeFinder
Diff: main.cpp
- Revision:
- 27:b1653e9bc81c
- Parent:
- 26:71288f42dbc6
- Child:
- 28:e04fb7a2a51e
diff -r 71288f42dbc6 -r b1653e9bc81c main.cpp --- a/main.cpp Thu Apr 23 14:46:50 2015 +0000 +++ b/main.cpp Thu Apr 23 16:50:27 2015 +0000 @@ -15,8 +15,8 @@ //AnalogIn corVert(p20); // TO REMOVE. Temporary changed to potmeter // Global variables -float corHoriz = 0.5; // horizontal variable arrives from OpenCV -float corVert = 0.5; // vertical variable arrives from OpenCV +float corHoriz = -1; // horizontal variable arrives from OpenCV +float corVert = -1; // vertical variable arrives from OpenCV float distance = 0.5;// variable holds the distance in meters 0 to 3.3 float norm=0; // variable holds the normalised values form the sonar sensor float outVert; // output to vertical servo @@ -30,7 +30,11 @@ retreating } LampState; -LampState lamp; +LampState currentTask = retreating; + +time_t lastSearchMovementTime = time(NULL); +time_t lastVisionEvent = time(NULL); +time_t initialVisionTime = lastVisionEvent; /* parallax ultrasound range finder p21 pin the range finder is connected to. @@ -70,11 +74,12 @@ // 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.printf("outTilt: %0.3f", 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.printf("Vert: %0.3f, OutVert: %0.3f", corVert, outVert); + lcd.printf("OutHoriz: %0.3f", outHoriz); //lcd.cls(); // clear the display lcd.locate(0,20); // the location where you want your charater to be displayed @@ -85,32 +90,64 @@ } } +void followPerson() { + // 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; +} + +void searchPerson() { + time_t currentTime = time(NULL); + if((currentTime - lastSearchMovementTime) > 10) { + if(outHoriz < 0.5) { + outHoriz = 0.8; + } else { + outHoriz = 0.3; + } + lastSearchMovementTime = currentTime; + } +} + +void shooPerson() { + outVert = 0.1; + outTilt = 0.1; + Thread::wait(5000); + outTilt = 1; + Thread::wait(5000); + outTilt = 0.1; + Thread::wait(5000); + outTilt = 1; +} + /* 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 = - }*/ + time_t currentTime = time(NULL); + if((currentTime - lastVisionEvent) > 200) { + currentTask = searching; + } else if(distance <= 0.5) { + currentTask = retreating; + } else { + currentTask = tracking; + } + switch(currentTask) { + case tracking: + followPerson(); + break; + case searching: + searchPerson(); + break; + case retreating: + shooPerson(); + break; } mutexIn.unlock(); Thread::wait(250);