File content as of revision 16:d60c845e149e:

//libraries
#include "mbed.h"
#include "MODSERIAL.h"
#include "QEI.h"
#include "BiQuad.h"
#include "FastPWM.h"

//~~~~~~~~~~~~ Algemeen ~~~~~~~~~~~~~~
DigitalIn button3(SW3);
DigitalIn button2(SW2);
DigitalIn But2(D12);                    //Button 2 van het motorshield
DigitalIn But1(D13);                    //Button 1 van het motorshield

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

MODSERIAL pc(USBTX, USBRX);
Timer t;                                // State timer, na vijf minuten gaat de state automatisch terug naar een vorige state
Timer t2;                               // Timer voor de grafiek in matlab
int count = 0;                          // Counter voor de grafiek in matlab
float counts = 0;                       // Counter voor de MOVEMENT state, zorgt ervoor dat de tickers niet steeds opnieuw attached worden


//~~~~~~~~ Motor waardes ~~~~~~~~~~~~~~~
DigitalOut direction1(D4);              // Motor 1 is de rotatie motor
FastPWM pwmpin1(D5);
FastPWM pwmpin2(D6);                    // Motor 2 is de motor in de elleboog
DigitalOut direction2(D7);
volatile float pwm1;                    //Deze 'pwm' wordt gemaakt met de potmeters, wordt gebruikt voor het testen, maar niet voor de uiteindelijke besturing
volatile float pwm2;

//Encoder
QEI encoder1 (D1, D0, NC, 1200, QEI::X4_ENCODING);
QEI encoder2 (D3, D2, NC, 4800, QEI::X4_ENCODING);
double Pulses1;                         // Afgelezen Encoder pulses
double motor_position1;                 // De berekende hoek
double Pulses2;
double motor_position2;
double error1;                          // Berekende error tussen de berekende hoek en de referentie hoek
double error2;
double u1;                              // PWM signaal dat naar de motor gaat
double u2;

//Pot meter
AnalogIn pot(A5);                       // De potmeters worden alleen gebruikt bij het testen
AnalogIn pot0(A0);
float Pot2;
float Pot1;

//Ticker
Ticker Pwm;                             // Ticker om de motoren te laten bewegen
Ticker PotRead;                         // Ticker voor de Continuous read functie
Ticker ServoTick;                       // Ticker om een PWM signaal met een periode van 20 ms bij een DigitalOut pin te creeëren

//Servo
DigitalOut myservo1(D8);                //Duim                              
DigitalOut myservo2(D11);               //Pink tot Middel vinger
DigitalOut myservo3(D10);               //wijsvinger

float Periodlength = 0.02;              // de MG996R Servo heeft een PWM periode van 20 ms
double servo_position1;                 // in percentage van 0 tot 1, 0 is met de klok mee, 1 is tegen de klok in.
double servo_position2;
double servo_position3;

// Vinger posities
float Duim_krom = 0.05;
float Duim_recht = 0.85;
float MWP_krom = 0.06;
float MWP_recht = 0.89;
float Wijsvinger_krom = 0.15;
float Wijsvinger_recht = 0.93;





//~~~~~~~~ EMG waardes ~~~~~~~~~~~~~~~~~
//Input
AnalogIn    emg1( A1 ); //Duim
AnalogIn    emg2( A2 ); //Bicep
AnalogIn    emg3( A3 ); //Dorsaal
AnalogIn    emg4( A4 ); //Palmair

// Tickers
Ticker sample_ticker;                   //ticker voor filteren met 1000Hz
Ticker sample_timer;                    //ticker voor printen van de uiteindelijke EMG waardes met 2 Hz
Ticker threshold_check_ticker;          //ticker voor het checken van de threshold met 100Hz
Timer timer_calibration;                //timer voor EMG Kalibratie
double ts = 0.001;                      //tijdsstap

float Input1;                           //Input voor motoren
float Input2;

//Gefilterde EMG signalen
volatile double temp_highest_emg1 = 0;  //Hoogste waarde gevonden tijdens kalibratie
volatile double temp_highest_emg2 = 0;
volatile double temp_highest_emg3 = 0;
volatile double temp_highest_emg4 = 0;

//Percentage van de hoogste waarde waar de bovenste treshold gezet moet worden
double Duim_p_t = 0.5;
double Bicep_p_t = 0.4;
double Dorsaal_p_t = 0.6;
double Palmair_p_t = 0.5;

//Percentage van de hoogste waarde waar de onderste treshold gezet moet worden
double Duim_p_tL = 0.5;
double Bicep_p_tL = 0.4;
double Dorsaal_p_tL = 0.5;
double Palmair_p_tL = 0.5;

// Waarde bovenste treshold waar het signaal overheen moet om de arm te activeren
volatile double threshold1;
volatile double threshold2;
volatile double threshold3;
volatile double threshold4;

// Waarde onderste treshold waar het signaal onder moet om de arm te deactiveren
volatile double threshold1L;
volatile double threshold2L;
volatile double threshold3L;
volatile double threshold4L;

bool thresholdreach1 = false;           // Of de waarde boven de bovenste threshold is geweest
bool thresholdreach2 = false;
bool thresholdreach3 = false;
bool thresholdreach4 = false;

// thresholdreads bools
bool Duim;
bool Bicep;
bool Dorsaal;
bool Palmair;

// filters
volatile double emg1_filtered, emg2_filtered, emg3_filtered, emg4_filtered;

//EMG1
//Highpass vierde orde cutoff 20Hz, Band filter om 49, 50, 51Hz eruit te filteren
BiQuadChain highp1;
BiQuad highp1_1( 0.8485, -1.6969, 0.8485, 1.0000, -1.7783, 0.7924 );
BiQuad highp1_2( 1.0000, -2.0000, 1.0000, 1.0000, -1.8934, 0.9085 );
BiQuad notch1_1( 0.9907, -1.8843, 0.9907, 1.0000, -1.8843, 0.9813 );

//Lowpass first order cutoff 0.4Hz
BiQuad lowp1( 0.0013, 0.0013, 0, 1.0000, -0.9975, 0 );

//EMG2
//Highpass vierde orde cutoff 20Hz, Band filter om 49, 50, 51Hz eruit te filteren
BiQuadChain highp2;
BiQuad highp2_1( 0.8485, -1.6969, 0.8485, 1.0000, -1.7783, 0.7924 );
BiQuad highp2_2( 1.0000, -2.0000, 1.0000, 1.0000, -1.8934, 0.9085 );
BiQuad notch2_1( 0.9907, -1.8843, 0.9907, 1.0000, -1.8843, 0.9813 );

//Lowpass first order cutoff 0.4Hz
BiQuad lowp2( 0.0013, 0.0013, 0, 1.0000, -0.9975, 0 );

//EMG3
//Highpass vierde orde cutoff 20Hz, Band filter om 49, 50, 51Hz eruit te filteren
BiQuadChain highp3;
BiQuad highp3_1( 0.8485, -1.6969, 0.8485, 1.0000, -1.7783, 0.7924 );
BiQuad highp3_2( 1.0000, -2.0000, 1.0000, 1.0000, -1.8934, 0.9085 );
BiQuad notch3_1( 0.9907, -1.8843, 0.9907, 1.0000, -1.8843, 0.9813 );

//Lowpass first order cutoff 0.4Hz
BiQuad lowp3( 0.0013, 0.0013, 0, 1.0000, -0.9975, 0 );

//EMG4
//Highpass vierde orde cutoff 20Hz, Band filter om 49, 50, 51Hz eruit te filteren
BiQuadChain highp4;
BiQuad highp4_1( 0.8485, -1.6969, 0.8485, 1.0000, -1.7783, 0.7924 );
BiQuad highp4_2( 1.0000, -2.0000, 1.0000, 1.0000, -1.8934, 0.9085 );
BiQuad notch4_1( 0.9907, -1.8843, 0.9907, 1.0000, -1.8843, 0.9813 );

//Lowpass first order cutoff 0.4Hz
BiQuad lowp4( 0.0013, 0.0013, 0, 1.0000, -0.9975, 0 );

//~~~~~~~~ Kinematica ~~~~~~~~~~~~~~~~~~
double stap1;                           //Stap die bij de referentie hoek wordt opgeteld bij een EMG signaal van 1
double stap2;
double KPot;

float ElbowReference;                   
float Ellebooghoek1;
float Ellebooghoek2;
float Ellebooghoek3;
float Ellebooghoek4;

float PolsReference;
float Polshoek1;
float Polshoek2;
float Polshoek3;
float Polshoek4;

float Hoeknieuw1;
float Hoeknieuw2;

//Limiet in graden
float lowerlim1 = -900;                 //Rotatie motor gewricht limiet, gelijk aan 180 graden rotatie beide kanten op
float upperlim1 = 900;
float lowerlim2 = 0;                    // Elleboog limiet, van 0 graden (helemaal gebogen) tot 110 graden (helemaal gestrekt)
float upperlim2 = 1500;

//~~~~~~~~ PID controler waardes ~~~~~~
double Kp1 = 12.5;
double Ki1 = 0;
double Kd1 = 1;
double Kp2 = 20;          
double Ki2 = 0;
double Kd2 = 1;
double Ts = 0.0005;                     // tijdstap voor pwm ticker    


// ~~~~~~~~~~~~~~~~~~~ EMG functies ~~~~~~~~~~~~~~~~~~
void emgsample()
{

    // EMG signaal lezen
    double emgread1 = emg1.read();
    double emgread2 = emg2.read();
    double emgread3 = emg3.read();
    double emgread4 = emg4.read();

    // Vierde orde highpass filter + notch filter
    double emg1_highpassed = highp1.step(emgread1);
    double emg2_highpassed = highp2.step(emgread2);
    double emg3_highpassed = highp3.step(emgread3);
    double emg4_highpassed = highp4.step(emgread4);

    //Rectificatie
    double emg1_abs = abs(emg1_highpassed);
    double emg2_abs = abs(emg2_highpassed);
    double emg3_abs = abs(emg3_highpassed);
    double emg4_abs = abs(emg4_highpassed);

    //All EMG abs into lowpass
    emg1_filtered = lowp1.step(emg1_abs);
    emg2_filtered = lowp2.step(emg2_abs);
    emg3_filtered = lowp3.step(emg3_abs);
    emg4_filtered = lowp4.step(emg4_abs);

}

//Check of emg_filtered boven de threshold is
void threshold_check()
{

// EMG1 Check
    if (thresholdreach1 == false) {         //Als emg_filtered nog niet boven de bovenste threshold is geweest
        //bovenste threshold check
        if(emg1_filtered>threshold1) {
            Duim = 1;
            thresholdreach1 = true;

        } else {
            Duim= 0;
        }
    } else {                                //Als emg_filtered boven de bovenste threshold is geweest
        //onderste threshold check
        if(emg1_filtered<threshold1L) {
            Duim = 0;
            thresholdreach1 = false;

        } else {
            Duim= 1;
        }
    }

// EMG2 Check
    if (thresholdreach2 == false) {         //Als emg_filtered nog niet boven de bovenste threshold is geweest
        //bovenste threshold check
        if(emg2_filtered>threshold2) {
            Bicep = 1;
            thresholdreach2 = true;
        } else {
            Bicep= 0;
        }
    } else {                                //Als emg_filtered boven de bovenste threshold is geweest
        //onderste threshold check
        if(emg2_filtered<threshold2L) {
            Bicep = 0;
            thresholdreach2 = false;

        } else {
            Bicep= 1;
        }
    }

// EMG3 Check
    if (thresholdreach3 == false) {         //Als emg_filtered nog niet boven de bovenste threshold is geweest
        //bovenste threshold check
        if(emg3_filtered>threshold3) {
            Dorsaal = 1;
            thresholdreach3 = true;
        } else {
            Dorsaal= 0;
        }
    } else {                                //Als emg_filtered boven de bovenste threshold is geweest
        //onderste threshold check
        if(emg3_filtered<threshold3L) {
            Dorsaal = 0;
            thresholdreach3 = false;

        } else {
            Dorsaal= 1;
        }
    }

// EMG4 Check
    if (thresholdreach4 == false) {         //Als emg_filtered nog niet boven de bovenste threshold is geweest
        //bovenste threshold check
        if(emg4_filtered>threshold4) {
            Palmair = 1;
            thresholdreach4 = true;
        } else {
            Palmair= 0;
        }
    } else {                                //Als emg_filtered boven de bovenste threshold is geweest
        //onderste threshold check
        if(emg4_filtered<threshold4L) {
            Palmair = 0;
            thresholdreach4 = false;

        } else {
            Palmair= 1;
        }
    }

}

//~~~~~~~~~ Functies Kinematica ~~~~~~~~~~~~
float Kinematics1(float Bicep)
{

    if (Bicep == 1 && Duim == 0) {          //Als alleen Bicep flexie wordt afgelezen gaat de waarde van de referentie hoek omhoog             
        stap1 = 450*Ts;
        Hoeknieuw1 = PolsReference + stap1;
        return Hoeknieuw1;
    }

    else if (Bicep == 1 && Duim == 1) {     //Als Bicep en Duim flexie wordt afgelezen gaat de waarde van de referentie hoek omlaag
        stap1 = 450*Ts;
        Hoeknieuw1 = PolsReference - stap1;
        return Hoeknieuw1;
    }

    else {
        return PolsReference;
    }
}
float Kinematics2(bool Dorsaal)
{

    if (Dorsaal == 1 && Duim == 0) {        //Als alleen Dorsaal flexie wordt afgelezen gaat de waarde van de referentie hoek omhoog
        stap2 = 300*Ts;
        Hoeknieuw2 = ElbowReference + stap2;
        return Hoeknieuw2;
    }

    else if (Dorsaal == 1 && Duim == 1) {   //Als Dorsaal en Duim flexie wordt afgelezen gaat de waarde van de referentie hoek omhoog
        stap2 = 300*Ts;
        Hoeknieuw2 = ElbowReference - stap2;
        return Hoeknieuw2;
    }

    else {
        return ElbowReference;
    }
}

float Limits1(float Polshoek2)
{

    if (Polshoek2 <= upperlim1 && Polshoek2 >= lowerlim1) {         //Binnen de limieten
        Polshoek3 = Polshoek2;
    }

    else {
        if (Polshoek2 >= upperlim1) {                               //Boven de limiet
            Polshoek3 = upperlim1;
        } else {                                                    //Onder de limiet
            Polshoek3 = lowerlim1;
        }
    }

    return Polshoek3;
}


float Limits2(float Ellebooghoek2)
{

    if (Ellebooghoek2 <= upperlim2 && Ellebooghoek2 >= lowerlim2) { //Binnen de limieten
        Ellebooghoek3 = Ellebooghoek2;
    }

    else {
        if (Ellebooghoek2 >= upperlim2) {                           //Boven de limiet
            Ellebooghoek3 = upperlim2;
        } else {                                                    //Onder de limiet
            Ellebooghoek3 = lowerlim2;
        }
    }

    return Ellebooghoek3;
}


//~~~~~~~~ PID Controller~~~~~~~~~~~~~~~
double PID_controller1(double error1)
{
    static double error1_integral = 0;
    static double error1_prev = error1; // 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 * error1;

    // Integral part
    error1_integral = error1_integral + error1 * Ts;
    double u_i1 = Ki1* error1_integral;

    // Derivative part
    double error1_derivative = (error1 - error1_prev)/Ts;
    double filtered_error1_derivative = LowPassFilter.step(error1_derivative);
    double u_d1 = Kd1 * filtered_error1_derivative;
    error1_prev = error1;

    // Sum all parts and return it
    return u_k1 + u_i1 + u_d1;
}
double PID_controller2(double error2)
{
    static double error2_integral = 0;
    static double error2_prev = error2; // initialization with this value only done once!
    static BiQuad LowPassFilter(0.0640, 0.1279, 0.0640, -1.1683, 0.4241); //(BIQUAD_FILTER_TYPE type, T dbGain, T freq, T srate, T bandwidth);

    // Proportional part:
    double u_k2 = Kp2 * error2;

    // Integral part
    error2_integral = error2_integral + error2 * Ts;
    double u_i2 = Ki2 * error2_integral;

    // Derivative part
    double error2_derivative = (error2 - error2_prev)/Ts;
    double filtered_error2_derivative = LowPassFilter.step(error2_derivative);
    double u_d2 = Kd2 * filtered_error2_derivative;
    error2_prev = error2;

    // Sum all parts and return it
    return u_k2 + u_i2 + u_d2;
}

//~~~~~~~~~~~~ Functies Motor ~~~~~~
void moter1_control(double u1)
{
    direction1= u1 > 0.0f;          //positief = CW
    if (fabs(u1)> 0.7f) {
        u1 = 0.7f;
    } else {
        u1= u1;
    }
    pwmpin1.write(fabs(u1)) ; 
}

void moter2_control(double u2)
{
    direction2= u2 < 0.0f;          //positief = CW
    if (fabs(u2)> 0.99f) {
        u2 = 0.99f;
    } else {
        u2= u2;
    }
    pwmpin2.write(fabs(u2)) ; 
}

void PwmMotor(void)
{
    //Continues Read
    Pot2 = pot.read();           //Potmeters alleen voor testen
    Pot1 = pot0.read();
    pwm2 =(Pot2*2)-1;            //scaling naar -1 tot 1
    pwm1 =(Pot1*2)-1;
    Input1 = Bicep;              //Voor testen hier potmeter waarde 'pwm1'invullen
    Input2 = Palmair;
    
    // Reference hoek berekenen, in graden
    float Polshoek1 = Kinematics1(Input1);
    float Polshoek4 = Limits1(Polshoek1);
    PolsReference = Polshoek4;

    // Reference hoek berekenen, in graden
    float Ellebooghoek1 = Kinematics2(Input2);
    float Ellebooghoek4 = Limits2(Ellebooghoek1);
    ElbowReference = Ellebooghoek4;

    // Positie motor berekenen, in graden
    Pulses1 = encoder1.getPulses();
    motor_position1 = -(Pulses1/1200)*360;
    Pulses2 = encoder2.getPulses();
    motor_position2 = -(Pulses2/4800)*360;
    
    // Error berekenen, naar PID controller en dan naar motor
    double error1 = PolsReference - motor_position1;
    double u1 = PID_controller1(error1);
    moter1_control(u1);

    double error2 = ElbowReference - motor_position2;
    double u2 = PID_controller2(error2);
    moter2_control(u2);

}

void MotorOn(void)
{
    pwmpin1 = 0;
    pwmpin2 = 0;
    Pwm.attach (PwmMotor, Ts);

}

//~~~~~~~~~~ Servo functies ~~~~~~~~~~~~
void servowait1(void)~                  //creeërt een PWM periode van 20 ms met een pulslengte tussen de 500 en 2500 microseconden
{
    double Pulslength1 = 0.0005 + (servo_position1 * 0.002); //in seconden, waarde tussen de 500 en de 2500 microseconden, als de waarde omhoog gaat beweegt de servo tegen de klok in
    myservo1 = true;
    wait(Pulslength1);
    myservo1 = false;
}

void servowait2(void)
{
    double Pulslength2 = 0.0005 + (servo_position2 * 0.002); //in seconden
    myservo2 = true;
    wait(Pulslength2);
    myservo2 = false;
}

void servowait3(void)
{
    double Pulslength3 = 0.0005 + (servo_position3 * 0.002); //in seconden
    myservo3 = true;
    wait(Pulslength3);
    myservo3 = false;
}

void ServoPeriod()
{
    servowait1();
    servowait2();
    servowait3();
}



void ContinuousReader(void)             //leest potmeters af
{
    Pot2 = pot.read();
    Pot1 = pot0.read();
    pwm2 =(Pot2*2)-1;                   //scaling naar -1 tot 1
    pwm1 =(Pot1*2)-1;
}

void sample()                           //Print uiteindelijke EMG output
{
    pc.printf("Duim = %i\r\n", Duim);
    pc.printf("Bicep = %i\r\n",Bicep);
    pc.printf("Dorsaal = %i\r\n", Dorsaal);
    pc.printf("Palmair = %i\r\n", Palmair);

}

// StateMachine

enum states {MOTORS_OFF,CALIBRATION,HOMING1,HOMING2,DEMO,MOVEMENT,FREEZE};
int f = 1;
states currentState = MOTORS_OFF;
bool stateChanged = true; // Make sure the initialization of first state is executed

void ProcessStateMachine(void)
{
    switch (currentState) {
        case MOTORS_OFF:
            // Actions
            if (stateChanged) {
                // state initialization: rood

                led1 = 1;
                led2 = 0;
                led3 = 1;
                wait (1);
                stateChanged = false;
            }

            // State transition logic: Als button 3 word ingedrukt --> calibratie, anders motor uithouden
            if (!button3) {
                currentState = CALIBRATION  ;
                stateChanged = true;
            } else {
                currentState = MOTORS_OFF;
                stateChanged = true;
            }

            break;

        case CALIBRATION:
            // Actions
            if (stateChanged) {
                // state initialization: oranje
                led1 = 0;
                led2 = 0;
                led3 = 1;
                sample_ticker.attach(&emgsample, 0.001);            // Leest het ruwe EMG signaal af met een frequentie van 1000Hz
                timer_calibration.reset();
                timer_calibration.start();
                while(timer_calibration<20) {                           //Binnen 20 seconden alle spieren maximaal aanspannen, hoogste waarde wordt opgeslagen
                    if(timer_calibration>0 && timer_calibration<20) {

                        if(emg1_filtered>temp_highest_emg1) {
                            temp_highest_emg1= emg1_filtered;
                            pc.printf("Highest value Duim= %f \r\n", temp_highest_emg1);
                        }

                        if(emg2_filtered>temp_highest_emg2) {
                            temp_highest_emg2= emg2_filtered;
                            pc.printf("Highest value Bicep= %f \r\n", temp_highest_emg2);
                        }

                        if(emg3_filtered>temp_highest_emg3) {
                            temp_highest_emg3= emg3_filtered;
                            pc.printf("Highest value Dorsaal= %f \r\n", temp_highest_emg3);
                        }

                        if(emg4_filtered>temp_highest_emg4) {
                            temp_highest_emg4= emg4_filtered;
                            pc.printf("Highest value Palmair= %f \r\n", temp_highest_emg4);
                        }

                    }

                }
                pc.printf("threshold calculation\r\n");
                threshold1 = temp_highest_emg1*Duim_p_t;                //Percentage van de hoogste waarde wordt gebruikt om de bovenste en onderste treshold aan te geven
                threshold2 = temp_highest_emg2*Bicep_p_t;
                threshold3 = temp_highest_emg3*Dorsaal_p_t;
                threshold4 = temp_highest_emg4*Palmair_p_t;

                threshold1L = temp_highest_emg1*Duim_p_tL;
                threshold2L = temp_highest_emg2*Bicep_p_tL;
                threshold3L = temp_highest_emg3*Dorsaal_p_tL;
                threshold4L = temp_highest_emg4*Palmair_p_tL;
                stateChanged = false;

            }
            sample_ticker.detach();
            timer_calibration.stop();
            // State transition logic: automatisch naar HOMING 1

            currentState = HOMING1;
            stateChanged = true;
            break;

        case HOMING1:                       //Gebruik de knoppen op het motor shield om het rotatie gewricht naar de beginpositie te brengen
            // Actions
            if (stateChanged) {
                // state initialization: green
                t.start();
                led1 = 0;
                led2 = 1;
                led3 = 1;

                if (!But1) {
                    led1 = 1;
                    float H1 = 0.99f;
                    moter1_control(H1);
                    wait(0.001f);
                } else if (!But2) {
                    led1 = 1;
                    float H1 = -0.99f;
                    moter1_control(H1);
                    wait(0.001f);
                }
                encoder1.reset();
                motor_position1 = 0;
                pwmpin1 = 0;
                pwmpin2 = 0;
                ;

                stateChanged = false;
            }

            // State transition logic: naar HOMING 2(button3), na 5 min naar MOTORS_OFF

            if (!button3) {
                t.stop();
                t.reset();
                currentState = HOMING2  ;
                stateChanged = true;
                wait(1);
            } else if (t>300) {
                t.stop();
                t.reset();
                currentState = MOTORS_OFF  ;
                stateChanged = true;
            } else {
                currentState = HOMING1  ;
                stateChanged = true;
            }
            break;

        case HOMING2:                               // gebruik knoppen op het motor shield om het elleboog gewricht naar de begin positie te brengen ( helemaal gebogen)
            // Actions

            if (stateChanged) {
                // state initialization: white
                t.start();
                led1 = 0;
                led2 = 0;
                led3 = 0;

                if (!But1) {
                    led1 = 1;
                    float H2 = 0.98f;
                    moter2_control(H2);
                    wait(0.001f);
                } else if (!But2) {
                    led1 = 1;
                    float H2 = -0.98f;
                    moter2_control(H2);
                    wait(0.001f);
                }
                encoder2.reset();
                motor_position2 = 0;
                pwmpin1 = 0;
                pwmpin2 = 0;
                ;

                stateChanged = false;
            }

            // State transition logic: naar DEMO (button2), naar MOVEMENT(button3), na 5 min. automatisch naar MOTORS OFF
            if (!button2) {
                currentState = DEMO;
                stateChanged = true;
                t.stop();
                t.reset();
            } else if (!button3) {
                currentState = MOVEMENT  ;
                stateChanged = true;
                t.stop();
                t.reset();
            } else if (t>300) {
                t.stop();
                t.reset();
                currentState = MOTORS_OFF  ;
                stateChanged = true;
            } else {
                currentState = HOMING2  ;
                stateChanged = true;
            }
            break;

        case DEMO:                              // Mogelijkheid om hier je demo routine in te zetten
            // Actions
            if (stateChanged) {
                // state initialization: light blue
                led1 = 0;
                led2 = 1;
                led3 = 0;

                servo_position1 = 0.5;
                servo_position2 = 0.5;
                servo_position3 = 0.5;
                ServoTick.attach(&ServoPeriod, Periodlength);

                wait(1.5 );
                servo_position1 = 0.9;
                servo_position2 = 0.9;
                servo_position3 = 0.9;
                wait(1.5);
                servo_position1 = 0.1;
                servo_position2 = 0.1;
                servo_position3 = 0.1;
                wait(1);

                ServoTick.detach();


                wait (1);

                stateChanged = false;
            }

            // State transition logic: automatisch terug naar HOMING2
            currentState = HOMING2;
            stateChanged = true;
            break;

        case MOVEMENT:                              // Hier worden de motoren bestuurd met EMG input
            // Actions

            if (stateChanged) {
                // state initialization: purple
                t.start();          // na 5 minuten terug naar Homing
                led1 = 1;
                led2 = 0;
                led3 = 0;

                // Tickers aan
                if (counts == 0) {
                    pwmpin1 = 0;
                    pwmpin2 = 0;
                    Input1 = pwm1;
                    Input2 = pwm2;
                    
                    Pwm.attach (PwmMotor, Ts);                          // Motoren gaan aan
                    sample_ticker.attach(&emgsample, 0.001);            // Leest het ruwe EMG signaal af met een frequentie van 1000Hz
                    threshold_check_ticker.attach(&threshold_check, 0.01);  //Checkt of threshold overgeschreden wordt
                    sample_timer.attach(&sample, 0.5);                  // print de EMG waardes
                    servo_position1 = Duim_krom;
                    servo_position2 = MWP_krom;
                    servo_position3 = Wijsvinger_krom;

                    ServoTick.attach(&ServoPeriod, Periodlength);       //Servo's gaan aan
                    counts++;
                    wait(1);                                            //Zorgt ervoor dat hij niet doorschiet naar de volgende staat als je te lang de knop indrukt
                }

                // Servo positie

                if (Dorsaal == 1 && Duim == 0) {                       
                    servo_position1 = Duim_krom;
                    servo_position2 = MWP_krom;
                    servo_position3 = Wijsvinger_krom;
                    led1 = !led1;
                }
                if (Dorsaal == 1 && Duim == 1) {
                    servo_position1 = Duim_recht;
                    servo_position2 = MWP_recht;
                    servo_position3 = Wijsvinger_recht;
                    led1 = !led1;
                }

                stateChanged = false;
            }

            // State transition logic: naar FREEZE (button2), naar MOTORS_OFF(button3), na vijf min automatisch naar HOMING, anders naar MOVEMENT
            if (!button2) {
                currentState = FREEZE;
                stateChanged = true;
                threshold_check_ticker.detach();
                t.stop();
                t.reset();
            } else if (!button3) {
                Pwm.detach ();
                ServoTick.detach();
                sample_ticker.detach();
                threshold_check_ticker.detach();
                sample_timer.detach();
                t.stop();
                t.reset();
                pwmpin2 = 0;
                pwmpin1 = 0;
                counts = 0;
                currentState = MOTORS_OFF  ;
                stateChanged = true;
            } else if (t>300) {
                t.stop();
                t.reset();
                Pwm.detach ();
                ServoTick.detach();
                sample_ticker.detach();
                sample_timer.detach();
                threshold_check_ticker.detach();
                counts = 0;
                currentState = HOMING1  ;
                stateChanged = true;
            } else {
                currentState = MOVEMENT  ;
                stateChanged = true;
            }
            break;

        case FREEZE:                                        //Koppelt motoren los van EMG input, de motoren zullen niet meer bewegen
            // Actions
            if (stateChanged) {
                // state initialization: blue
                Duim = 0;
                Bicep = 0;
                Dorsaal = 0;
                Palmair = 0;
                led1 = 1;
                led2 = 1;
                led3 = 0;
                wait(1);
                stateChanged = false;
            }

            // State transition logic: button 2 terug naar MOVEMENT.
            if (!button2) {
                currentState = MOVEMENT;
                stateChanged = true;
                counts = 0;
            } else {
                currentState = FREEZE  ;
                stateChanged = false;
            }

            break;

    }
}

int main()
{

    t2.start();                                 //Timer voor printen naar matlab
    int counter = 0;                            //Counter voor printen naar matlab
    pwmpin1.period_us(60);                      // PWM periode voor motoren is 60 microseconden
    //PotRead.attach(ContinuousReader,Ts);      //Gebruikt in test fase voor aflezen potmeters
    pc.baud(115200);                            // om naar de PC te printen

//BiQuad Chain add (voegt de filters samen tot een reeks)
    highp1.add( &highp1_1 ).add( &highp1_2 ).add( &notch1_1 );
    highp2.add( &highp2_1 ).add( &highp2_2 ).add( &notch2_1 );
    highp3.add( &highp3_1 ).add( &highp3_2 ).add( &notch3_1 );
    highp4.add( &highp4_1 ).add( &highp4_2 ).add( &notch4_1 );

    while(true) {
        led1 = 1;
        led2 =1;
        led3 =1;
        /* //Dit is voor het printen naar matlab
        if(counter==10)
            {
            float tmp = t2.read();
            printf("%f,%f,%f,%f\n\r",tmp,motor_position2,ElbowReference,Pulses2);
            counter = 0;
            }
        counter++;
        */
        ProcessStateMachine();              //Zet de statemachine aan


        wait(0.001);
    }
}