WIT_EmbOS_Gr1 / Team_Sprint2

Code for Sprint 2

Dependencies:   C12832 mbed rtos RangeFinder

main.cpp@26:71288f42dbc6, 2015-04-23 (annotated)

Committer:
Argensis
Date:
Thu Apr 23 14:46:50 2015 +0000
Revision:
26:71288f42dbc6
Parent:
23:7d29f2132197
Parent:
25:582c5a0c868c
Child:
27:b1653e9bc81c
Merge Ali's changes

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