WIT_EmbOS_Gr1 / Team_Sprint2

Code for Sprint 2

Dependencies:   C12832 mbed rtos RangeFinder

main.cpp@27:b1653e9bc81c, 2015-04-23 (annotated)

Committer:
Argensis
Date:
Thu Apr 23 16:50:27 2015 +0000
Revision:
27:b1653e9bc81c
Parent:
26:71288f42dbc6
Child:
28:e04fb7a2a51e
Changes for full state machine and clamped values.

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
Argensis 27:b1653e9bc81c 18 float corHoriz = -1; // horizontal variable arrives from OpenCV
Argensis 27:b1653e9bc81c 19 float corVert = -1; // 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 27:b1653e9bc81c 33 LampState currentTask = retreating;
Argensis 27:b1653e9bc81c 34
Argensis 27:b1653e9bc81c 35 time_t lastSearchMovementTime = time(NULL);
Argensis 27:b1653e9bc81c 36 time_t lastVisionEvent = time(NULL);
Argensis 27:b1653e9bc81c 37 time_t initialVisionTime = lastVisionEvent;
Argensis 23:7d29f2132197 38
Soldier7 21:f098ffcd192a 39 /* parallax ultrasound range finder
Ali_taher 16:af76305da577 40 p21 pin the range finder is connected to.
Ali_taher 16:af76305da577 41 10 is Time of pulse to send to the rangefinder to trigger a measurement, in microseconds.
Ali_taher 16:af76305da577 42 5800 is Scaling of the range finder's output pulse from microseconds to metres.
Soldier7 21:f098ffcd192a 43 100000 Time to wait for a pulse from the range finder before giving up */
Ali_taher 16:af76305da577 44
Ali_taher 25:582c5a0c868c 45 RangeFinder rf(p21, 10, 5800.0, 100000);
Soldier7 0:5edc27224a37 46 /* Thread Serial 1 - handles the output data from the control thread, and pass to the servo.
Soldier7 0:5edc27224a37 47 @update s1, s2 */
Soldier7 12:f6d4980c48d6 48 void serial_thread(void const *args)
Soldier7 12:f6d4980c48d6 49 {
Soldier7 0:5edc27224a37 50 while (true) {
Argensis 26:71288f42dbc6 51
Argensis 26:71288f42dbc6 52 // mutexIn.lock();
Argensis 26:71288f42dbc6 53 // mutexOut.lock();
Argensis 26:71288f42dbc6 54
Argensis 26:71288f42dbc6 55 //if not range finding
Argensis 26:71288f42dbc6 56 //mutex_sonar.lock();
Argensis 26:71288f42dbc6 57 if(pc.readable())
Argensis 26:71288f42dbc6 58 pc.scanf("%f,%f", &corHoriz, &corVert);// read from serial port the data
Argensis 26:71288f42dbc6 59
Argensis 26:71288f42dbc6 60 //mutex_sonar.unlock();
Argensis 26:71288f42dbc6 61 //mutexIn.unlock();
Argensis 26:71288f42dbc6 62 //mutexOut.unlock();
Ali_taher 25:582c5a0c868c 63 Thread::wait(1000);
Soldier7 0:5edc27224a37 64 }
Soldier7 0:5edc27224a37 65 }
Soldier7 12:f6d4980c48d6 66
Soldier7 0:5edc27224a37 67 /* Thread LCD 2 - handles the input data from the sonar sensor, and display on the LCD screen.
Soldier7 0:5edc27224a37 68 @update inData */
Soldier7 12:f6d4980c48d6 69 void lcd_thread(void const *args)
Soldier7 12:f6d4980c48d6 70 {
Soldier7 0:5edc27224a37 71 while (true) {
Ali_taher 16:af76305da577 72 mutex_sonar.lock();
Soldier7 0:5edc27224a37 73 // Display values on the LCD screen
Argensis 26:71288f42dbc6 74
Argensis 26:71288f42dbc6 75 // lcd.cls(); // clear the display
Argensis 26:71288f42dbc6 76 lcd.locate(0,0); // the location where you want your charater to be displayed
Argensis 27:b1653e9bc81c 77 lcd.printf("outTilt: %0.3f", outTilt);
Argensis 26:71288f42dbc6 78
Argensis 26:71288f42dbc6 79 // lcd.cls(); // clear the display
Argensis 26:71288f42dbc6 80 lcd.locate(0,10); // the location where you want your charater to be displayed
Argensis 27:b1653e9bc81c 81 //lcd.printf("Vert: %0.3f, OutVert: %0.3f", corVert, outVert);
Argensis 27:b1653e9bc81c 82 lcd.printf("OutHoriz: %0.3f", outHoriz);
Argensis 26:71288f42dbc6 83
Ali_taher 25:582c5a0c868c 84 //lcd.cls(); // clear the display
Ali_taher 3:3d53799c2f18 85 lcd.locate(0,20); // the location where you want your charater to be displayed
Ali_taher 25:582c5a0c868c 86 lcd.printf("dis: %0.2f\n\r", distance);// Display the distance in meters from the sonar
Ali_taher 25:582c5a0c868c 87 mutex_sonar.unlock();
Argensis 9:e5d24b7a921b 88 Thread::wait(250);
Argensis 23:7d29f2132197 89 mutex_sonar.unlock();
Soldier7 0:5edc27224a37 90 }
Soldier7 0:5edc27224a37 91 }
Soldier7 0:5edc27224a37 92
Argensis 27:b1653e9bc81c 93 void followPerson() {
Argensis 27:b1653e9bc81c 94 // moves lamp down by the fraction of the difference from the middle
Argensis 27:b1653e9bc81c 95 if (corVert >= .5) {
Argensis 27:b1653e9bc81c 96 outVert = corVert - ((corVert - .5) * sonar);
Argensis 27:b1653e9bc81c 97 } else {
Argensis 27:b1653e9bc81c 98 outVert = corVert + ((.5 - corVert) * sonar);
Argensis 27:b1653e9bc81c 99 }
Argensis 27:b1653e9bc81c 100 outTilt = corVert;
Argensis 27:b1653e9bc81c 101 outHoriz = corHoriz;
Argensis 27:b1653e9bc81c 102 }
Argensis 27:b1653e9bc81c 103
Argensis 27:b1653e9bc81c 104 void searchPerson() {
Argensis 27:b1653e9bc81c 105 time_t currentTime = time(NULL);
Argensis 27:b1653e9bc81c 106 if((currentTime - lastSearchMovementTime) > 10) {
Argensis 27:b1653e9bc81c 107 if(outHoriz < 0.5) {
Argensis 27:b1653e9bc81c 108 outHoriz = 0.8;
Argensis 27:b1653e9bc81c 109 } else {
Argensis 27:b1653e9bc81c 110 outHoriz = 0.3;
Argensis 27:b1653e9bc81c 111 }
Argensis 27:b1653e9bc81c 112 lastSearchMovementTime = currentTime;
Argensis 27:b1653e9bc81c 113 }
Argensis 27:b1653e9bc81c 114 }
Argensis 27:b1653e9bc81c 115
Argensis 27:b1653e9bc81c 116 void shooPerson() {
Argensis 27:b1653e9bc81c 117 outVert = 0.1;
Argensis 27:b1653e9bc81c 118 outTilt = 0.1;
Argensis 27:b1653e9bc81c 119 Thread::wait(5000);
Argensis 27:b1653e9bc81c 120 outTilt = 1;
Argensis 27:b1653e9bc81c 121 Thread::wait(5000);
Argensis 27:b1653e9bc81c 122 outTilt = 0.1;
Argensis 27:b1653e9bc81c 123 Thread::wait(5000);
Argensis 27:b1653e9bc81c 124 outTilt = 1;
Argensis 27:b1653e9bc81c 125 }
Argensis 27:b1653e9bc81c 126
Soldier7 0:5edc27224a37 127 /* Thread Control 3 - handles the input data from the sonar sensor, and display on the LCD screen.
Soldier7 0:5edc27224a37 128 @update inData */
Soldier7 12:f6d4980c48d6 129 void control_thread(void const *args)
Soldier7 12:f6d4980c48d6 130 {
Soldier7 0:5edc27224a37 131 while (true) {
Soldier7 0:5edc27224a37 132 mutexIn.lock();
Argensis 27:b1653e9bc81c 133 time_t currentTime = time(NULL);
Argensis 27:b1653e9bc81c 134 if((currentTime - lastVisionEvent) > 200) {
Argensis 27:b1653e9bc81c 135 currentTask = searching;
Argensis 27:b1653e9bc81c 136 } else if(distance <= 0.5) {
Argensis 27:b1653e9bc81c 137 currentTask = retreating;
Argensis 27:b1653e9bc81c 138 } else {
Argensis 27:b1653e9bc81c 139 currentTask = tracking;
Argensis 27:b1653e9bc81c 140 }
Argensis 27:b1653e9bc81c 141 switch(currentTask) {
Argensis 27:b1653e9bc81c 142 case tracking:
Argensis 27:b1653e9bc81c 143 followPerson();
Argensis 27:b1653e9bc81c 144 break;
Argensis 27:b1653e9bc81c 145 case searching:
Argensis 27:b1653e9bc81c 146 searchPerson();
Argensis 27:b1653e9bc81c 147 break;
Argensis 27:b1653e9bc81c 148 case retreating:
Argensis 27:b1653e9bc81c 149 shooPerson();
Argensis 27:b1653e9bc81c 150 break;
Soldier7 12:f6d4980c48d6 151 }
Soldier7 0:5edc27224a37 152 mutexIn.unlock();
Ali_taher 16:af76305da577 153 Thread::wait(250);
Soldier7 0:5edc27224a37 154 }
Soldier7 0:5edc27224a37 155 }
Soldier7 0:5edc27224a37 156
Soldier7 0:5edc27224a37 157 /* Thread Servo 4 - handles the output data from the control thread, and pass to the servo.
Soldier7 0:5edc27224a37 158 @update s1, s2 */
Soldier7 12:f6d4980c48d6 159 void servo_thread(void const *args)
Soldier7 12:f6d4980c48d6 160 {
Soldier7 0:5edc27224a37 161 while (true) {
Soldier7 0:5edc27224a37 162 mutexOut.lock();
Argensis 10:ca6f2769964e 163 tiltServo = outTilt;
Argensis 10:ca6f2769964e 164 panServo = outHoriz;
Argensis 10:ca6f2769964e 165 vertServo = outVert;
Soldier7 0:5edc27224a37 166 mutexOut.unlock();
Ali_taher 16:af76305da577 167 Thread::wait(250);
Soldier7 0:5edc27224a37 168 }
Soldier7 0:5edc27224a37 169 }
Soldier7 0:5edc27224a37 170
Argensis 26:71288f42dbc6 171 /* Thread sonar 5 - handles the sonar values which can be in meter or normailsed value to one */
Argensis 26:71288f42dbc6 172 void sonar_thread(void const *args)
Argensis 26:71288f42dbc6 173 {
Ali_taher 16:af76305da577 174 while (true) {
Ali_taher 16:af76305da577 175 mutex_sonar.lock();
Ali_taher 16:af76305da577 176 distance = rf.read_m(); // read the distance from the sonar sensor in meter
Ali_taher 16:af76305da577 177 norm= distance/3.3; // normalised value from the sonar sensor
Argensis 26:71288f42dbc6 178 // lcd.cls(); // clear the display
Argensis 26:71288f42dbc6 179 // lcd.locate(0,5); // the location where you want your charater to be displayed
Ali_taher 25:582c5a0c868c 180 printf("dis: %0.2f\n\r", distance);// Display the distance in meters from the sonar
Ali_taher 16:af76305da577 181 mutex_sonar.unlock();
Ali_taher 16:af76305da577 182 Thread::wait(250);
Ali_taher 16:af76305da577 183 }
Ali_taher 16:af76305da577 184 }
Argensis 17:974430ee2fbb 185
Argensis 26:71288f42dbc6 186 int main()
Argensis 26:71288f42dbc6 187 {
Soldier7 0:5edc27224a37 188 Thread thread_1(serial_thread); // Start Serial Thread
Soldier7 0:5edc27224a37 189 Thread thread_2(lcd_thread); // Start LCD Thread
Argensis 10:ca6f2769964e 190 Thread thread_3(control_thread); // Start Servo Thread
Soldier7 0:5edc27224a37 191 Thread thread_4(servo_thread); // Start Servo Thread
Ali_taher 16:af76305da577 192 Thread thread_5(sonar_thread); // Start Servo Thread
Argensis 26:71288f42dbc6 193 while(1) {
Ali_taher 25:582c5a0c868c 194 Thread::wait(1);
Soldier7 0:5edc27224a37 195 }
Soldier7 0:5edc27224a37 196 }