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

Embed: (wiki syntax)

« Back to documentation index

Show/hide line numbers main.cpp Source File

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 }