sonar sensor is working in the version of code but the reading are not correct
Dependencies: C12832 Pulse RangeFinder Servo mbed rtos
Fork of Team_Sprint2 by
Diff: main.cpp
- Revision:
- 22:5f6df7ae19a3
- Parent:
- 21:32270b453137
- Child:
- 23:f098ffcd192a
--- a/main.cpp Mon Apr 20 14:07:31 2015 +0000 +++ b/main.cpp Thu Apr 23 10:40:09 2015 +0000 @@ -21,7 +21,7 @@ float outVert; // output to vertical servo float outTilt; // output to tilt servo float outHoriz; // output to horizontal servo -float differ; // temporary global. Can be local. +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. @@ -29,7 +29,7 @@ void serial_thread(void const *args) { while (true) { - pc.scanf("%f,%f", &corHoriz, &corVert1);// read from serial port the data + pc.scanf("%f,%f", &corHoriz, &corVert1);// read from serial port the data // temporary Vert1 } } @@ -41,7 +41,7 @@ // 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("differ: %0.3f, OutTilt: %0.3f", differ, outTilt); + 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 @@ -56,24 +56,26 @@ { while (true) { mutexIn.lock(); - //float differ; - differ = ((exp(corVert + sonar * outTilt) / (1 + exp(corVert + sonar * outTilt))) - .49 ) * 2.5; 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 - outVert = (corVert * 2 * (1 - differ)); - // tilt down by the fraction of the difference from the middle - outTilt = (corVert * differ) + .15; - /*} else if (0 < corVert < 0.3) { - outVert = 0; - outTilt = corVert * .9; // Follow the low movement only with the tilt servo. - // (.9 is the correction of the side of the screen at OpenCV) - } else if (corVert > 0.7 && corVert < 1) { + 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 * 1.1; */ - } else { // Else this is the case when there is no input from the OpenCV - outVert = corVert; 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);