File content as of revision 23:174f955e5c15:

#include "mbed.h"
#include "emg.h"
#include "inbetweencontroller.h"
#include "pidControl.h"
#include "PinDetect.h"
#include "compute.h"
#include "main.h"
 
/// Define Objects
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;
bool        demoing = false;
int         demoInt = 0;
int         demoRound = 0;
 
void tick()
{
    if(demoing) {
        if(demoRound == 7) {
            NVIC_SystemReset();
            return;   
        }
        double a,b;
        if(demoInt == 0) {
            Point2Angles(-20, 18, a, b);
            demoRound++;
        }
        if(demoInt == 1)
            Point2Angles(-20, 35, a, b);
        if(demoInt == 2)
            Point2Angles(20, 35, a, b);
        if(demoInt == 3)
            Point2Angles(20, 18, a, b);
        demoInt++;
        demoInt = demoInt%4;
        rotate(a,b);
        if(demoRound == 6) {
            Point2Angles(0, 24, a, b);
            demoRound++;
            rotate(a,b);
            return;
        }
        return;  
    }
    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
    bool pushing;
    newPos(emg_out0, emg_out1, emg_out2, a, b, x, y, pushing);
   
    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);
 
}
 
/** 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) {
        emg_cal(calibrated);
        calibrated++;
        return;
    }
    started = !started;
    if(!started) {
        go_motor = false;
        PID_stop();
        movement_timer.detach();
    } else
        movement_timer.attach(&goMotor, 0.01f);
}
 
/** Function called by pressing the SW2 Button */
void startRound()
{
    if(calibrated < 3) {
        calibrated = 4;
        calc_timer.attach(&goTick, 2.0);
        demoing = true;  
    }
    else {
        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();
}
 
/** 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);
    PID_init();
   
    /** Enable buttons */
    init();
 
    movement_timer.attach(&goMotor, 0.01f);
 
    rotate(a,b);
    
    /* COMMENT THIS TO USE THE EMG */
    calibrated = 4;
    calc_timer.attach(&goTick, 2.0);
    demoing = true;  
    /* COMMENT THIS TO USE THE EMG */
 
    /** 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();
        }
    }
}