WIT_EmbOS_Gr1 / Team_Sprint2

Code for Sprint 2

Dependencies:   C12832 mbed rtos RangeFinder

main.cpp@29:e8f7b891b5c1, 2015-04-27 (annotated)

Committer:
Argensis
Date:
Mon Apr 27 12:55:56 2015 +0000
Revision:
29:e8f7b891b5c1
Parent:
28:e04fb7a2a51e 
Adjusted limits so servos work and adjusted states

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