File content as of revision 18:ff312179cf44:

#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 < 1) {
        emg_out0 = 0;
        emg_out1 = 0;
        emg_out2 = 0;
    }
    debug_tick++;

    // 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);

    Angles2Point(x,y,a,b);
    if (pushing) {
        motorSpeed = 1;
        pc2.printf("X: %f | Y: %f\r\n",a,b);
    }
    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);

}

/* 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 < 4) {
        pc2.printf("Calibrating %d\r\n", calibrated);
        emg_cal(calibrated);
        calibrated++;
        pc2.printf("Done\r\n");
        return;
    }
    started = !started;
    if(!started) {
        pc2.printf("Stopped\r\n");
        go_motor = false;
        PID_stop();
        movement_timer.detach();
    } else {
        pc2.printf("Starting\r\n");
        movement_timer.attach(&goMotor, 0.01f);
    }
}

/* 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()
{
    /* 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");
    
    /* Enable buttons */
    init();

    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();
        }
    }
}