File content as of revision 104:750d7e13137d:

/*
   ________                      ____        __          __ 
  / ____/ /_  ___  __________   / __ \____  / /_  ____  / /_
 / /   / __ \/ _ \/ ___/ ___/  / /_/ / __ \/ __ \/ __ \/ __/
/ /___/ / / /  __(__  |__  )  / _, _/ /_/ / /_/ / /_/ / /_  
\____/_/ /_/\___/____/____/  /_/ |_|\____/_.___/\____/\__/  
                                                            
*/

#include "mbed.h"
#include "config.h"  // settings and pin configurations
#include "actuators.h"
#include "buttons.h"
#include "debug.h"
#include "emg.h"

Ticker switchesTick, debugTick, motorTick, EMGTick, safetyTick, servoTick;
volatile bool switches_go=false, debug_go=false, motor_go=false, emg_go=false, safety_go=false, servo_go=false;

void switches_activate(){switches_go=true;};
void debug_activate(){debug_go=true;};
void motor_activate(){motor_go=true;};
void emg_activate(){emg_go=true;};
void safety_activate(){safety_go=true;};
void servo_activate(){servo_go=true;};

const double    emgCall     = 0.005;        // Set EMG sampling period
const double    motorCall   = 0.005;        // Set motor control period global so it can be used for speed.
const double    servoCall   = 0.025;         // Set servo control speed  
const int       sample      = 0;            // Constant for mode switching for program readability
const int       normalize   = 1;            // Constant for mode switching for program readability
bool            mode        = sample;    // Set program mode


int main(){
    wait_ms(2000);
    motorsEnable = true;
    //motorInit();    
//    calibrateMotors(); // start motor calibration
    blueLed.write(1),redLed.write(1),greenLed.write(1);
    servo.pulsewidth(0.0015); // Set servo to zero position
    
    //EMGTick.attach(&emg_activate, emgCall);
    //switchesTick.attach(&switches_activate, 0.02f);
    //debugTick.attach(&debug_activate, 0.03f);
    motorTick.attach(&motor_activate, motorCall);
    servoTick.attach(&servo_activate, servoCall);
    safetyTick.attach(&safety_activate, 0.001f);

    while (true) {
        if(emg_go){
            emg_go=false;
            readEMG();
        }
        if(mode==0){  // wait until EMG is done with calibration
            if(safety_go){
                safety_go=false;
                safety();
            }
            if(emg_go){
                emg_go=false;
                readEMG();
            }
            if(motor_go){
                motor_go=false;
                motorControl();
            }
            if(servo_go){
                servo_go=false;
                servoControl();
            }                        
            if(switches_go){
                switches_go=false;
                checkSwitches();
            }
            if(debug_go){
                debug_go=false;
                debugProcess();
            }
        }
    }
}