File content as of revision 0:4a30986db2fb:

/**
*
*   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;
}