File content as of revision 0:66d33cc8318f:

#include "mbed.h"
#include "MODSERIAL.h"
#include "math.h"
#include "BiQuad.h"
#include "QEI.h"

//Inputs outputs e.t.c.

//Serial
MODSERIAL pc(USBTX, USBRX);

//Leds
DigitalOut led1(LED_GREEN);
DigitalOut led2(LED_BLUE);
DigitalOut led3(LED_RED);

//Servo's
DigitalOut StirUp(D2);
DigitalOut StirDown(D3);


//Timing
Ticker LedTimer;
Timeout LedTimeOut;

Ticker ScopeTimer;
Timeout SignalTimeout;

Ticker ControlTicker;
Ticker ControlTicker1;

//Buttons / Pots
DigitalIn button1(D0);
DigitalIn button2(D1);
AnalogIn Pot1(A5);
AnalogIn Pot2(A4);

//Emg
AnalogIn emg1(A0);
AnalogIn emg2(A1);
AnalogIn emg3(A2);


//Control
PwmOut Motor2PWR(D5);
DigitalOut Motor2DIR(D4);
PwmOut Motor1PWR(D6);
DigitalOut Motor1DIR(D7);
QEI Encoder1(D12,D13,NC,4200);
QEI Encoder2(D10,D11,NC,4200);

//Filter
BiQuadChain bqc;



//Declare variables
double fsample = 500; //EMG sample frequency Hz
double fcontrol = 1000; //Control frequency Hz
int max_n;  // length of measured arrays
double pi = 3.14159265; // define PI
int Cteller = 0;    //some int variables that I use to count in some functions. Important for if statements. 
int Cteller1 = 0;
int Cnum = 0;
int signalsC = 0;

// variables that depend on fsample
double Ts = (double)1/(double)1000; // Sample time in seconds of control loop
double emg1_log[500*6]; // allocating space for the EMG arrays
double emg2_log[500*6];
double emg3_log[500*6];
    
double RadPerCount = 2*pi/4200; // turning counts into rads using this value

//Encoders
int counts1;
int counts2;

//PID
double refpos1 = 1.48;  //angles for reference positions
double refpos2 = 1.05;
double Kp1 = 25; //values for first PID controller
double Ki1 = 13;
double Kd1 = 25;

double Kp2 = 25;        //values for second controller
double Ki2 = 13;
double Kd2 = 25;

//Movavg        //variables needed for the moving average filter
int avgn;
double avgh1;
double avgh2;
double avgh3;
double ms = 100; // number of datapoints used in moving average filter
double highnum1;
double highnum2;
double highnum3;

//Bools             //all kinds of bools used in if statements    They specify if some actions are allowed or not
bool stirred = false;
bool MeasureBool = true;
bool MoveBool = true;
bool CalibratedE = false;
volatile bool CalibratedM = false;
volatile bool Homing;
bool BlinkBool;
volatile bool pause_loop = true;
bool start = true;

//States                //there are 3 statemachines.  1. LED colors 2.states    3.directions 
bool statechanged = false;
bool logcq;
enum Colors {Green, Blue, Red, Yellow, Purple, Off};
enum states {CalibrateM, TestStand, Coffee, CalibrateE, MandM, Done};
enum Directions {Up, Down, Left, Right};
Directions CurrentDir = Down;
Colors LedState = Off;
states ProgramState = CalibrateM;

//Systeem eigenschappen                 /arm lengths in meters
double L0 = 0.39; //cm
double L1 = 0.295; //dm

//Speeds and Positions          //joint positions and speeds        also desired velocities
double setpoint = 0;
double MaxV = 1;//6.3; //rad/s
double qv[2]; //setpoint velocity
double qr[2]; //setpoint pos
double q1;
double q2;
int dir_i;                  //int used during the changing of direction
double dir_c;
double cqlog[2500];             //can be used to log measured joint positions

//Maak filters
BiQuad bq1(0.9475454809720195,-1.895090961944039,0.9475454809720195,-1.8932193507849706,0.8969625731031077); //highpass 5Hz
BiQuad bq2(0.843582000346111,-1.3649443488576334,0.843582000346111,-1.3649443488576334,0.6871640006922219); //notch 50Hz
BiQuad bq3(0.9824,-0.6071,0.9824,-0.6071,0.9647); //notch 100Hz
BiQuad bq4(0.9738  ,  0.6018 ,   0.9738,0.6018  ,  0.9475); //notch 150Hz
BiQuad bq5(0.9653  ,  1.5619  ,  0.9653,1.5619  ,  0.9307); //notch 200Hz

//Programma

void CancelPause(){                 // used to cancel a pause loop
    pause_loop = false;
}

void ClearEmg(){                    // Clears all EMG arrays and variables ascociated with them (moving avarage for example)
    int n;
    for(n = 0; n <= max_n; n++){
        emg1_log[n] = 0;
    }
    for(n = 0; n <= max_n; n++){
        emg2_log[n] = 0;
    }
    for(n = 0; n <= max_n; n++){
        emg3_log[n] = 0;
    }
    avgh1 = 0;
    avgh2 = 0;
    avgh3 = 0;
    avgn = 0;
    max_n = 0;
}

void MeasurePos(){                  //function to measure the current joint angles
    if(CalibratedM){
    counts1 = Encoder1.getPulses();
    counts2 = Encoder2.getPulses();
    q1 = (pi/(double)4)+RadPerCount*counts1;
    q2 = (pi/(double)4)-RadPerCount*counts2;
    }
    else{
        counts1 = Encoder1.getPulses();
    counts2 = Encoder2.getPulses();
    q1 = RadPerCount*counts1;
    q2 = -RadPerCount*counts2;
    }
}

void ReferencePos(){    //function to turn reference velocities into reference positions
    if(CalibratedM){
    qr[0] += qv[0]*1/fcontrol;
    qr[1] += qv[1]*1/fcontrol;
    }
}

void fmovavg(){         //moving average filter
    double avgm1;
    double avgm2;
    double avgm3;
    avgh1 += emg1_log[max_n];
    avgh2 += emg2_log[max_n];
    avgh3 += emg3_log[max_n];
    if(max_n >= ms){
        avgm1 = emg1_log[avgn];
        avgm2 = emg2_log[avgn];
        avgm3 = emg3_log[avgn];
        emg1_log[avgn] = avgh1/ms;
        emg2_log[avgn] = avgh2/ms;
        emg3_log[avgn] = avgh3/ms;
        avgh1 -= avgm1;
        avgh2 -= avgm2;
        avgh3 -= avgm3;
        avgn++;
    }
}

void EmgMeasure(){    //meet de EMG waarden en stop ze in een array
    if(MeasureBool && CalibratedE){
    emg1_log[max_n] =  fabs(bqc.step(emg1.read()))/highnum1;
    emg2_log[max_n] =  fabs(bqc.step(emg2.read()))/highnum2;
    emg3_log[max_n] =  fabs(bqc.step(emg2.read()))/highnum3;
    fmovavg();
    max_n ++;
    }
    else if(MeasureBool){
    emg1_log[max_n] =  fabs(bqc.step(emg1.read()));
    emg2_log[max_n] =  fabs(bqc.step(emg2.read()));
    emg3_log[max_n] =  fabs(bqc.step(emg3.read()));
    fmovavg();
    max_n++;
    }
}

void MakeEmg(){         //was used during testing.          the function creates a predetermined array that can be analised
    int n;
    for(n =0 ;n<=max_n;n++){
        if((n >= 500) && (n <= 600)){
        emg1_log[n] = 0.3;
        }
        else{
            emg1_log[n] = 0;
        }
    }
}

void LedBlink(){                    //function that regulates the LED colors
    switch(LedState){
        case Green:
            if(BlinkBool){
            led1.write(!led1.read());
            }
            else{
                led1.write(0);
            }
            led2.write(1);
            led3.write(1);
            break;
        case Blue:
            if(BlinkBool){
                led2.write(!led2.read());
            }
            else{
                led2.write(0);
            }            
            led1.write(1);
            led3.write(1);
            break;
        case Red:
            if(BlinkBool){
                led3.write(!led3.read());
            }
            else{
                led3.write(0);
            }            
            led1.write(1);
            led2.write(1);
            break;
        case Yellow:
            if(BlinkBool){
                led3.write(!led3.read());
                led1.write(!led1.read());
            }
            else{
                led3.write(0);
                led1.write(0);
            }            
            led2.write(1);
            break;
        case Purple:
            if(BlinkBool){
                led3.write(!led3.read());
                led2.write(!led1.read());
            }
            else{
                led3.write(0);
                led2.write(0);
            }            
            led1.write(1);
            break;
        default:
            led1.write(1);
            led2.write(1);
            led3.write(1);
            break;
    }
}

void LedSet(){                  //function that changes LED colors based on direction of movement
    if(CurrentDir == Up){
        LedState = Red;
    }
    else if(CurrentDir == Down){
        LedState = Blue;
    }
    else if(CurrentDir == Left){
        LedState = Green;
    }
    else if(CurrentDir == Right){
        LedState = Yellow;
    }
    //LedState = Off;
}

void CheckDIR(){                            //old function that was designed to change direction. IS NOT USED ANYMORE
    if(dir_c >= 0.8 && dir_i<=10){
        if(dir_i >= 1){
            //LedState = Blue;
            LedBlink();
        }
        else if(dir_i >= 5){
            //LedState = Red;
            LedBlink();
        }
        dir_i++;
        }
        else{
        if(dir_i >= 10 && CurrentDir == (Up || Down)){
                    CurrentDir = Left;
                    LedSet();
                    dir_i = 0;
                }
                else if(dir_i >= 10 && CurrentDir == (Left || Right)){
                    CurrentDir = Up;
                    LedSet();
                    dir_i = 0;                    
                }
                else if(dir_i >= 5 && CurrentDir == Left){
                    CurrentDir = Right;
                    LedSet();
                    dir_i = 0;                    
                }
                else if(dir_i >= 5 && CurrentDir == Right){
                    CurrentDir = Left;
                    LedSet();
                    dir_i = 0;                    
                }
                else if(dir_i >= 5 && CurrentDir == Up){
                    CurrentDir = Down;
                    LedSet();
                    dir_i = 0;                    
                }
                else if(dir_i >= 5 && CurrentDir == Down){
                    CurrentDir = Up;
                    LedSet();
                    dir_i = 0;                    
                }
            }
                
        
}

void CheckValue(){              //Searches for the highest value in the emg signal up until now
    setpoint = 0;
    dir_c = 0;
    int n;
    int n1;
    for(n = 0;n<=max_n;n++){
        if(emg1_log[n] >= setpoint){
            setpoint = emg1_log[n];
        }
        
        emg1_log[n] = 0;
        
        
        
        for(n1=0;n1<=max_n;n1++){
            if(emg2_log[n1] >= dir_c){
            dir_c = emg2_log[n1];
        }
        emg2_log[n] = 0;
            
            }
    }
    if(setpoint>1){     //Saturaion
            setpoint = 1;
        }
        if(setpoint<0.2){   //Rest
            setpoint = 0;
        }
    if(dir_c<0.2){   //Rest
            dir_c = 0;
        }
}

void InvKin(){                  //function that converts desired cartesian velocities to reference joint velocities.
    double Inverse_J[2][2] = {{(-cos(q1) + sin(q2))/((cos(q1)*(L1 + L0*cos(q2)) - L1*sin(q1)*sin(q2)))     ,       (L1 + L0*cos(q2) - L1*sin(q1))/(L0*(cos(q1)*(L1 + L0*cos(q2)) - L1*sin(q1)*sin(q2)))}     ,       {sin(q2)/(-cos(q1)*(L1 + L0*cos(q2)) + L1*sin(q1)*sin(q2))     ,       -((L1 + L0*cos(q2))/(L0*(cos(q1)*(L1 + L0*cos(q2)) - L1*sin(q1)*sin(q2))))}};
    switch(CurrentDir){
        case Up:
            qv[0] =  0; //Inverse_J[0][1]*setpoint;
            qv[1] =  setpoint; //Inverse_J[1][1]*setpoint;
            break;
        case Down:
            qv[0] =   0;        //Inverse_J[0][1]*-(setpoint);
            qv[1] =   -setpoint;      //Inverse_J[1][1]*-(setpoint);
            break;
        case Left:
            qv[0] =  -setpoint;  //Inverse_J[0][0]*-(setpoint);
            qv[1] =  0;    //Inverse_J[1][0]*-(setpoint);
            break;
        case Right:
            qv[0] =  setpoint;    //Inverse_J[0][0]*setpoint;
            qv[1] =  0;  //Inverse_J[1][0]*setpoint;
            break;
        default:
            break;
        }
    qv[0] = qv[0]*MaxV;
    qv[1] = qv[1]*MaxV;
}

void CalibrateEmg(){                //function that finds the highest value in the EMG signal in order to normalise it using this value afterwards.
    MeasureBool = false;
    LedState = Off;
    int n;
    int n1;
    for(n = 0;n<=max_n;n++){
        if(emg1_log[n] >= highnum1){
            highnum1 = emg1_log[n];
        }
        emg1_log[n] = 0;
    }
    for(n1 = 0;n<=max_n;n1++){
    if(emg2_log[n1] >= highnum2){
            highnum2 = emg2_log[n1];
        }
        emg2_log[n1] = 0;
        }
    if(highnum1>1){
        highnum1 = 1;
    }
    if(highnum2>1){
        highnum2 = 1;
    }
    max_n = 0;
    /*
    if(highnum1 <= 0.2){
        pc.printf("Done calibrating \r\n%.2f Oei! Dat is jammer \r\n",highnum1);
    }
    else if(highnum1 <= 0.4){
        pc.printf("Done calibrating \r\n%.2f Kan beter he \r\n",highnum1);
    }
    else if(highnum1 <= 0.6){
        pc.printf("Done calibrating \r\n%.2f Komt in de buurt \r\n",highnum1);
    }
    else if(highnum1 <= 0.8){
        pc.printf("Done calibrating \r\n%.2f Prima! \r\n",highnum1);
    }
    else if(highnum1 <= 1){
        pc.printf("Done calibrating \r\n%.2f Lekker Bezig man!\r\n",highnum1);
    }
    */
    
    
    /*
    if(highnum2 <= 0.2){
        pc.printf("Done calibrating \r\n%.2f Oei! Dat is jammer \r\n",highnum2);
    }
    else if(highnum2 <= 0.4){
        pc.printf("Done calibrating \r\n%.2f Kan beter he \r\n",highnum2);
    }
    else if(highnum2 <= 0.6){
        pc.printf("Done calibrating \r\n%.2f Komt in de buurt \r\n",highnum2);
    }
    else if(highnum2 <= 0.8){
        pc.printf("Done calibrating \r\n%.2f Prima! \r\n",highnum2);
    }
    else if(highnum2 <= 1){
        pc.printf("Done calibrating \r\n%.2f Lekker Bezig man!\r\n",highnum2);
    }
    */   
    CalibratedE = true;
    MeasureBool = true;
    wait(0.1f);
    pause_loop = false;
}

void CalibrateMotors(){         //funtion used to calibrate the motors to the reference position after moving towards the mechanical limit.
    static double q1_prev = q1;
    static double q2_prev = q2;
    /*
    if(CalibratedM){
        pc.printf("\r\n %i        %f       %f         %f        %f         %i",Motor2DIR.read(),Motor1PWR.read(), Motor2PWR.read(),q2,qr[1],CalibratedM);
        wait(0.05);
    }
    */
    //pc.printf("\r\n%f",q1);
    MeasurePos();
    if((((fabs(q1_prev-q1)<0.02 && fabs(q2_prev-q2)<0.02) && Homing == false) && Cteller1 >= 2) && !CalibratedM){
        if(Cteller>=3){
        Motor1PWR.write(0);
        Motor2PWR.write(0);
        counts1 = 0;
        counts2 = 0;
        Encoder1.reset();
        Encoder2.reset();
        q1 = 0;
        q2 = 0;
        //pc.printf("\r\n%i",counts1);
        Homing = true;
        Cteller = 0;
        }
        Cteller++;
        Cteller1 = 0;
    }
    else if(Homing){
        logcq = true;
        qr[0] = refpos1;//-1.3962634;
        qr[1] = refpos2;
        //pc.printf("\r\n %i         %f         %f        %f         %i",Motor2DIR.read(),Motor2PWR.read(),q2,qr[1],CalibratedM);
        //wait(0.05);
        if(((fabs(qr[0]-q1)<0.02) && Cteller >=3) && (fabs(qr[1]-q2)<0.02)){
                //pc.printf("\r\n Calibrated\r\n\r\n");
                CalibratedM = true;
                Homing = false;
                Cteller = 0;
        }
        else if(((fabs(qr[0]-q1)<0.02) && (fabs(qr[1]-q2)<0.02)) && !CalibratedM){
            Cteller++;
        }
    }
    if(CalibratedM){
        if(Cteller == 0){
        //pc.printf("\r\n Calibrated\r\n\r\n");
        //wait(0.05f);
        /*
        int n;
        for(n = 0; n <= max_n;n++){
            pc.printf(" %f,",cqlog[n]);
            wait(0.01f);
        }
        */
        max_n = 0;
        
        Motor1PWR.write(0);
        Motor2PWR.write(0);
        counts1 = 0;
        counts2 = 0;
        Encoder1.reset();
        Encoder2.reset();
        q1 = pi/4;
        q2 = pi/4;
        qr[0] = pi/4;
        qr[1] = pi/4;
        
        pause_loop = false;
        }
        Cteller++;
    //pause_loop = false;
    }
    Cteller1 ++;
    q1_prev = q1;
    q2_prev = q2;
}

double PID_Controller(double error, int n){             //this function performs the task of PID controller for both motors
    double fout = 0;
    if(n == 1){
    static double error_integral1 = 0;
    static double error_prev1 = error; // initialization with this value only done once!
    static BiQuad LowPassFilter(0.0640, 0.1279, 0.0640, -1.1683, 0.4241);  
    
    // Proportional part:
    double u_k1 = Kp1 * error;

    // Integral part
    error_integral1 = error_integral1 + error * Ts;
    double u_i1 = Ki1 * error_integral1;

    // Derivative part
    double error_derivative1 = (error - error_prev1)/Ts;
    double filtered_error_derivative1 = LowPassFilter.step(error_derivative1);
    double u_d1 = Kd1 * filtered_error_derivative1;
    error_prev1 = error;
    
    fout = u_k1 + u_i1 + u_d1;
    
    }
    
    if(n == 2){
    static double error_integral2 = 0;
    static double error_prev2 = error; // initialization with this value only done once!
    static BiQuad LowPassFilter(0.0640, 0.1279, 0.0640, -1.1683, 0.4241);  
    
    // Proportional part:
    double u_k2 = Kp2 * error;

    // Integral part
    error_integral2 = error_integral2 + error * Ts;
    double u_i2 = Ki2 * error_integral2;

    // Derivative part
    double error_derivative2 = (error - error_prev2)/Ts;
    double filtered_error_derivative2 = LowPassFilter.step(error_derivative2);
    double u_d2 = Kd2 * filtered_error_derivative2;
    error_prev2 = error;
    
    // Sum all parts and return it
    fout =  u_k2 + u_i2 + u_d2;
    }
    
    // Sum all parts and return it
    if (fabs(error)>=0.01){
    return fout;
    }
    else{
        return 0;
    }
}

void LogSignals(){     //sends a log of th signal to the pc via the serial port
    MeasureBool = false;
    int n;
    for(n = 0;n<=max_n;n++){
        emg1_log[n] = emg1_log[n]/highnum1;
        //pc.printf("%f, ",emg1_log[n]);
        //wait(0.01f);
    }
    pc.printf("\r\n\r\n%i", max_n);
    ClearEmg();
    pause_loop = false;
    wait(0.1f);
}

void CheckDIR1(){               //new function used to check the direction that the arm should move in.
    if(button1.read() == 0){
        if(CurrentDir == Up){
            CurrentDir = Down;
            LedSet();
        }
        else if(CurrentDir == Down){
            CurrentDir = Left;
            LedSet();
        }
        else if(CurrentDir == Left){
            CurrentDir = Right;
            LedSet();
        }
        else if(CurrentDir == Right){
            CurrentDir = Up;
            LedSet();
        }
    }
}

void SetPointJoint(){               //sets the desirdered cartesian velocity depending on the highest measured value in the array up untill now     array is cleared afterwards.
    if(MoveBool){
        MeasureBool = false;
        CheckDIR1();
        if(button2.read() == 0){
        setpoint = 0.2;
    }
    else{
        setpoint = 0;  
        }
       
        //CheckValue();
        //pc.printf("\r\n%f       %f",q1,q2);
        //wait(0.05f);
        ClearEmg();
        MeasureBool = true;
    }
}




void Movemotors(double input1, double input2){          //function to move the two motors given two inputs
    if(input1>0){
        Motor1DIR.write(0);
    }
    else{
        Motor1DIR.write(1);
    }
    if(fabs(input1)>1){
        Motor1PWR.write(1);
    }
    else{
        Motor1PWR.write(fabs(input1));
    }
    if(input2>0){
        Motor2DIR.write(0);
    }
    else{
        Motor2DIR.write(1);
    }
    if(fabs(input2)>1){
        Motor2PWR.write(1);
    }
    else{
        Motor2PWR.write(fabs(input2));
    }
}

void MeasureAndControl(){               //function that serves as the control loop operating at 1000Hz. It compares the current position to the desired position and feeds this error to the PID. Then the PID sends an output to the motors
    if(Homing){
    InvKin();
    ReferencePos();
    MeasurePos();
    if((logcq &&  Cnum >= 10) && !CalibratedM){
        cqlog[max_n] = q2;
        max_n++;
        Cnum = 0;
    }
    double motorinput1 = PID_Controller(qr[0] - q1,1);
    double motorinput2 = PID_Controller(qr[1] - q2,2);
    if(motorinput1> 0.1){
        motorinput1 += 0.45;
    }
    else if(motorinput1 < 0.1){
    motorinput1 -= 0.45;
    }
    if(motorinput2>0.1){
        motorinput2 += 0.1;
    }
    else if(motorinput2 < -0.1){
    motorinput2 -= 0.1;
    }
    Movemotors(motorinput1,motorinput2);
    Cnum ++;
    }
    if(CalibratedM){
        InvKin();
        ReferencePos();
        MeasurePos();
        double motorinput1 = PID_Controller(qr[0] - q1,1);
        double motorinput2 = PID_Controller(qr[1] - q2,2);
        if(motorinput1> 0.05){
            motorinput1 += 0.45;
        }
        else if(motorinput1 < 0.05){
            motorinput1 -= 0.45;
        }
        if(motorinput2>0.05){
            motorinput2 += 045;
        }
        else if(motorinput2 < -0.05){
            motorinput2 -= 0.45;
        }
        Movemotors(motorinput1,motorinput2);
    }
}

void PIDTuning(){           //function that was used to tune the PID using trial and error methods
    if(Pot1*4<1){
        Kp2 += button1*0.1;
        Kp2-= button2*0.1;
    }
    else if(Pot1*4<2){
        Ki2+= button1*0.1;
        Ki2-= button2*0.1;
    }
    else if(Pot1*4<3){
        Kd2 += button1*0.1;
        Kd2  -= button2*0.1;
    }
    else if(Pot1*4<4){
        refpos2 += button1*0.1;
        refpos2  -= button2*0.1;
    }
    if(Cteller == 5){
        printf("\r\n        %f      %f      %f      %f       %f",Kp2,Ki2,Kd2,refpos2,Pot1.read()*4);
        Cteller = 0;
    }
    if(Pot2>0.8f){
        pause_loop = false;
    }
    Cteller++;
}

void StirU(){ // function that was used to stir and move the stirring servo up
        if(stirred == true){
        StirDown.write(0);
        StirUp.write(1);
        pc.printf("Ho");
        wait(0.05f);
        pause_loop = false;
        }
}

void StirD(){           //used to move the stirring servo down
    if(button1.read() == 0){
        StirDown.write(1);
        StirUp.write(0);
    }
        
}


void CancelStir(){              //used to cancel the stirring operation.
        StirDown.write(0);
        StirUp.write(1);
        pause_loop = false;
        stirred = false;
        pc.printf("hee");
        wait(0.05f);
}

void SwitchState(){             //switches to the coffee state
    if((button1.read() == 0) && (button2.read() == 0)){
    pause_loop = false;
    }
}

void ProcessStateMachine(){             //main statemachine
    switch(ProgramState){
    case CalibrateM:
        ScopeTimer.attach(&PIDTuning,0.1);              //tune PID
        while(pause_loop){}
        if(start){
            Motor1DIR.write(1);                     //moving to mechainical limit
            Motor2DIR.write(1);
            Motor1PWR.write(0.55);
            Motor2PWR.write(0.6);
            start = false;
        }
        pause_loop = true;
        //BlinkBool = true;
        LedState = Red;
        LedBlink();                                             //change LED
        ScopeTimer.attach(&CalibrateMotors,0.3);                //move to reference position
        LedTimer.attach(MeasureAndControl,1/fcontrol);          //control loop
    
        while(pause_loop){}
        pause_loop = true;
        if(CalibratedM){
            ProgramState = MandM;                           // go to measure and move STATE
        }
        break;
    case TestStand:                                                      //auxiliary state
        LedState = Purple;
        ScopeTimer.attach(&SetPointJoint,0.2);
        ControlTicker.attach(&LedBlink,0.2);
        while(pause_loop){}
        
        break;
    case CalibrateE:                            //calibrate EMG signals
        LedState = Green;
        pause_loop = true;
        LedTimer.attach(&LedBlink,0.5);
        ScopeTimer.attach(&EmgMeasure,1/fsample);
        //MakeEmg();
        SignalTimeout.attach(&CalibrateEmg,5);
        while(pause_loop){}
        ProgramState = MandM; 
        break;
    case MandM:                             //measure and move state.
        LedState = Off;
        pause_loop = true;
        MoveBool = true;
        LedTimer.attach(&LedBlink,0.2);         //blinks
        ControlTicker.attach(&SwitchState,0.3);     //if buttons are both pressed switches to coffee state
        //SignalTimeout.attach(CancelPause,0.5);
        //SignalTimeout.attach(&LogSignals,5);
        ScopeTimer.attach(&SetPointJoint,0.5);      //determines the setpoint joint velocity. based on button presses. in full design based on EMG1 input
        ControlTicker1.attach(&MeasureAndControl,1/fcontrol);       //the 100HZ control loop
        while(pause_loop){}
        ProgramState = Coffee;          //go to coffee state
        break;
        case Coffee:
        LedState = Purple;       
        LedBlink();
        pause_loop = true;
        LedTimer.attach(&StirD,0.2);            //stir down
        SignalTimeout.attach(&CancelStir,3);            //after 3 seconds stir up
        while(pause_loop){}
        ProgramState = MandM;       // go to measure and move state
        break;
    default:                //state if program is done
        LedState = Off;
        if(statechanged){
        pc.printf("\r\nyeeee\r\n");
        statechanged = false;
        }
        break;
    }
}
    
int main(){
    led1.write(1);          //at the start all LEDs are off
    led2.write(1); 
    led3.write(1);
    StirUp.write(0);
    StirDown.write(0);  //All servos off
    
    pc.baud(115200);        // set baud rate
    Motor1PWR.period_us(60);    // set PWM period
    Motor2PWR.period_us(60);
    bqc.add( &bq1).add( &bq2 ).add( &bq3 ).add( &bq4 ).add( &bq5 );     //make filter cascade
    
    while(1){
        ProcessStateMachine();      //execute statemachine.
     }
}