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?
User | Revision | Line number | New 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 | } |