Kevin Wu / Mbed 2 deprecated TurretController

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?

UserRevisionLine numberNew 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 }