File content as of revision 9:70cab4bf38a5:

#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   cali(SW2);
PinDetect   start(SW3);
bool        started = true;
bool        go_motor = true;
bool        go_tick = false;
bool        go_sample = false;

double      emg_out0, emg_out1, emg_out2;

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;
 
    // Calculate the new position using the EMG output and the current position
    newPos(emg_out0, emg_out1, 0, a, b, x, y);
    pc2.printf("O0: %f | O1: %f | O2: %f\r\n",emg_out0, emg_out1, emg_out2);
    // Rotate the motors
    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 startAll()
{
    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 calibrate()
{
    sample_timer.attach(&goSample, 0.002);
    calc_timer.attach(&goTick, 0.5);
}

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

    cali.mode(PullUp);
    cali.attach_deasserted(&calibrate);
    cali.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();
        }
        // 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();
        }
    }
}