File content as of revision 7:aad7d5cdecd3:

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

double generateSample()
{
    counter++;
    if(counter < 10)
        return 0.0;
    else if(counter < 50)
        return 0.1;
    else if(counter < 100)
        return 0.2;
    else if(counter < 150)
        return 0.3;
    else if(counter < 200)
        return 0.4;
    else if(counter < 250)
        return 0.5;
    return 0.0;
}

void tick()
{
    if(!started) return;
    // EMG Reader
    double output0 = generateSample();
    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: %d deg | %d deg\r\nNEW: %d deg | %d deg\r\n",rad2deg(lastA), rad2deg(lastB), rad2deg(nextA), rad2deg(nextB));
    // Rotate the motors
    rotate(nextA,nextB);
}

void goMotor()
{
    go_motor = true;
}

void goTick()
{
    go_tick = true;
}

void startAll()
{
    started = !started;
    if(started)
        movement_timer.attach(&goMotor, 0.01f);
    else {
        go_motor = false;
        PID_stop();
        movement_timer.detach();
    }
}

int ding = 0;
void calibrate()
{
    ding++;
    double a,b;
    if(ding == 1) {
        //sample_timer.attach(&goTick, 0.002);
        Point2Angles(0,20,a,b);
        double radA, radB;
        float pX, pY;
        //pc2.printf("Moving to (0,20) A: %d | B: %d\r\n",rad2deg(a),rad2deg(b));
        rotate(a,b);
        getCurrent(radA, radB);
        Angles2Point(-radA + getOffset(1),-radB + getOffset(2),pX,pY);
        pc2.printf("Px1: %f | Py1: %f\r\n",pX,pY);
    }

    if(ding == 2) {
        Point2Angles(-20,20,a,b);
        double radA, radB;
        float pX, pY;
        //pc2.printf("Moving to (-20,20) A: %d | B: %d\r\n",rad2deg(a),rad2deg(b));
        rotate(a,b);
        getCurrent(radA, radB);
        Angles2Point(-radA + getOffset(1),-radB + getOffset(2),pX,pY);
        pc2.printf("Px2: %f | Py2: %f\r\n",pX,pY);
        ding = 0;
    }
}

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

    //movement_timer.attach(&moveTick, 0.01f);

    pc2.printf("A: %f | B: %f\r\n",a,b);

    movement_timer.attach(&goMotor, 0.01f);

    rotate(a,b);

    while(1) {
        if(go_motor) {
            go_motor = false;
            moveTick();
        }
        if(go_tick) {
            go_tick = false;
            tick();
        }
    }
}