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
Revision 21:32270b453137, committed 2015-04-20
- Comitter:
- Soldier7
- Date:
- Mon Apr 20 14:07:31 2015 +0000
- Parent:
- 20:5dfebb54cdbd
- Child:
- 22:5f6df7ae19a3
- Commit message:
- The tilt and the vertical servo now approximately moves together with the algorithm.
Changed in this revision
main.cpp | Show annotated file Show diff for this revision Revisions of this file |
main.cpp.orig | Show annotated file Show diff for this revision Revisions of this file |
--- a/main.cpp Mon Apr 20 13:34:28 2015 +0000 +++ b/main.cpp Mon Apr 20 14:07:31 2015 +0000 @@ -60,9 +60,9 @@ 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 * (1 - differ)) + .5; + outVert = (corVert * 2 * (1 - differ)); // tilt down by the fraction of the difference from the middle - outTilt = corVert * differ; + outTilt = (corVert * differ) + .15; /*} else if (0 < corVert < 0.3) { outVert = 0; outTilt = corVert * .9; // Follow the low movement only with the tilt servo.
--- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/main.cpp.orig Mon Apr 20 14:07:31 2015 +0000 @@ -0,0 +1,131 @@ +#include "mbed.h" +#include "rtos.h" +#include "Servo.h" +#include "C12832.h" +#include "RangeFinder.h"// header files for sonar sensor + + +Servo tiltServo(p24); +Servo panServo(p25); +Serial pc(USBTX, USBRX); +Servo vertServo(p23); + +Mutex mutexIn;// protect globel variables +Mutex mutexOut;// protect globel variables +Mutex mutex_sonar; +AnalogIn sonar(p19); // temporary changed to potmeter +Mutex mutexIn; +Mutex mutexOut; + +// Global variables +float corHoriz = 0; // horizontal variable arrives from OpenCV +float corVert = 0; // vertical variable arrives from OpenCV +float distance = 0;// 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 +float outTilt; // output to tilt servo +float outHoriz; // output to horizontal servo +C12832 lcd(p5, p7, p6, p8, p11);// lcd is an object from class c12832 initialised by p5,p7.... +float differ; // temporary global. Can be local. + + +/*parallax ultrasound range finder +p21 pin the range finder is connected to. +10 is Time of pulse to send to the rangefinder to trigger a measurement, in microseconds. +5800 is Scaling of the range finder's output pulse from microseconds to metres. +100000 Time to wait for a pulse from the range finder before giving up +*/ + +RangeFinder rf(p26, 10, 5800.0, 100000); +/* 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, &corVert);// read from serial port the data + } +} + +/* 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) { + mutex_sonar.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("differ: %0.3f, OutTilt: %0.3f", differ, outTilt); + lcd.locate(0,20); // the location where you want your charater to be displayed + lcd.printf("Vert: %0.3f, OutVert: %0.3f", corVert, outVert); + 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(); + //float differ; + differ = exp(corVert + sonar * (outTilt / 20)) / (1 + exp(corVert + sonar * (outTilt / 20))); + if (corVert > 0.1 && corVert < 0.9) { // if corVert is valid (between 0 - 1) then do movements + // moves lamp down by the fraction of the difference from the middle + outVert = corVert * (1 - differ); + // tilt down by the fraction of the difference from the middle + outTilt = corVert * differ; /* + } 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) { + outVert = 1; + outTilt = corVert * 1.1; */ + } else { // Else this is the case when there is no input from the OpenCV + outVert = .5; + outTilt = corVert * differ; + // TODO Pan search code here. (Searching personality.) + } + mutexIn.unlock(); + Thread::wait(250); + } +} + +/* 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(250); + } +} + +/* Thread sonar 5 - handles the sonar values which can be in meter or normailsed value to one */ +void sonar_thread(void const *args) { + while (true) { + mutex_sonar.lock(); + distance = rf.read_m(); // read the distance from the sonar sensor in meter + norm= distance/3.3; // normalised value from the sonar sensor + printf("dis: %0.2f", distance);// Display the distance in meters from the sonar + mutex_sonar.unlock(); + Thread::wait(250); + } +} + +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 + Thread thread_5(sonar_thread); // Start Servo Thread + while(1) { + wait(1); + } +}