The controller for a NERF turret, allowing it to track and fire at a human face with visual input gotten by polling a web server. This is part of the final project of the EE149 course of UC Berkeley. The project name is "Pew Pew".
Dependencies: HTTPClient TSI cc3000_hostdriver_mbedsocket mbed
main.cpp@0:4a30986db2fb, 2014-12-19 (annotated)
- Committer:
- impguard
- Date:
- Fri Dec 19 11:49:27 2014 +0000
- Revision:
- 0:4a30986db2fb
Final version of the controller for the NERF turret.
Who changed what in which revision?
User | Revision | Line number | New contents of line |
---|---|---|---|
impguard | 0:4a30986db2fb | 1 | /** |
impguard | 0:4a30986db2fb | 2 | * |
impguard | 0:4a30986db2fb | 3 | * Controller for the NERF turret, allowing for tracking of target (human face) via visual input. |
impguard | 0:4a30986db2fb | 4 | * |
impguard | 0:4a30986db2fb | 5 | * authors: Leo Kam |
impguard | 0:4a30986db2fb | 6 | **/ |
impguard | 0:4a30986db2fb | 7 | |
impguard | 0:4a30986db2fb | 8 | #include "mbed.h" |
impguard | 0:4a30986db2fb | 9 | #include "camera.h" |
impguard | 0:4a30986db2fb | 10 | #include "TSISensor.h" |
impguard | 0:4a30986db2fb | 11 | |
impguard | 0:4a30986db2fb | 12 | //Relays for firing the NERF gun. |
impguard | 0:4a30986db2fb | 13 | DigitalOut flywheel(PTE5); |
impguard | 0:4a30986db2fb | 14 | DigitalOut trigger(PTE4); |
impguard | 0:4a30986db2fb | 15 | //Relays for turning the turret. |
impguard | 0:4a30986db2fb | 16 | DigitalOut step1(PTE2); |
impguard | 0:4a30986db2fb | 17 | DigitalOut step2(PTE3); |
impguard | 0:4a30986db2fb | 18 | //Relays for tilting the gun up and down. |
impguard | 0:4a30986db2fb | 19 | DigitalOut h_bridge1(PTB8); |
impguard | 0:4a30986db2fb | 20 | DigitalOut h_bridge2(PTB9); |
impguard | 0:4a30986db2fb | 21 | DigitalOut h_bridge3(PTB10); |
impguard | 0:4a30986db2fb | 22 | DigitalOut h_bridge4(PTB11); |
impguard | 0:4a30986db2fb | 23 | |
impguard | 0:4a30986db2fb | 24 | |
impguard | 0:4a30986db2fb | 25 | double x, y, z; //The coordinates of the target, relative to the barrel of the gun. |
impguard | 0:4a30986db2fb | 26 | double x_offset, y_offset, z_offset; //The offsets to transform the camera data into a coordinate system relative to barrel of the gun. |
impguard | 0:4a30986db2fb | 27 | TSISensor tsi; //The touch sensor of the mbed. |
impguard | 0:4a30986db2fb | 28 | double eps = 40; //The error margin. |
impguard | 0:4a30986db2fb | 29 | double interval = 200; //The time between each poll of the server, in ms. |
impguard | 0:4a30986db2fb | 30 | bool safety = true; //Whether the safety of the NERF gun is on or off. |
impguard | 0:4a30986db2fb | 31 | |
impguard | 0:4a30986db2fb | 32 | void initialize(); |
impguard | 0:4a30986db2fb | 33 | void calibrateCamera(); |
impguard | 0:4a30986db2fb | 34 | bool getCoordinates(double&, double&, double&); |
impguard | 0:4a30986db2fb | 35 | bool onTarget(double); |
impguard | 0:4a30986db2fb | 36 | void stepMotorStates(int state); |
impguard | 0:4a30986db2fb | 37 | void turnLeft(double); |
impguard | 0:4a30986db2fb | 38 | void turnRight(double); |
impguard | 0:4a30986db2fb | 39 | void tiltUp(double); |
impguard | 0:4a30986db2fb | 40 | void tiltDown(double); |
impguard | 0:4a30986db2fb | 41 | void trackTarget(); |
impguard | 0:4a30986db2fb | 42 | void safetyOn(); |
impguard | 0:4a30986db2fb | 43 | void safetyOff(); |
impguard | 0:4a30986db2fb | 44 | bool fire(); |
impguard | 0:4a30986db2fb | 45 | |
impguard | 0:4a30986db2fb | 46 | int main() { |
impguard | 0:4a30986db2fb | 47 | initialize(); |
impguard | 0:4a30986db2fb | 48 | double hysteresis = 0; |
impguard | 0:4a30986db2fb | 49 | while(1) { |
impguard | 0:4a30986db2fb | 50 | getCoordinates(x, y, z); |
impguard | 0:4a30986db2fb | 51 | if(!onTarget(eps + hysteresis)) { |
impguard | 0:4a30986db2fb | 52 | hysteresis = 0; |
impguard | 0:4a30986db2fb | 53 | pc.printf("Tracking target...\r\n"); |
impguard | 0:4a30986db2fb | 54 | safetyOn(); |
impguard | 0:4a30986db2fb | 55 | trackTarget(); |
impguard | 0:4a30986db2fb | 56 | } else { |
impguard | 0:4a30986db2fb | 57 | pc.printf("Target acquired!\r\n"); |
impguard | 0:4a30986db2fb | 58 | if(safety) { |
impguard | 0:4a30986db2fb | 59 | safetyOff(); |
impguard | 0:4a30986db2fb | 60 | } |
impguard | 0:4a30986db2fb | 61 | fire(); |
impguard | 0:4a30986db2fb | 62 | wait(3); |
impguard | 0:4a30986db2fb | 63 | hysteresis = 0.5 * eps; |
impguard | 0:4a30986db2fb | 64 | } |
impguard | 0:4a30986db2fb | 65 | } |
impguard | 0:4a30986db2fb | 66 | |
impguard | 0:4a30986db2fb | 67 | } |
impguard | 0:4a30986db2fb | 68 | |
impguard | 0:4a30986db2fb | 69 | /* All the initialization are done here. */ |
impguard | 0:4a30986db2fb | 70 | void initialize() { |
impguard | 0:4a30986db2fb | 71 | x = 0; |
impguard | 0:4a30986db2fb | 72 | y = 0; |
impguard | 0:4a30986db2fb | 73 | z = 0; |
impguard | 0:4a30986db2fb | 74 | x_offset = 0; |
impguard | 0:4a30986db2fb | 75 | y_offset = 0; |
impguard | 0:4a30986db2fb | 76 | z_offset = 0; |
impguard | 0:4a30986db2fb | 77 | initializeWiFi(); |
impguard | 0:4a30986db2fb | 78 | calibrateCamera(); |
impguard | 0:4a30986db2fb | 79 | |
impguard | 0:4a30986db2fb | 80 | wait(3); |
impguard | 0:4a30986db2fb | 81 | led_red = 0; |
impguard | 0:4a30986db2fb | 82 | led_green = 1; |
impguard | 0:4a30986db2fb | 83 | pc.printf("Tap when ready.\r\n"); |
impguard | 0:4a30986db2fb | 84 | while(1) { |
impguard | 0:4a30986db2fb | 85 | if (tsi.readPercentage() > 0.001) {break;} |
impguard | 0:4a30986db2fb | 86 | } |
impguard | 0:4a30986db2fb | 87 | pc.printf("It's hunting season.\r\n"); |
impguard | 0:4a30986db2fb | 88 | led_red = 1; |
impguard | 0:4a30986db2fb | 89 | led_green = 0; |
impguard | 0:4a30986db2fb | 90 | } |
impguard | 0:4a30986db2fb | 91 | |
impguard | 0:4a30986db2fb | 92 | /* Calibrate the camera to calculate the offsets for transforming the camera coordinates into gun coordinates. */ |
impguard | 0:4a30986db2fb | 93 | void calibrateCamera() { |
impguard | 0:4a30986db2fb | 94 | //Put someone's face right in front of the gun barrel. |
impguard | 0:4a30986db2fb | 95 | |
impguard | 0:4a30986db2fb | 96 | double x0, y0, z0; |
impguard | 0:4a30986db2fb | 97 | x0 = 0; |
impguard | 0:4a30986db2fb | 98 | y0 = 0; |
impguard | 0:4a30986db2fb | 99 | z0 = 0; |
impguard | 0:4a30986db2fb | 100 | wait(3); |
impguard | 0:4a30986db2fb | 101 | pc.printf("Tap to start calibration of camera.\r\n"); |
impguard | 0:4a30986db2fb | 102 | while(1) { |
impguard | 0:4a30986db2fb | 103 | //Wait for user to tap in order to start the calibration. |
impguard | 0:4a30986db2fb | 104 | led_red = 0; |
impguard | 0:4a30986db2fb | 105 | led_green = 1; |
impguard | 0:4a30986db2fb | 106 | if (tsi.readPercentage() > 0.001) {break;} |
impguard | 0:4a30986db2fb | 107 | } |
impguard | 0:4a30986db2fb | 108 | led_red = 1; |
impguard | 0:4a30986db2fb | 109 | led_green = 0; |
impguard | 0:4a30986db2fb | 110 | wait(3); |
impguard | 0:4a30986db2fb | 111 | pc.printf("Put face in front of gun barrel and tap when centered.\r\n"); |
impguard | 0:4a30986db2fb | 112 | while(1) { |
impguard | 0:4a30986db2fb | 113 | //Continuously poll data from camera until mbed's slider is tapped. |
impguard | 0:4a30986db2fb | 114 | getCameraData(x0, y0, z0); |
impguard | 0:4a30986db2fb | 115 | led_red = 0; |
impguard | 0:4a30986db2fb | 116 | led_green = 1; |
impguard | 0:4a30986db2fb | 117 | if (tsi.readPercentage() > 0.001) {break;} |
impguard | 0:4a30986db2fb | 118 | } |
impguard | 0:4a30986db2fb | 119 | x_offset = -x0; |
impguard | 0:4a30986db2fb | 120 | y_offset = -y0; |
impguard | 0:4a30986db2fb | 121 | z_offset = -z0; |
impguard | 0:4a30986db2fb | 122 | pc.printf("Offests: %f %f %f\r\n", x_offset, y_offset, z_offset); |
impguard | 0:4a30986db2fb | 123 | //Green led on. |
impguard | 0:4a30986db2fb | 124 | led_red = 1; |
impguard | 0:4a30986db2fb | 125 | led_green = 0; |
impguard | 0:4a30986db2fb | 126 | } |
impguard | 0:4a30986db2fb | 127 | |
impguard | 0:4a30986db2fb | 128 | /* Get the coordinates of the target, relative to gun barrel. Return true if successfully retrieve data, else return false and do not change parameters. */ |
impguard | 0:4a30986db2fb | 129 | bool getCoordinates(double &x, double &y, double &z) { |
impguard | 0:4a30986db2fb | 130 | if (getCameraData(x, y, z)) { |
impguard | 0:4a30986db2fb | 131 | x += x_offset; |
impguard | 0:4a30986db2fb | 132 | y += y_offset; |
impguard | 0:4a30986db2fb | 133 | z += z_offset; |
impguard | 0:4a30986db2fb | 134 | pc.printf("Offsetted position: %f %f %f\r\n", x, y, z); |
impguard | 0:4a30986db2fb | 135 | return true; |
impguard | 0:4a30986db2fb | 136 | } else { |
impguard | 0:4a30986db2fb | 137 | //Fail to connect to server. Maybe track to last known target location? |
impguard | 0:4a30986db2fb | 138 | return false; |
impguard | 0:4a30986db2fb | 139 | } |
impguard | 0:4a30986db2fb | 140 | } |
impguard | 0:4a30986db2fb | 141 | |
impguard | 0:4a30986db2fb | 142 | /* Check if the gun is aiming at a square area centered on the target, with TOLERANCE as the side length. */ |
impguard | 0:4a30986db2fb | 143 | bool onTarget(double tolerance) { |
impguard | 0:4a30986db2fb | 144 | //Ignore depth for now. |
impguard | 0:4a30986db2fb | 145 | return abs(x) <= tolerance && abs(y) <= tolerance; |
impguard | 0:4a30986db2fb | 146 | } |
impguard | 0:4a30986db2fb | 147 | |
impguard | 0:4a30986db2fb | 148 | /* Helper function for using the stepper motor. */ |
impguard | 0:4a30986db2fb | 149 | void stepMotorStates(int state) { |
impguard | 0:4a30986db2fb | 150 | switch(state) { |
impguard | 0:4a30986db2fb | 151 | case 0: |
impguard | 0:4a30986db2fb | 152 | step1 = 0; |
impguard | 0:4a30986db2fb | 153 | step2 = 0; |
impguard | 0:4a30986db2fb | 154 | break; |
impguard | 0:4a30986db2fb | 155 | case 1: |
impguard | 0:4a30986db2fb | 156 | step1 = 1; |
impguard | 0:4a30986db2fb | 157 | step2 = 0; |
impguard | 0:4a30986db2fb | 158 | break; |
impguard | 0:4a30986db2fb | 159 | case 2: |
impguard | 0:4a30986db2fb | 160 | step1 = 1; |
impguard | 0:4a30986db2fb | 161 | step2 = 1; |
impguard | 0:4a30986db2fb | 162 | break; |
impguard | 0:4a30986db2fb | 163 | case 3: |
impguard | 0:4a30986db2fb | 164 | step1 = 0; |
impguard | 0:4a30986db2fb | 165 | step2 = 1; |
impguard | 0:4a30986db2fb | 166 | break; |
impguard | 0:4a30986db2fb | 167 | default: |
impguard | 0:4a30986db2fb | 168 | break; |
impguard | 0:4a30986db2fb | 169 | } |
impguard | 0:4a30986db2fb | 170 | } |
impguard | 0:4a30986db2fb | 171 | |
impguard | 0:4a30986db2fb | 172 | /* Turn the turret left for the given milliseconds. */ |
impguard | 0:4a30986db2fb | 173 | void turnLeft(double ms) { |
impguard | 0:4a30986db2fb | 174 | stepMotorStates(3); |
impguard | 0:4a30986db2fb | 175 | wait_ms(ms); |
impguard | 0:4a30986db2fb | 176 | stepMotorStates(2); |
impguard | 0:4a30986db2fb | 177 | wait_ms(ms); |
impguard | 0:4a30986db2fb | 178 | stepMotorStates(1); |
impguard | 0:4a30986db2fb | 179 | wait_ms(ms); |
impguard | 0:4a30986db2fb | 180 | stepMotorStates(0); |
impguard | 0:4a30986db2fb | 181 | wait_ms(ms); |
impguard | 0:4a30986db2fb | 182 | } |
impguard | 0:4a30986db2fb | 183 | |
impguard | 0:4a30986db2fb | 184 | /* Turn the turret left for the given milliseconds. */ |
impguard | 0:4a30986db2fb | 185 | void turnRight(double ms) { |
impguard | 0:4a30986db2fb | 186 | stepMotorStates(0); |
impguard | 0:4a30986db2fb | 187 | wait_ms(ms); |
impguard | 0:4a30986db2fb | 188 | stepMotorStates(1); |
impguard | 0:4a30986db2fb | 189 | wait_ms(ms); |
impguard | 0:4a30986db2fb | 190 | stepMotorStates(2); |
impguard | 0:4a30986db2fb | 191 | wait_ms(ms); |
impguard | 0:4a30986db2fb | 192 | stepMotorStates(3); |
impguard | 0:4a30986db2fb | 193 | wait_ms(ms); |
impguard | 0:4a30986db2fb | 194 | } |
impguard | 0:4a30986db2fb | 195 | |
impguard | 0:4a30986db2fb | 196 | /* Tilt the gun up for the given milliseconds. */ |
impguard | 0:4a30986db2fb | 197 | void tiltUp(double ms) { |
impguard | 0:4a30986db2fb | 198 | h_bridge1 = 1; |
impguard | 0:4a30986db2fb | 199 | h_bridge2 = 0; |
impguard | 0:4a30986db2fb | 200 | h_bridge3 = 0; |
impguard | 0:4a30986db2fb | 201 | h_bridge4 = 1; |
impguard | 0:4a30986db2fb | 202 | wait_ms(ms); |
impguard | 0:4a30986db2fb | 203 | h_bridge1 = 0; |
impguard | 0:4a30986db2fb | 204 | h_bridge2 = 0; |
impguard | 0:4a30986db2fb | 205 | h_bridge3 = 0; |
impguard | 0:4a30986db2fb | 206 | h_bridge4 = 0; |
impguard | 0:4a30986db2fb | 207 | } |
impguard | 0:4a30986db2fb | 208 | |
impguard | 0:4a30986db2fb | 209 | /* Tilt the gun down for the given milliseconds. */ |
impguard | 0:4a30986db2fb | 210 | void tiltDown(double ms) { |
impguard | 0:4a30986db2fb | 211 | h_bridge1 = 0; |
impguard | 0:4a30986db2fb | 212 | h_bridge2 = 1; |
impguard | 0:4a30986db2fb | 213 | h_bridge3 = 1; |
impguard | 0:4a30986db2fb | 214 | h_bridge4 = 0; |
impguard | 0:4a30986db2fb | 215 | wait_ms(ms); |
impguard | 0:4a30986db2fb | 216 | h_bridge1 = 0; |
impguard | 0:4a30986db2fb | 217 | h_bridge2 = 0; |
impguard | 0:4a30986db2fb | 218 | h_bridge3 = 0; |
impguard | 0:4a30986db2fb | 219 | h_bridge4 = 0; |
impguard | 0:4a30986db2fb | 220 | } |
impguard | 0:4a30986db2fb | 221 | |
impguard | 0:4a30986db2fb | 222 | /* Move the turret to aim at the target. */ |
impguard | 0:4a30986db2fb | 223 | void trackTarget() { |
impguard | 0:4a30986db2fb | 224 | if (x < -eps) { |
impguard | 0:4a30986db2fb | 225 | pc.printf("Turning left.\r\n"); |
impguard | 0:4a30986db2fb | 226 | turnLeft(200); |
impguard | 0:4a30986db2fb | 227 | } else if(x >= eps) { |
impguard | 0:4a30986db2fb | 228 | pc.printf("Turning right.\r\n"); |
impguard | 0:4a30986db2fb | 229 | turnRight(200); |
impguard | 0:4a30986db2fb | 230 | } else |
impguard | 0:4a30986db2fb | 231 | if(y < -eps) { |
impguard | 0:4a30986db2fb | 232 | pc.printf("Tilting down.\r\n"); |
impguard | 0:4a30986db2fb | 233 | tiltDown(500); |
impguard | 0:4a30986db2fb | 234 | } else if(y >= eps) { |
impguard | 0:4a30986db2fb | 235 | pc.printf("Tilting up.\r\n"); |
impguard | 0:4a30986db2fb | 236 | tiltUp(1000); |
impguard | 0:4a30986db2fb | 237 | } |
impguard | 0:4a30986db2fb | 238 | |
impguard | 0:4a30986db2fb | 239 | |
impguard | 0:4a30986db2fb | 240 | } |
impguard | 0:4a30986db2fb | 241 | |
impguard | 0:4a30986db2fb | 242 | /* Turn the safety of the NERF gun off (firewheel on). */ |
impguard | 0:4a30986db2fb | 243 | void safetyOff() { |
impguard | 0:4a30986db2fb | 244 | flywheel = 1; |
impguard | 0:4a30986db2fb | 245 | safety = false; |
impguard | 0:4a30986db2fb | 246 | wait(0.3); |
impguard | 0:4a30986db2fb | 247 | } |
impguard | 0:4a30986db2fb | 248 | |
impguard | 0:4a30986db2fb | 249 | /* Turn the safety on (firewheel off). */ |
impguard | 0:4a30986db2fb | 250 | void safetyOn() { |
impguard | 0:4a30986db2fb | 251 | flywheel = 0; |
impguard | 0:4a30986db2fb | 252 | safety = true; |
impguard | 0:4a30986db2fb | 253 | } |
impguard | 0:4a30986db2fb | 254 | |
impguard | 0:4a30986db2fb | 255 | /* Fire the gun. Return true if successfully pull trigger, else false. */ |
impguard | 0:4a30986db2fb | 256 | bool fire() { |
impguard | 0:4a30986db2fb | 257 | if (!safety) { |
impguard | 0:4a30986db2fb | 258 | trigger = 1; |
impguard | 0:4a30986db2fb | 259 | wait(0.4); |
impguard | 0:4a30986db2fb | 260 | trigger = 0; |
impguard | 0:4a30986db2fb | 261 | return true; |
impguard | 0:4a30986db2fb | 262 | } |
impguard | 0:4a30986db2fb | 263 | return false; |
impguard | 0:4a30986db2fb | 264 | } |