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@26:71288f42dbc6, 2015-04-23 (annotated)

Committer:
Argensis
Date:
Thu Apr 23 14:46:50 2015 +0000
Revision:
26:71288f42dbc6
Merge Ali's changes

Who changed what in which revision?

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