WIT_EmbOS_Gr1 / Team_Sprint2

Code for Sprint 2

Dependencies:   C12832 mbed rtos RangeFinder

main.cpp@28:e04fb7a2a51e, 2015-04-24 (annotated)

Committer:
Argensis
Date:
Fri Apr 24 10:29:27 2015 +0000
Revision:
28:e04fb7a2a51e
Parent:
27:b1653e9bc81c
Child:
29:e8f7b891b5c1
Drop Servo library, use PwmOut, limit output values to 0.01 to 0.1

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