File content as of revision 14:0a66bce6ee19:

#include "mbed.h"
#include "emg.h"
#include "inbetweencontroller.h"
#include "pidControl.h"
#include "PinDetect.h"
#include "compute.h"

// Define Objects
Serial      pc2(USBTX, USBRX);
Ticker      sample_timer;
Ticker      movement_timer;
Ticker      calc_timer;
PinDetect   sRound(SW2);
PinDetect   caliAndSS(SW3);
bool        started = true;
bool        go_motor = true;
bool        go_tick = false;
bool        go_sample = false;
double      motorSpeed = 0.2;
int         calibrated = 0;

double      emg_out0, emg_out1, emg_out2;

int debug_tick = 0;
void tick()
{
    double a,b,x,y;
    
    /* If the game has not started, don't do anything. */
    if(!started) return;
    
    // Get the current rotation
    getCurrent(a,b);
    
    // Remove the offset
    a = getOffset(1) - a;
    b = getOffset(2) - b;
    
    // TODO: Remove debugging overrides
    if (debug_tick < 5) {
        emg_out0 = 0;
        emg_out1 = 0;
        emg_out2 = 0;    
    } else if (debug_tick < 10) {
        emg_out0 = 0;
        emg_out1 = 0;
        emg_out2 = 1;
    } else if (debug_tick < 20) {
        emg_out0 = 0;
        emg_out1 = 0;
        emg_out2 = 0;
    } else if (debug_tick < 25) {
        emg_out0 = 1;
        emg_out1 = 0;
        emg_out2 = 0;
    } else if (debug_tick < 30) {
        emg_out0 = 0;
        emg_out1 = 0;
        emg_out2 = 0;
    } else if (debug_tick < 40) {
        emg_out0 = 0;
        emg_out1 = 0;
        emg_out2 = 1;
    } else if (debug_tick < 45) {
        emg_out0 = 0;
        emg_out1 = 0;
        emg_out2 = 0;
    } else if (debug_tick < 55) {
        emg_out0 = 0;
        emg_out1 = 1;
        emg_out2 = 0;
    } else if (debug_tick < 60) {
        emg_out0 = 0;
        emg_out1 = 0;
        emg_out2 = 0;
    } else if (debug_tick < 70) {
        emg_out0 = 0;
        emg_out1 = 0;
        emg_out2 = 1;
    } else {
        emg_out0 = 0;
        emg_out1 = 0;
        emg_out2 = 0;
    }
    
    /*if (debug_tick < 10) {
        emg_out0 = 0;
        emg_out1 = 0;
        emg_out2 = 0;   
    } else if (debug_tick < 15) {
        emg_out0 = 1;
        emg_out1 = 0;
        emg_out2 = 0;   
    } else if (debug_tick < 25) {
        emg_out0 = 0;
        emg_out1 = 1;
        emg_out2 = 0;   
    } else if (debug_tick < 35) {
        emg_out0 = 0;
        emg_out1 = 0;
        emg_out2 = 0;   
    } else if (debug_tick < 45) {
        emg_out0 = 1;
        emg_out1 = 0;
        emg_out2 = 0;   
    } else if (debug_tick < 55) {
        emg_out0 = 0;
        emg_out1 = 0;
        emg_out2 = 0;   
    } else if (debug_tick < 65) {
        emg_out0 = 0;
        emg_out1 = 1;
        emg_out2 = 0;   
    } else {
        emg_out0 = 0;    
        emg_out1 = 0;
        emg_out2 = 0;
    }*/
 
    // Calculate the new position using the EMG output and the current position
    bool pushing;
    newPos(emg_out0, emg_out1, emg_out2, a, b, x, y, pushing);
    // pc2.printf("O0: %f | O1: %f | O2: %f\r\n",emg_out0, emg_out1, emg_out2);
    
    Angles2Point(x,y,a,b);
    
    if (pushing)
        motorSpeed = 1;
    else if(a < -20 || a > 20) 
        motorSpeed = 0.14;
    else
        motorSpeed = 0.25;
    
    // Rotate the motors
    if (pushing)
        push(x, y);
    else
        rotate(x,y);
    
    debug_tick++;
}

/* Functions which get called by tickers */
void goMotor() { go_motor = true; }

void goTick() { go_tick = true; }

void goSample() { go_sample = true; }

/* Function called by pressing the SW3 Button */
void calibrateAndStartStop()
{
    if(calibrated < 3) {
        calibrated++;
        emg_cal(calibrated);
        return;   
    }
    started = !started;
    if(started)
        movement_timer.attach(&goMotor, 0.01f);
    else {
        go_motor = false;
        PID_stop();
        movement_timer.detach();
    }
}

/* Function called by pressing the SW2 Button */
void startRound()
{
    if(calibrated < 3) return;
    sample_timer.attach(&goSample, 0.002);
    calc_timer.attach(&goTick, 0.15);
}

/* Initialize the buttons */
void init()
{
    // Set the shutup and start buttons
    caliAndSS.mode(PullUp);
    caliAndSS.attach_deasserted(&calibrateAndStartStop);
    caliAndSS.setSampleFrequency();

    sRound.mode(PullUp);
    sRound.attach_deasserted(&startRound);
    sRound.setSampleFrequency();

    pc2.printf("Buttons done\r\n\r\n");
}

/* The main function
    Initializes every library
    Starts the timers
    Has a while-loop which calls functions on a timer
*/
int main()
{
    init();
    
    /* Initialize libraries */
    double a,b;
    IBC_init(a,b);
    pc2.printf("(--- In Between Controller Initialized ---)\r\n");
    
    PID_init();
    pc2.printf("(--- PID Controller Initialized ---)\r\n");

    movement_timer.attach(&goMotor, 0.01f);

    rotate(a,b);

    /* The main while-loop */
    while(1) {
        // The motor timer
        if(go_motor) {
            go_motor = false;
            moveTick(motorSpeed);
        }
        // The sample timer
        if(go_sample) {
            go_sample = false;
            sample(emg_out0, emg_out1, emg_out2);   
        }
        // The tick timer
        if(go_tick) {
            go_tick = false;
            tick();
        }
    }
}