Diff: main.cpp

Revision:

0:4a30986db2fb

diff -r 000000000000 -r 4a30986db2fb main.cpp
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/main.cpp	Fri Dec 19 11:49:27 2014 +0000
@@ -0,0 +1,264 @@
+/**
+*
+*   Controller for the NERF turret, allowing for tracking of target (human face) via visual input.
+*
+*   authors: Leo Kam
+**/
+
+#include "mbed.h"
+#include "camera.h"
+#include "TSISensor.h"
+
+//Relays for firing the NERF gun.
+DigitalOut flywheel(PTE5);
+DigitalOut trigger(PTE4);
+//Relays for turning the turret.
+DigitalOut step1(PTE2);
+DigitalOut step2(PTE3);
+//Relays for tilting the gun up and down.
+DigitalOut h_bridge1(PTB8);
+DigitalOut h_bridge2(PTB9);
+DigitalOut h_bridge3(PTB10);
+DigitalOut h_bridge4(PTB11);
+
+
+double x, y, z;                          //The coordinates of the target, relative to the barrel of the gun.
+double x_offset, y_offset, z_offset;     //The offsets to transform the camera data into a coordinate system relative to barrel of the gun.
+TSISensor tsi;                           //The touch sensor of the mbed.
+double eps = 40;                         //The error margin.
+double interval = 200;                   //The time between each poll of the server, in ms.
+bool safety = true;                      //Whether the safety of the NERF gun is on or off.
+
+void initialize();
+void calibrateCamera();
+bool getCoordinates(double&, double&, double&);
+bool onTarget(double);
+void stepMotorStates(int state);
+void turnLeft(double);
+void turnRight(double);
+void tiltUp(double);
+void tiltDown(double);
+void trackTarget();
+void safetyOn();
+void safetyOff();
+bool fire();
+
+int main() {
+    initialize();
+    double hysteresis = 0;
+    while(1) {
+        getCoordinates(x, y, z);
+        if(!onTarget(eps + hysteresis)) {
+            hysteresis = 0;
+            pc.printf("Tracking target...\r\n");
+            safetyOn();
+            trackTarget();  
+        } else {
+            pc.printf("Target acquired!\r\n");
+            if(safety) {
+                safetyOff();
+            }
+            fire();
+            wait(3);
+            hysteresis = 0.5 * eps;
+        }
+    }
+
+}
+
+/* All the initialization are done here. */
+void initialize() {
+    x = 0;
+    y = 0;
+    z = 0;
+    x_offset = 0;
+    y_offset = 0;
+    z_offset = 0;
+    initializeWiFi();
+    calibrateCamera();
+    
+    wait(3);
+    led_red = 0;
+    led_green = 1;
+    pc.printf("Tap when ready.\r\n");
+    while(1) {
+        if (tsi.readPercentage() > 0.001) {break;}
+    }
+    pc.printf("It's hunting season.\r\n");
+    led_red = 1;
+    led_green = 0;
+}
+
+/* Calibrate the camera to calculate the offsets for transforming the camera coordinates into gun coordinates. */
+void calibrateCamera() {
+    //Put someone's face right in front of the gun barrel.
+    
+    double x0, y0, z0;
+    x0 = 0;
+    y0 = 0;
+    z0 = 0;
+    wait(3);
+    pc.printf("Tap to start calibration of camera.\r\n");
+    while(1) {
+        //Wait for user to tap in order to start the calibration.
+        led_red = 0;
+        led_green = 1;
+        if (tsi.readPercentage() > 0.001) {break;}
+    }
+    led_red = 1;
+    led_green = 0;
+    wait(3);
+    pc.printf("Put face in front of gun barrel and tap when centered.\r\n");
+    while(1) {
+        //Continuously poll data from camera until mbed's slider is tapped.
+        getCameraData(x0, y0, z0);
+        led_red = 0;
+        led_green = 1;
+        if (tsi.readPercentage() > 0.001) {break;}
+    }
+    x_offset = -x0;
+    y_offset = -y0;
+    z_offset = -z0;
+    pc.printf("Offests: %f %f %f\r\n", x_offset, y_offset, z_offset);
+    //Green led on.
+    led_red = 1;
+    led_green = 0;
+}
+
+/* Get the coordinates of the target, relative to gun barrel. Return true if successfully retrieve data, else return false and do not change parameters. */
+bool getCoordinates(double &x, double &y, double &z) {
+    if (getCameraData(x, y, z)) {
+        x += x_offset;
+        y += y_offset;
+        z += z_offset;
+        pc.printf("Offsetted position: %f %f %f\r\n", x, y, z);
+        return true;   
+    } else {
+        //Fail to connect to server. Maybe track to last known target location?
+        return false;    
+    }
+}
+
+/* Check if the gun is aiming at a square area centered on the target, with TOLERANCE as the side length. */
+bool onTarget(double tolerance) {
+    //Ignore depth for now.
+    return abs(x) <= tolerance && abs(y) <= tolerance;
+}
+
+/* Helper function for using the stepper motor. */
+void stepMotorStates(int state) {
+    switch(state) {
+        case 0:
+            step1 = 0;
+            step2 = 0;
+            break;
+        case 1:
+            step1 = 1;
+            step2 = 0;
+            break;
+        case 2:
+            step1 = 1;
+            step2 = 1;
+            break;
+        case 3:
+            step1 = 0;
+            step2 = 1;
+            break;
+        default:
+            break;
+    }
+}
+
+/* Turn the turret left for the given milliseconds. */
+void turnLeft(double ms) {
+    stepMotorStates(3);
+    wait_ms(ms);
+    stepMotorStates(2);
+    wait_ms(ms);
+    stepMotorStates(1);
+    wait_ms(ms);
+    stepMotorStates(0);
+    wait_ms(ms);
+}
+
+/* Turn the turret left for the given milliseconds. */
+void turnRight(double ms) {
+    stepMotorStates(0);
+    wait_ms(ms);
+    stepMotorStates(1);
+    wait_ms(ms);
+    stepMotorStates(2);
+    wait_ms(ms);
+    stepMotorStates(3);
+    wait_ms(ms);
+}
+
+/* Tilt the gun up for the given milliseconds. */
+void tiltUp(double ms) {
+    h_bridge1 = 1;
+    h_bridge2 = 0;
+    h_bridge3 = 0;
+    h_bridge4 = 1;
+    wait_ms(ms);
+    h_bridge1 = 0;
+    h_bridge2 = 0;
+    h_bridge3 = 0;
+    h_bridge4 = 0;
+}
+
+/* Tilt the gun down for the given milliseconds. */
+void tiltDown(double ms) {
+    h_bridge1 = 0;
+    h_bridge2 = 1;
+    h_bridge3 = 1;
+    h_bridge4 = 0;
+    wait_ms(ms);
+    h_bridge1 = 0;
+    h_bridge2 = 0;
+    h_bridge3 = 0;
+    h_bridge4 = 0;
+}
+
+/* Move the turret to aim at the target. */
+void trackTarget() {
+    if (x < -eps) {
+        pc.printf("Turning left.\r\n");
+        turnLeft(200);
+    } else if(x >= eps) {
+        pc.printf("Turning right.\r\n");
+        turnRight(200);    
+    } else
+    if(y < -eps) {
+        pc.printf("Tilting down.\r\n");
+        tiltDown(500);        
+    } else if(y >= eps) {
+        pc.printf("Tilting up.\r\n");
+        tiltUp(1000);
+    }
+    
+    
+}
+
+/* Turn the safety of the NERF gun off (firewheel on). */
+void safetyOff() {
+    flywheel = 1;
+    safety = false;
+    wait(0.3);
+}
+
+/* Turn the safety on (firewheel off). */
+void safetyOn() {
+    flywheel = 0;
+    safety = true;
+}
+
+/* Fire the gun. Return true if successfully pull trigger, else false. */
+bool fire() {
+    if (!safety) {
+        trigger = 1;
+        wait(0.4);
+        trigger = 0;
+        return true;
+    }
+    return false;
+}
\ No newline at end of file