WIT_EmbOS_Gr1 / Team_Sprint2

Important changes to repositories hosted on mbed.com

Mbed hosted mercurial repositories are deprecated and are due to be permanently deleted in July 2026.

To keep a copy of this software download the repository Zip archive or clone locally using Mercurial.

It is also possible to export all your personal repositories from the account settings page.

Dependencies:   C12832 mbed rtos RangeFinder

main.cpp.orig@19:32270b453137, 2015-04-20 (annotated)

Committer:
Soldier7
Date:
Mon Apr 20 14:07:31 2015 +0000
Revision:
19:32270b453137
The tilt and the vertical servo now approximately moves together with the algorithm.

Who changed what in which revision?

UserRevisionLine numberNew contents of line
Soldier7 19:32270b453137 1 #include "mbed.h"
Soldier7 19:32270b453137 2 #include "rtos.h"
Soldier7 19:32270b453137 3 #include "Servo.h"
Soldier7 19:32270b453137 4 #include "C12832.h"
Soldier7 19:32270b453137 5 #include "RangeFinder.h"// header files for sonar sensor
Soldier7 19:32270b453137 6
Soldier7 19:32270b453137 7
Soldier7 19:32270b453137 8 Servo tiltServo(p24);
Soldier7 19:32270b453137 9 Servo panServo(p25);
Soldier7 19:32270b453137 10 Serial pc(USBTX, USBRX);
Soldier7 19:32270b453137 11 Servo vertServo(p23);
Soldier7 19:32270b453137 12
Soldier7 19:32270b453137 13 Mutex mutexIn;// protect globel variables
Soldier7 19:32270b453137 14 Mutex mutexOut;// protect globel variables
Soldier7 19:32270b453137 15 Mutex mutex_sonar;
Soldier7 19:32270b453137 16 AnalogIn sonar(p19); // temporary changed to potmeter
Soldier7 19:32270b453137 17 Mutex mutexIn;
Soldier7 19:32270b453137 18 Mutex mutexOut;
Soldier7 19:32270b453137 19
Soldier7 19:32270b453137 20 // Global variables
Soldier7 19:32270b453137 21 float corHoriz = 0; // horizontal variable arrives from OpenCV
Soldier7 19:32270b453137 22 float corVert = 0; // vertical variable arrives from OpenCV
Soldier7 19:32270b453137 23 float distance = 0;// variable holds the distance in meters 0 to 3.3
Soldier7 19:32270b453137 24 float norm=0; // variable holds the normalised values form the sonar sensor
Soldier7 19:32270b453137 25 float outVert; // output to vertical servo
Soldier7 19:32270b453137 26 float outTilt; // output to tilt servo
Soldier7 19:32270b453137 27 float outHoriz; // output to horizontal servo
Soldier7 19:32270b453137 28 C12832 lcd(p5, p7, p6, p8, p11);// lcd is an object from class c12832 initialised by p5,p7....
Soldier7 19:32270b453137 29 float differ; // temporary global. Can be local.
Soldier7 19:32270b453137 30
Soldier7 19:32270b453137 31
Soldier7 19:32270b453137 32 /*parallax ultrasound range finder
Soldier7 19:32270b453137 33 p21 pin the range finder is connected to.
Soldier7 19:32270b453137 34 10 is Time of pulse to send to the rangefinder to trigger a measurement, in microseconds.
Soldier7 19:32270b453137 35 5800 is Scaling of the range finder's output pulse from microseconds to metres.
Soldier7 19:32270b453137 36 100000 Time to wait for a pulse from the range finder before giving up
Soldier7 19:32270b453137 37 */
Soldier7 19:32270b453137 38
Soldier7 19:32270b453137 39 RangeFinder rf(p26, 10, 5800.0, 100000);
Soldier7 19:32270b453137 40 /* Thread Serial 1 - handles the output data from the control thread, and pass to the servo.
Soldier7 19:32270b453137 41 @update s1, s2 */
Soldier7 19:32270b453137 42 void serial_thread(void const *args)
Soldier7 19:32270b453137 43 {
Soldier7 19:32270b453137 44 while (true) {
Soldier7 19:32270b453137 45 pc.scanf("%f,%f", &corHoriz, &corVert);// read from serial port the data
Soldier7 19:32270b453137 46 }
Soldier7 19:32270b453137 47 }
Soldier7 19:32270b453137 48
Soldier7 19:32270b453137 49 /* Thread LCD 2 - handles the input data from the sonar sensor, and display on the LCD screen.
Soldier7 19:32270b453137 50 @update inData */
Soldier7 19:32270b453137 51 void lcd_thread(void const *args)
Soldier7 19:32270b453137 52 {
Soldier7 19:32270b453137 53 while (true) {
Soldier7 19:32270b453137 54 mutex_sonar.lock();
Soldier7 19:32270b453137 55 // Display values on the LCD screen
Soldier7 19:32270b453137 56 lcd.cls(); // clear the display
Soldier7 19:32270b453137 57 lcd.locate(0,5); // the location where you want your charater to be displayed
Soldier7 19:32270b453137 58
Soldier7 19:32270b453137 59 lcd.printf("differ: %0.3f, OutTilt: %0.3f", differ, outTilt);
Soldier7 19:32270b453137 60 lcd.locate(0,20); // the location where you want your charater to be displayed
Soldier7 19:32270b453137 61 lcd.printf("Vert: %0.3f, OutVert: %0.3f", corVert, outVert);
Soldier7 19:32270b453137 62 Thread::wait(250);
Soldier7 19:32270b453137 63 }
Soldier7 19:32270b453137 64 }
Soldier7 19:32270b453137 65
Soldier7 19:32270b453137 66 /* Thread Control 3 - handles the input data from the sonar sensor, and display on the LCD screen.
Soldier7 19:32270b453137 67 @update inData */
Soldier7 19:32270b453137 68 void control_thread(void const *args)
Soldier7 19:32270b453137 69 {
Soldier7 19:32270b453137 70 while (true) {
Soldier7 19:32270b453137 71 mutexIn.lock();
Soldier7 19:32270b453137 72 //float differ;
Soldier7 19:32270b453137 73 differ = exp(corVert + sonar * (outTilt / 20)) / (1 + exp(corVert + sonar * (outTilt / 20)));
Soldier7 19:32270b453137 74 if (corVert > 0.1 && corVert < 0.9) { // if corVert is valid (between 0 - 1) then do movements
Soldier7 19:32270b453137 75 // moves lamp down by the fraction of the difference from the middle
Soldier7 19:32270b453137 76 outVert = corVert * (1 - differ);
Soldier7 19:32270b453137 77 // tilt down by the fraction of the difference from the middle
Soldier7 19:32270b453137 78 outTilt = corVert * differ; /*
Soldier7 19:32270b453137 79 } else if (0 < corVert < 0.3) {
Soldier7 19:32270b453137 80 outVert = 0;
Soldier7 19:32270b453137 81 outTilt = corVert * .9; // Follow the low movement only with the tilt servo.
Soldier7 19:32270b453137 82 // (.9 is the correction of the side of the screen at OpenCV)
Soldier7 19:32270b453137 83 } else if (corVert > 0.7 && corVert < 1) {
Soldier7 19:32270b453137 84 outVert = 1;
Soldier7 19:32270b453137 85 outTilt = corVert * 1.1; */
Soldier7 19:32270b453137 86 } else { // Else this is the case when there is no input from the OpenCV
Soldier7 19:32270b453137 87 outVert = .5;
Soldier7 19:32270b453137 88 outTilt = corVert * differ;
Soldier7 19:32270b453137 89 // TODO Pan search code here. (Searching personality.)
Soldier7 19:32270b453137 90 }
Soldier7 19:32270b453137 91 mutexIn.unlock();
Soldier7 19:32270b453137 92 Thread::wait(250);
Soldier7 19:32270b453137 93 }
Soldier7 19:32270b453137 94 }
Soldier7 19:32270b453137 95
Soldier7 19:32270b453137 96 /* Thread Servo 4 - handles the output data from the control thread, and pass to the servo.
Soldier7 19:32270b453137 97 @update s1, s2 */
Soldier7 19:32270b453137 98 void servo_thread(void const *args)
Soldier7 19:32270b453137 99 {
Soldier7 19:32270b453137 100 while (true) {
Soldier7 19:32270b453137 101 mutexOut.lock();
Soldier7 19:32270b453137 102 tiltServo = outTilt;
Soldier7 19:32270b453137 103 panServo = outHoriz;
Soldier7 19:32270b453137 104 vertServo = outVert;
Soldier7 19:32270b453137 105 mutexOut.unlock();
Soldier7 19:32270b453137 106 Thread::wait(250);
Soldier7 19:32270b453137 107 }
Soldier7 19:32270b453137 108 }
Soldier7 19:32270b453137 109
Soldier7 19:32270b453137 110 /* Thread sonar 5 - handles the sonar values which can be in meter or normailsed value to one */
Soldier7 19:32270b453137 111 void sonar_thread(void const *args) {
Soldier7 19:32270b453137 112 while (true) {
Soldier7 19:32270b453137 113 mutex_sonar.lock();
Soldier7 19:32270b453137 114 distance = rf.read_m(); // read the distance from the sonar sensor in meter
Soldier7 19:32270b453137 115 norm= distance/3.3; // normalised value from the sonar sensor
Soldier7 19:32270b453137 116 printf("dis: %0.2f", distance);// Display the distance in meters from the sonar
Soldier7 19:32270b453137 117 mutex_sonar.unlock();
Soldier7 19:32270b453137 118 Thread::wait(250);
Soldier7 19:32270b453137 119 }
Soldier7 19:32270b453137 120 }
Soldier7 19:32270b453137 121
Soldier7 19:32270b453137 122 int main() {
Soldier7 19:32270b453137 123 Thread thread_1(serial_thread); // Start Serial Thread
Soldier7 19:32270b453137 124 Thread thread_2(lcd_thread); // Start LCD Thread
Soldier7 19:32270b453137 125 Thread thread_3(control_thread); // Start Servo Thread
Soldier7 19:32270b453137 126 Thread thread_4(servo_thread); // Start Servo Thread
Soldier7 19:32270b453137 127 Thread thread_5(sonar_thread); // Start Servo Thread
Soldier7 19:32270b453137 128 while(1) {
Soldier7 19:32270b453137 129 wait(1);
Soldier7 19:32270b453137 130 }
Soldier7 19:32270b453137 131 }