File content as of revision 6:4f8760baa448:

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

// Define Objects
Serial      pc2(USBTX, USBRX);
Ticker      sample_timer;
PinDetect   cali(SW2);
PinDetect   start(SW3);
int         counter = 0;
bool        started = false;
bool        calibrating = false;

double generateSample() {
    counter++;
    if(counter < 100)
        return 0.0;
    else if(counter < 2250)
        return 0.1;
    else if(counter < 3750)
        return 0.0;
    else if(counter < 5050)
        return 0.0;
    else if(counter < 6750)
        return 0.0;
    else if(counter < 8250)
        return 0.0;
    return 0.0;
}

void tick() {  
    if(!started) return;
    // EMG Reader
    double output0 = generateSample();
    pc2.printf("OUT: %f\r\nCOUNTER: %d\r\n",output0,counter);
    double output1 = 0.0;
    double output2 = 0.0;
    
    // In Between Controller
    double lastA, lastB, nextA, nextB;
    getCurrent(lastA, lastB);
    newPos(output0, output1, output2, lastA, lastB, nextA, nextB);
    pc2.printf("CURRENT: %f rad | %f rad\r\nNEW: %f rad | %f rad\r\n",lastA, lastB, nextA, nextB);
    // Rotate the motors
    rotate(nextA,nextB);
}

void startAll() {
    started = !started;
}

void calibrate() {
    calibrating = false;   
}

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\nStarting Initialization...\r\n\r\n");
}

int main()
{
    init();
    pc2.printf("STARTED!");
    // Initialize libraries
    double a,b;
    IBC_init(a,b);
    PID_init(a,b);
    
    init();
    
    // Attach a timer to the function 'Sample' which fires every 0.002 seconds.
    sample_timer.attach(&tick, 0.002);
    
    while(1) { }
}