Ali Alshaqaq / Team_Sprint2

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 WIT_EmbOS_Gr1

main.cpp@17:4730277de7c2, 2015-04-20 (annotated)

Committer:
Soldier7
Date:
Mon Apr 20 12:02:45 2015 +0000
Revision:
17:4730277de7c2
Parent:
16:74733a28eb80
Child:
18:974430ee2fbb
Child:
20:5dfebb54cdbd
An other trial version. Still doesn't work.

Who changed what in which revision?

UserRevisionLine numberNew contents of line
Soldier7 0:5edc27224a37 1 #include "mbed.h"
Soldier7 0:5edc27224a37 2 #include "rtos.h"
Soldier7 0:5edc27224a37 3 #include "Servo.h"
Soldier7 0:5edc27224a37 4 #include "C12832.h"
Argensis 9:e5d24b7a921b 5
Soldier7 16:74733a28eb80 6 Servo tiltServo(p24);
Soldier7 16:74733a28eb80 7 Servo panServo(p25);
Soldier7 0:5edc27224a37 8 Serial pc(USBTX, USBRX);
Argensis 10:ca6f2769964e 9 Servo vertServo(p23);
Soldier7 17:4730277de7c2 10
Soldier7 16:74733a28eb80 11 AnalogIn sonar(p19); // temporary changed to potmeter
Soldier7 0:5edc27224a37 12 Mutex mutexIn;
Soldier7 0:5edc27224a37 13 Mutex mutexOut;
Soldier7 0:5edc27224a37 14
Soldier7 8:fe434a018d96 15 // Global variables
Soldier7 17:4730277de7c2 16 float corHoriz = 0.5; // horizontal variable arrives from OpenCV
Soldier7 17:4730277de7c2 17 float corVert = 0.5; // vertical variable arrives from OpenCV
Soldier7 17:4730277de7c2 18 float distance = 0.5; // distance from the sonar sensor
Soldier7 16:74733a28eb80 19 float outVert; // output to vertical servo
Soldier7 16:74733a28eb80 20 float outTilt; // output to tilt servo
Soldier7 16:74733a28eb80 21 float outHoriz; // output to horizontal servo
Soldier7 17:4730277de7c2 22 float differ; // temporary global. Can be local.
Soldier7 16:74733a28eb80 23 C12832 lcd(p5, p7, p6, p8, p11);// lcd is an object from class c12832 initialised by p5, p7....
Soldier7 0:5edc27224a37 24
Soldier7 0:5edc27224a37 25 /* Thread Serial 1 - handles the output data from the control thread, and pass to the servo.
Soldier7 0:5edc27224a37 26 @update s1, s2 */
Soldier7 14:f6d4980c48d6 27 void serial_thread(void const *args)
Soldier7 14:f6d4980c48d6 28 {
Soldier7 0:5edc27224a37 29 while (true) {
Argensis 9:e5d24b7a921b 30 pc.scanf("%f,%f", &corHoriz, &corVert);// read from serial port the data
Soldier7 0:5edc27224a37 31 }
Soldier7 0:5edc27224a37 32 }
Soldier7 14:f6d4980c48d6 33
Soldier7 0:5edc27224a37 34 /* Thread LCD 2 - handles the input data from the sonar sensor, and display on the LCD screen.
Soldier7 0:5edc27224a37 35 @update inData */
Soldier7 14:f6d4980c48d6 36 void lcd_thread(void const *args)
Soldier7 14:f6d4980c48d6 37 {
Soldier7 0:5edc27224a37 38 while (true) {
Soldier7 0:5edc27224a37 39 // Display values on the LCD screen
Soldier7 0:5edc27224a37 40 lcd.cls(); // clear the display
Soldier7 4:70090f3b1f07 41 lcd.locate(0,5); // the location where you want your charater to be displayed
Soldier7 17:4730277de7c2 42 lcd.printf("differ: %0.3f, OutTilt: %0.3f", differ, outTilt);
Ali_taher 3:3d53799c2f18 43 lcd.locate(0,20); // the location where you want your charater to be displayed
Soldier7 16:74733a28eb80 44 lcd.printf("Vert: %0.3f, OutVert: %0.3f", corVert, outVert);
Argensis 9:e5d24b7a921b 45 Thread::wait(250);
Soldier7 0:5edc27224a37 46 }
Soldier7 0:5edc27224a37 47 }
Soldier7 0:5edc27224a37 48
Soldier7 0:5edc27224a37 49 /* Thread Control 3 - handles the input data from the sonar sensor, and display on the LCD screen.
Soldier7 0:5edc27224a37 50 @update inData */
Soldier7 14:f6d4980c48d6 51 void control_thread(void const *args)
Soldier7 14:f6d4980c48d6 52 {
Soldier7 0:5edc27224a37 53 while (true) {
Soldier7 0:5edc27224a37 54 mutexIn.lock();
Soldier7 17:4730277de7c2 55 //float differ;
Soldier7 17:4730277de7c2 56 differ = exp(corVert + sonar * (outTilt / 20)) / (1 + exp(corVert + sonar * (outTilt / 20)));
Soldier7 17:4730277de7c2 57 if (corVert > 0.1 && corVert < 0.9) { // if corVert is valid (between 0 - 1) then do movements
Soldier7 14:f6d4980c48d6 58 // moves lamp down by the fraction of the difference from the middle
Soldier7 17:4730277de7c2 59 outVert = corVert * (1 - differ);
Soldier7 16:74733a28eb80 60 // tilt down by the fraction of the difference from the middle
Soldier7 17:4730277de7c2 61 outTilt = corVert * differ; /*
Soldier7 16:74733a28eb80 62 } else if (0 < corVert < 0.3) {
Soldier7 16:74733a28eb80 63 outVert = 0;
Soldier7 16:74733a28eb80 64 outTilt = corVert * .9; // Follow the low movement only with the tilt servo.
Soldier7 16:74733a28eb80 65 // (.9 is the correction of the side of the screen at OpenCV)
Soldier7 16:74733a28eb80 66 } else if (corVert > 0.7 && corVert < 1) {
Soldier7 16:74733a28eb80 67 outVert = 1;
Soldier7 17:4730277de7c2 68 outTilt = corVert * 1.1; */
Soldier7 16:74733a28eb80 69 } else { // Else this is the case when there is no input from the OpenCV
Soldier7 16:74733a28eb80 70 outVert = .5;
Soldier7 17:4730277de7c2 71 outTilt = corVert * differ;
Soldier7 16:74733a28eb80 72 // TODO Pan search code here. (Searching personality.)
Soldier7 14:f6d4980c48d6 73 }
Soldier7 0:5edc27224a37 74 mutexIn.unlock();
Soldier7 0:5edc27224a37 75 Thread::wait(25);
Soldier7 0:5edc27224a37 76 }
Soldier7 0:5edc27224a37 77 }
Soldier7 0:5edc27224a37 78
Soldier7 0:5edc27224a37 79 /* Thread Servo 4 - handles the output data from the control thread, and pass to the servo.
Soldier7 0:5edc27224a37 80 @update s1, s2 */
Soldier7 14:f6d4980c48d6 81 void servo_thread(void const *args)
Soldier7 14:f6d4980c48d6 82 {
Soldier7 0:5edc27224a37 83 while (true) {
Soldier7 0:5edc27224a37 84 mutexOut.lock();
Argensis 10:ca6f2769964e 85 tiltServo = outTilt;
Argensis 10:ca6f2769964e 86 panServo = outHoriz;
Argensis 10:ca6f2769964e 87 vertServo = outVert;
Soldier7 0:5edc27224a37 88 mutexOut.unlock();
Soldier7 0:5edc27224a37 89 Thread::wait(200);
Soldier7 0:5edc27224a37 90 }
Soldier7 0:5edc27224a37 91 }
Soldier7 0:5edc27224a37 92
Soldier7 14:f6d4980c48d6 93 int main()
Soldier7 14:f6d4980c48d6 94 {
Soldier7 0:5edc27224a37 95 Thread thread_1(serial_thread); // Start Serial Thread
Soldier7 0:5edc27224a37 96 Thread thread_2(lcd_thread); // Start LCD Thread
Argensis 10:ca6f2769964e 97 Thread thread_3(control_thread); // Start Servo Thread
Soldier7 0:5edc27224a37 98 Thread thread_4(servo_thread); // Start Servo Thread
Soldier7 0:5edc27224a37 99 while(1) {
Argensis 9:e5d24b7a921b 100 wait(1);
Soldier7 0:5edc27224a37 101 }
Soldier7 0:5edc27224a37 102 }