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
00001 /** 00002 * 00003 * Controller for the NERF turret, allowing for tracking of target (human face) via visual input. 00004 * 00005 * authors: Leo Kam 00006 **/ 00007 00008 #include "mbed.h" 00009 #include "camera.h" 00010 #include "TSISensor.h" 00011 00012 //Relays for firing the NERF gun. 00013 DigitalOut flywheel(PTE5); 00014 DigitalOut trigger(PTE4); 00015 //Relays for turning the turret. 00016 DigitalOut step1(PTE2); 00017 DigitalOut step2(PTE3); 00018 //Relays for tilting the gun up and down. 00019 DigitalOut h_bridge1(PTB8); 00020 DigitalOut h_bridge2(PTB9); 00021 DigitalOut h_bridge3(PTB10); 00022 DigitalOut h_bridge4(PTB11); 00023 00024 00025 double x, y, z; //The coordinates of the target, relative to the barrel of the gun. 00026 double x_offset, y_offset, z_offset; //The offsets to transform the camera data into a coordinate system relative to barrel of the gun. 00027 TSISensor tsi; //The touch sensor of the mbed. 00028 double eps = 40; //The error margin. 00029 double interval = 200; //The time between each poll of the server, in ms. 00030 bool safety = true; //Whether the safety of the NERF gun is on or off. 00031 00032 void initialize(); 00033 void calibrateCamera(); 00034 bool getCoordinates(double&, double&, double&); 00035 bool onTarget(double); 00036 void stepMotorStates(int state); 00037 void turnLeft(double); 00038 void turnRight(double); 00039 void tiltUp(double); 00040 void tiltDown(double); 00041 void trackTarget(); 00042 void safetyOn(); 00043 void safetyOff(); 00044 bool fire(); 00045 00046 int main() { 00047 initialize(); 00048 double hysteresis = 0; 00049 while(1) { 00050 getCoordinates(x, y, z); 00051 if(!onTarget(eps + hysteresis)) { 00052 hysteresis = 0; 00053 pc.printf("Tracking target...\r\n"); 00054 safetyOn(); 00055 trackTarget(); 00056 } else { 00057 pc.printf("Target acquired!\r\n"); 00058 if(safety) { 00059 safetyOff(); 00060 } 00061 fire(); 00062 wait(3); 00063 hysteresis = 0.5 * eps; 00064 } 00065 } 00066 00067 } 00068 00069 /* All the initialization are done here. */ 00070 void initialize() { 00071 x = 0; 00072 y = 0; 00073 z = 0; 00074 x_offset = 0; 00075 y_offset = 0; 00076 z_offset = 0; 00077 initializeWiFi(); 00078 calibrateCamera(); 00079 00080 wait(3); 00081 led_red = 0; 00082 led_green = 1; 00083 pc.printf("Tap when ready.\r\n"); 00084 while(1) { 00085 if (tsi.readPercentage() > 0.001) {break;} 00086 } 00087 pc.printf("It's hunting season.\r\n"); 00088 led_red = 1; 00089 led_green = 0; 00090 } 00091 00092 /* Calibrate the camera to calculate the offsets for transforming the camera coordinates into gun coordinates. */ 00093 void calibrateCamera() { 00094 //Put someone's face right in front of the gun barrel. 00095 00096 double x0, y0, z0; 00097 x0 = 0; 00098 y0 = 0; 00099 z0 = 0; 00100 wait(3); 00101 pc.printf("Tap to start calibration of camera.\r\n"); 00102 while(1) { 00103 //Wait for user to tap in order to start the calibration. 00104 led_red = 0; 00105 led_green = 1; 00106 if (tsi.readPercentage() > 0.001) {break;} 00107 } 00108 led_red = 1; 00109 led_green = 0; 00110 wait(3); 00111 pc.printf("Put face in front of gun barrel and tap when centered.\r\n"); 00112 while(1) { 00113 //Continuously poll data from camera until mbed's slider is tapped. 00114 getCameraData(x0, y0, z0); 00115 led_red = 0; 00116 led_green = 1; 00117 if (tsi.readPercentage() > 0.001) {break;} 00118 } 00119 x_offset = -x0; 00120 y_offset = -y0; 00121 z_offset = -z0; 00122 pc.printf("Offests: %f %f %f\r\n", x_offset, y_offset, z_offset); 00123 //Green led on. 00124 led_red = 1; 00125 led_green = 0; 00126 } 00127 00128 /* Get the coordinates of the target, relative to gun barrel. Return true if successfully retrieve data, else return false and do not change parameters. */ 00129 bool getCoordinates(double &x, double &y, double &z) { 00130 if (getCameraData(x, y, z)) { 00131 x += x_offset; 00132 y += y_offset; 00133 z += z_offset; 00134 pc.printf("Offsetted position: %f %f %f\r\n", x, y, z); 00135 return true; 00136 } else { 00137 //Fail to connect to server. Maybe track to last known target location? 00138 return false; 00139 } 00140 } 00141 00142 /* Check if the gun is aiming at a square area centered on the target, with TOLERANCE as the side length. */ 00143 bool onTarget(double tolerance) { 00144 //Ignore depth for now. 00145 return abs(x) <= tolerance && abs(y) <= tolerance; 00146 } 00147 00148 /* Helper function for using the stepper motor. */ 00149 void stepMotorStates(int state) { 00150 switch(state) { 00151 case 0: 00152 step1 = 0; 00153 step2 = 0; 00154 break; 00155 case 1: 00156 step1 = 1; 00157 step2 = 0; 00158 break; 00159 case 2: 00160 step1 = 1; 00161 step2 = 1; 00162 break; 00163 case 3: 00164 step1 = 0; 00165 step2 = 1; 00166 break; 00167 default: 00168 break; 00169 } 00170 } 00171 00172 /* Turn the turret left for the given milliseconds. */ 00173 void turnLeft(double ms) { 00174 stepMotorStates(3); 00175 wait_ms(ms); 00176 stepMotorStates(2); 00177 wait_ms(ms); 00178 stepMotorStates(1); 00179 wait_ms(ms); 00180 stepMotorStates(0); 00181 wait_ms(ms); 00182 } 00183 00184 /* Turn the turret left for the given milliseconds. */ 00185 void turnRight(double ms) { 00186 stepMotorStates(0); 00187 wait_ms(ms); 00188 stepMotorStates(1); 00189 wait_ms(ms); 00190 stepMotorStates(2); 00191 wait_ms(ms); 00192 stepMotorStates(3); 00193 wait_ms(ms); 00194 } 00195 00196 /* Tilt the gun up for the given milliseconds. */ 00197 void tiltUp(double ms) { 00198 h_bridge1 = 1; 00199 h_bridge2 = 0; 00200 h_bridge3 = 0; 00201 h_bridge4 = 1; 00202 wait_ms(ms); 00203 h_bridge1 = 0; 00204 h_bridge2 = 0; 00205 h_bridge3 = 0; 00206 h_bridge4 = 0; 00207 } 00208 00209 /* Tilt the gun down for the given milliseconds. */ 00210 void tiltDown(double ms) { 00211 h_bridge1 = 0; 00212 h_bridge2 = 1; 00213 h_bridge3 = 1; 00214 h_bridge4 = 0; 00215 wait_ms(ms); 00216 h_bridge1 = 0; 00217 h_bridge2 = 0; 00218 h_bridge3 = 0; 00219 h_bridge4 = 0; 00220 } 00221 00222 /* Move the turret to aim at the target. */ 00223 void trackTarget() { 00224 if (x < -eps) { 00225 pc.printf("Turning left.\r\n"); 00226 turnLeft(200); 00227 } else if(x >= eps) { 00228 pc.printf("Turning right.\r\n"); 00229 turnRight(200); 00230 } else 00231 if(y < -eps) { 00232 pc.printf("Tilting down.\r\n"); 00233 tiltDown(500); 00234 } else if(y >= eps) { 00235 pc.printf("Tilting up.\r\n"); 00236 tiltUp(1000); 00237 } 00238 00239 00240 } 00241 00242 /* Turn the safety of the NERF gun off (firewheel on). */ 00243 void safetyOff() { 00244 flywheel = 1; 00245 safety = false; 00246 wait(0.3); 00247 } 00248 00249 /* Turn the safety on (firewheel off). */ 00250 void safetyOn() { 00251 flywheel = 0; 00252 safety = true; 00253 } 00254 00255 /* Fire the gun. Return true if successfully pull trigger, else false. */ 00256 bool fire() { 00257 if (!safety) { 00258 trigger = 1; 00259 wait(0.4); 00260 trigger = 0; 00261 return true; 00262 } 00263 return false; 00264 }
Generated on Tue Jul 12 2022 21:44:50 by 1.7.2