File content as of revision 20:14edaecd7413:

//libaries
#include "mbed.h"
#include "BiQuad.h"
#include "HIDScope.h"
#include "encoder.h"
#include "MODSERIAL.h"

//State Machine en calibratie
enum States {Cal1, Cal2, CalEMG, SelectDevice, EMG, Rest, Demo};
States State;
int Counter;
bool Position_controller_on;
double looptime = 0.002f;
double value;
double home1;
DigitalIn button (D1);

//Encoder/motor
double Huidigepositie1;
double Huidigepositie2;
double motorValue1;
double motorValue2;

//globalvariables Motor
Ticker Treecko;             //We make a awesome ticker for our control system
Ticker printer;
PwmOut M1E(D6);             //Biorobotics Motor 1 PWM control of the speed 
PwmOut M2E(D5);
DigitalOut M1D(D7);         //Biorobotics Motor 1 diraction control
Encoder motor1(D13,D12,true);
Encoder motor2(D9,D8,true);
DigitalOut M2D(D4);

//DEMO
AnalogIn potMeter2(A1);
AnalogIn potMeter1(A2);

MODSERIAL pc(USBTX,USBRX);

double PwmPeriod = 1.0/5000.0;           //set up of PWM periode (5000 Hz, want 5000 periodes in 1 seconde)
const double Ts = 0.002f;                   // tickettijd/ sample time
double e_prev = 0; 
double e_int = 0;
double e_prev2 = 0;
double e_int2 = 0;

double tijdstap = 0.002;
volatile double LBF;
volatile double RBF;
volatile double LTF;
volatile double RTF;

//buttons  en leds voor calibration
DigitalIn button1(PTA4);

DigitalOut led(D2);

//MVC for calibration
double MVCLB = 0; double MVCRB = 0; double MVCLT = 0; double MVCRT = 0;
//MEAN for calibration - rest
double RESTMEANLB = 0; double RESTMEANRB =0; double RESTMEANLT = 0; double RESTMEANRT = 0;

double emgMEANSUBLB;double emgMEANSUBRB ;double emgMEANSUBLT ;double emgMEANSUBRT ;
double emgSUMLB;double emgSUMRB;double emgSUMLT;double emgSUMRT;


bool caldone = false; 
int CalibrationSample = 1000; //How long will we calibrate? Timersampletime*Calibrationsample

int Timescalibration = 0;
int TimescalibrationREST = 0;


// Biquad filters voor Left Bicep (LB)
// Biquad filters van respectievelijk Notch, High-pass en Low-pass filter
BiQuad N1LB( 8.63271e-01, -1.39680e+00, 8.63271e-01, -1.39680e+00, 7.26543e-01 );
BiQuadChain NFLB;
BiQuad HP1LB( 9.63001e-01, -9.62990e-01, 0.00000e+00, -9.62994e-01, 0.00000e+00 );
BiQuad HP2LB( 1.00000e+00, -2.00001e+00, 1.00001e+00, -1.96161e+00, 9.63007e-01 ); 
BiQuadChain HPFLB;
BiQuad LP1LB( 2.56971e-06, 2.56968e-06, 0.00000e+00, -9.72729e-01, 0.00000e+00 );
BiQuad LP2LB( 1.00000e+00, 2.00001e+00, 1.00001e+00, -1.97198e+00, 9.72734e-01 );
BiQuadChain LPFLB;

// Biquad filters voor Right Bicep (RB)
// Biquad filters van respectievelijk Notch, High-pass en Low-pass filter
BiQuad N1RB( 8.63271e-01, -1.39680e+00, 8.63271e-01, -1.39680e+00, 7.26543e-01 );
BiQuadChain NFRB;
BiQuad HP1RB( 9.63001e-01, -9.62990e-01, 0.00000e+00, -9.62994e-01, 0.00000e+00 );
BiQuad HP2RB( 1.00000e+00, -2.00001e+00, 1.00001e+00, -1.96161e+00, 9.63007e-01 ); 
BiQuadChain HPFRB;
BiQuad LP1RB( 2.56971e-06, 2.56968e-06, 0.00000e+00, -9.72729e-01, 0.00000e+00 );
BiQuad LP2RB( 1.00000e+00, 2.00001e+00, 1.00001e+00, -1.97198e+00, 9.72734e-01 );
BiQuadChain LPFRB;

// Biquad filters voor Left Tricep (LT)
// Biquad filters van respectievelijk Notch, High-pass en Low-pass filter
BiQuad N1LT( 8.63271e-01, -1.39680e+00, 8.63271e-01, -1.39680e+00, 7.26543e-01 );
BiQuadChain NFLT;
BiQuad HP1LT( 9.63001e-01, -9.62990e-01, 0.00000e+00, -9.62994e-01, 0.00000e+00 );
BiQuad HP2LT( 1.00000e+00, -2.00001e+00, 1.00001e+00, -1.96161e+00, 9.63007e-01 ); 
BiQuadChain HPFLT;
BiQuad LP1LT( 2.56971e-06, 2.56968e-06, 0.00000e+00, -9.72729e-01, 0.00000e+00 );
BiQuad LP2LT( 1.00000e+00, 2.00001e+00, 1.00001e+00, -1.97198e+00, 9.72734e-01 );
BiQuadChain LPFLT;

// Biquad filters voor Left Tricep (RT)
// Biquad filters van respectievelijk Notch, High-pass en Low-pass filter
BiQuad N1RT( 8.63271e-01, -1.39680e+00, 8.63271e-01, -1.39680e+00, 7.26543e-01 );
BiQuadChain NFRT;
BiQuad HP1RT( 9.63001e-01, -9.62990e-01, 0.00000e+00, -9.62994e-01, 0.00000e+00 );
BiQuad HP2RT( 1.00000e+00, -2.00001e+00, 1.00001e+00, -1.96161e+00, 9.63007e-01 ); 
BiQuadChain HPFRT;
BiQuad LP1RT( 2.56971e-06, 2.56968e-06, 0.00000e+00, -9.72729e-01, 0.00000e+00 );
BiQuad LP2RT( 1.00000e+00, 2.00001e+00, 1.00001e+00, -1.97198e+00, 9.72734e-01 );
BiQuadChain LPFRT;

Timer LooptimeEMG; //moetuiteindelijk weg

//filters
double emgNotchLB;
double emgHPLB;
double emgAbsHPLB;
double emgLPLB;

double emgNotchRB;
double emgHPRB;
double emgAbsHPRB;
double emgLPRB;

double emgNotchLT;
double emgHPLT;
double emgAbsHPLT;
double emgLPLT;

double emgNotchRT;
double emgHPRT;
double emgAbsHPRT;
double emgLPRT;

double f = 500;       // frequency
double dt = 1/f;      // sample frequency

AnalogIn emgLB(A0);   // EMG lezen
AnalogIn emgRB(A1);
AnalogIn emgLT(A2);
AnalogIn emgRT(A3);

//double MVCLB = 0.3;
//double MVCRB = 0.3;
//double MVCLT = 0.3;
//double MVCRT = 0.3;

// variabelen changePosition
int goalx, goaly;

// variables RKI
double pi = 3.14159265359;
double q1 = (pi/2);    //Reference position hoek 1 in radiance
double q2 = -(pi/2);   //Reference position hoek 2 in radiance
const double L1 = 0.30;        //Length arm 1 in mm
const double L2 = 0.38;        //Length arm 2 in mm
double B1 = 1;          //Friction constant motor 1
double B2 = 1;          //Friction constant motor 2
double K = 1;           //Spring constant movement from end-effector position to setpoint position
double Tijd = 1;           //Timestep value
double Rsx = 0.38;       //Reference x-component of the setpoint radius
double Rsy = 0.30;       //Reference y-component of the setpoint radius
double refP = 0;      //Reference position motor 1
double refP2 = 0.5*pi; //Reference position motor 2
double Rex = cos(q1)*L1 - sin(q2)*L2;   //The x-component of the end-effector radius 
double Rey = sin(q1)*L1 + cos(q2)*L2;   //The y-component of the end-effector radius
double R1x = 0;                         //The x-component of the joint 1 radius
double R1y = 0;                         //The y-component of the joint 1 radius
double R2x = cos(q1)*L1;                //The x-component of the joint 2 radius
double R2y = sin(q1)*L1;                //The y-component of the joint 1 radius   
double Fx = 0;
double Fy = 0;
double Tor1 = 0;
double Tor2 = 0;
double w1= 0;
double w2= 0;

void Filteren() 
{
   // LooptimeEMG.reset();
   // LooptimeEMG.start();
    
    //EMG 1
    
    emgNotchLB = NFLB.step(emgLB.read() );  // Notch filter
    emgHPLB = HPFLB.step(emgNotchLB);         // High-pass filter: also normalises around 0.
    emgAbsHPLB = abs(emgHPLB);            // Take absolute value
    emgLPLB = LPFLB.step(emgAbsHPLB);       // Low-pass filter: creates envelope
    emgMEANSUBLB = emgLPLB - RESTMEANLB;    //substract the restmean value
    LBF = emgLPLB/MVCLB;       // Scale to maximum signal: useful for motor. LBF should now be between 0-1.
    
    emgNotchRB = NFRB.step(emgRB.read());  // Notch filter
    emgHPRB = HPFRB.step(emgNotchRB);       // High-pass filter: also normalises around 0.
    emgAbsHPRB = abs(emgHPRB);            // Take absolute value
    emgLPRB = LPFRB.step(emgAbsHPRB);       // Low-pass filter: creates envelope
    emgMEANSUBLB = emgLPLB - RESTMEANLB;
    RBF = emgLPRB/MVCRB;       // Scale to maximum signal: useful for motor
    
    emgNotchLT = NFLT.step(emgLT.read() );  // Notch filter
    emgHPLT = HPFLT.step(emgNotchLT);       // High-pass filter: also normalises around 0.
    emgAbsHPLT = abs(emgHPLT);            // Take absolute value
    emgLPLT = LPFLT.step(emgAbsHPLT);       // Low-pass filter: creates envelope
    emgMEANSUBLT = emgLPLT - RESTMEANLT;    //substract the restmean value
    LTF = emgLPLT/MVCLT;       // Scale to maximum signal: useful for motor
    
    emgNotchRT = NFRT.step(emgRT.read() );  // Notch filter
    emgHPRT = HPFRT.step(emgNotchRT);       // High-pass filter: also normalises around 0.
    emgAbsHPRT = abs(emgHPRT);            // Take absolute value
    emgLPRT = LPFRT.step(emgAbsHPRT);       // Low-pass filter: creates envelope
    emgMEANSUBRT = emgLPRT - RESTMEANRT;    //substract the restmean value
    RTF = emgLPRT/MVCRT;       // Scale to maximum signal: useful for motor
        
   //if (emgFiltered >1)
    //{
    //    emgFiltered=1.00;
    //}
//pc.printf("emgreadLB = %f , emgFiltered = %f, maxi = %f, loop = %f \r\n, emgreadRB = %f , emgFiltered = %f, maxi = %f \r\n, emgreadLT = %f , emgFiltered = %f, maxi = %f \r\n, emgreadRT = %f , emgFiltered = %f, maxi = %f \r\n",emgLB.read(), LBF, maxiLB,looptime.read(),emgRB.read(), RBF, maxiRB,emgLT.read(), LTF, maxiLT, emgRT.read(), RTF, maxiRT);
    //int maxwaarde = 4096;                   // = 64x64
    //double refP = emgFiltered*maxwaarde;
    //return refP;                            // value between 0 and 4096 
   
}

void CalibrationEMG()
{
    pc.printf("Timescalibration = %i \r\n",Timescalibration);
    Timescalibration++;
    
    if(Timescalibration<2000)
    {
        pc.printf("calibratie rust EMG \r\n");
        led = 1;
       // wait(0.2);
        //led = 0;
        
    emgNotchLB = NFLB.step(emgLB.read() );
    emgHPLB = HPFLB.step(emgNotchLB);   
    emgAbsHPLB = abs(emgHPLB);
    emgLPLB = LPFLB.step(emgAbsHPLB);
    emgSUMLB += emgLPLB;                         //SUM all rest values LB
    
    emgNotchRB = NFRB.step(emgRB.read()); 
    emgHPRB = HPFRB.step(emgNotchRB);
    emgAbsHPRB = abs(emgHPRB);
    emgLPRB = LPFRB.step(emgAbsHPRB);
    emgSUMRB += emgLPRB;                        //SUM all rest values RB
    
    emgNotchLT = NFLT.step(emgLT.read() );
    emgHPLT = HPFLT.step(emgNotchLT);
    emgAbsHPLT = abs(emgHPLT);
    emgLPLT = LPFLT.step(emgAbsHPLT);
    emgSUMLT += emgLPLT;                         //SUM all rest values LT
    
    emgNotchRT = NFRT.step(emgRT.read() );  
    emgHPRT = HPFRT.step(emgNotchRT);       
    emgAbsHPRT = abs(emgHPRT);           
    emgLPRT = LPFRT.step(emgAbsHPRT);
    emgSUMRT += emgLPRT;                         //SUM all rest values RT
    }
    if(Timescalibration==1999)
    {
        /*led = 1;
        wait(0.2);
        led = 0; 
        wait(0.2);
        led = 1;
        wait(0.2);
        */
        led = 0;
        RESTMEANLB = emgSUMLB/Timescalibration; //determine the mean rest value
        RESTMEANRB = emgSUMRB/Timescalibration; //determine the mean rest value
        RESTMEANRT = emgSUMRT/Timescalibration; //determine the mean rest value
        RESTMEANLT = emgSUMLT/Timescalibration; //determine the mean rest value
    }
    if(Timescalibration>2000 && Timescalibration<6000)
    {
        pc.printf("maximum linker biceps \r\n");
        led = 1;
        /*wait(0.2);
        led = 0;
        wait(0.2);
        led = 1;
        wait(0.2);
        led = 0;
        wait(0.2);
        led = 1;
        wait(0.2);
        led = 0;
        */
    emgNotchLB = NFLB.step(emgLB.read() ); 
    emgHPLB = HPFLB.step(emgNotchLB);        
    emgAbsHPLB = abs(emgHPLB);          
    emgLPLB = LPFLB.step(emgAbsHPLB);       
    double emgfinalLB = emgLPLB;
    if (emgfinalLB > MVCLB)
    {                                       //determine what the highest reachable emg signal is
          MVCLB = emgfinalLB;
    }    
    }
    
    if(Timescalibration>6000 && Timescalibration<10000)
    {
        pc.printf(" maximum rechter biceps \r\n");
        /*led = 1;
        wait(0.2);
        led = 0;
        wait(0.2);
        led = 1;
        wait(0.2);
        led = 0;
        wait(0.2);
        led = 1;
        wait(0.2);
        led = 0;
        wait(0.2);
        led = 1;
        wait(0.2);
        */
        led = 0;
    emgNotchRB = NFRB.step(emgRB.read()); 
    emgHPRB = HPFRB.step(emgNotchRB); 
    emgAbsHPRB = abs(emgHPRB);           
    emgLPRB = LPFRB.step(emgAbsHPRB);      
    double emgfinalRB = emgLPRB;
    if (emgfinalRB > MVCRB)
    {                                       //determine what the highest reachable emg signal is
          MVCRB = emgfinalRB;
    }    
    }
    
    if(Timescalibration>10000 && Timescalibration<14000)
    {
        pc.printf("maximum linker triceps \r\n");
        led = 1;
        /*wait(0.2);
        led = 0;
        wait(0.2);
        led = 1;
        wait(0.2);
        led = 0;
        wait(0.2);
        led = 1;
        wait(0.2);
        led = 0;
        wait(0.2);
        led = 1;
        wait(0.2);
        led = 0;
        wait(0.2);
        led = 1;
        wait(0.2);
        led = 0;
        */
    emgNotchLT = NFLT.step(emgLT.read() );
    emgHPLT = HPFLT.step(emgNotchLT);
    emgAbsHPLT = abs(emgHPLT);
    emgLPLT = LPFLT.step(emgAbsHPLT);
    double emgfinalLT = emgLPLT;
    if (emgfinalLT > MVCLT)
    {                                       //determine what the highest reachable emg signal is
          MVCLT = emgfinalLT;
    }    
    }
    
    if(Timescalibration>14000 && Timescalibration<18000)
    {
        pc.printf("maximum rechter triceps");
    emgNotchRT = NFRT.step(emgRT.read() );  
    emgHPRT = HPFRT.step(emgNotchRT);       
    emgAbsHPRT = abs(emgHPRT);           
    emgLPRT = LPFRT.step(emgAbsHPRT);
    double emgfinalRT = emgLPRT;
    if (emgfinalRT > MVCRT)
    {                                       //determine what the highest reachable emg signal is
          MVCRT = emgfinalRT;
    }    
    }
    
    if(Timescalibration>18000)
    {
         pc.printf("calibratie afgelopen");
         State = SelectDevice; 
    }
   // pc.printf("maxi waarde = %f emgfinal = %f \r\n",maxi,emgfinal);
   //}
                                         //PAS ALS DEZE TRUE IS, MOET DE MOTOR PAS BEWEGEN!!!
  //return maxi;
}

void RKI()
{
    Rex = cos(q1)*L1 - sin(q2)*L2;
    Rey = sin(q1)*L1 + cos(q2)*L2;
    R2x = cos(q1)*L1;
    R2y = sin(q1)*L1;
    Fx = (Rsx-Rex)*K;
    Fy = (Rsy-Rey)*K;
    Tor1 = (Rex-R1x)*Fy + (R1y-Rey)*Fx;
    Tor2 = (Rex-R2x)*Fy + (R2y-Rey)*Fx;
    w1 = Tor1/B1;
    w2 = Tor2/B2;
    q1 = q1 + w1*Tijd;
    q2 = q2 + w2*Tijd;
    
    int maxwaarde = 4096;                   // = 64x64
    refP = (((0.5*pi) - q1)/(2*pi))*maxwaarde;
    refP2 = (((-pi) + q1 - q2)/(2*pi))*maxwaarde;    //Get reference positions was eerst 0.5*pi
}

void SetpointRobot()
{    
    double Potmeterwaarde2 = potMeter2.read();
    double Potmeterwaarde1 = potMeter1.read();
    
    if  (Potmeterwaarde2>0.6) {
        Rsx += 0.001;                    //het gaat telkens 1 mm verder wanneer de potmeter boven de 0.6 staat
    }
    else if  (Potmeterwaarde2<0.4) {
        Rsx -= 0.001;                    //het gaat telkens 1 mm terug wanneer de potmeter onder de 0.4 staat
    }
    else {                               //de x-waarde van de setpoint verandert niet
        }

    if (Potmeterwaarde1>0.6) {           //het gaat telkens 1 mm verder wanneer de potmeter boven de 0.6 staat
        Rsy += 0.001;
    }
    else if (Potmeterwaarde1<0.4) {      //het gaat telkens 1 mm terug wanneer de potmeter onder de 0.4
        Rsy -= 0.001;                       
    }
    else {                               //de y-waarde van de setpoint verandert niet
        }
}

double GetReferencePosition() 
{
    double Potmeterwaarde = potMeter2.read(); //naam moet universeel worden
    int maxwaarde = 4096;                   // = 64x64
    double refP = Potmeterwaarde*maxwaarde;
    return refP;                            // value between 0 and 4096 
}

double GetReferencePosition2() 
{
    double potmeterwaarde2 = potMeter1.read();
    int maxwaarde2 = 4096;                   // = 64x64
    double refP2 = potmeterwaarde2*maxwaarde2;
    return refP2;                            // value between 0 and 4096 
}
   
double FeedBackControl(double error, double &e_prev, double &e_int)   // schaalt de snelheid naar de snelheid zodat onze chip het begrijpt (is nog niet in werking)
{
    double kp = 0.0015;                               // kind of scaled.
    double Proportional= kp*error;
    
    double kd = 0.000008;                           // kind of scaled. 
    double VelocityError = (error - e_prev)/Ts; 
    double Derivative = kd*VelocityError;
    e_prev = error;
    
    double ki = 0.0001;                           // kind of scaled.
    e_int = e_int+Ts*error;
    double Integrator = ki*e_int;
    
    
    double motorValue = Proportional + Integrator + Derivative;
    return motorValue;
}

double FeedBackControl2(double error2, double &e_prev2, double &e_int2)   // schaalt de snelheid naar de snelheid zodat onze chip het begrijpt (is nog niet in werking)
{
    double kp2 = 0.002;                             // kind of scaled.
    double Proportional2= kp2*error2;
    
    double kd2 = 0.000008;                           // kind of scaled. 
    double VelocityError2 = (error2 - e_prev2)/Ts; 
    double Derivative2 = kd2*VelocityError2;
    e_prev2 = error2;
    
    double ki2 = 0.00005;                           // kind of scaled.
    e_int2 = e_int2+Ts*error2;
    double Integrator2 = ki2*e_int2;
    
    
    double motorValue2 = Proportional2 + Integrator2 + Derivative2;
    return motorValue2;
    
}

void SetMotor1(double motorValue)
{
    if (motorValue >= 0) {
        M1D = 0;
    }
    else {
        M1D = 1;
    }

    if  (fabs(motorValue) > 1) {
        M1E = 1;                    //de snelheid wordt teruggeschaald naar 8.4 rad/s (maximale snelheid, dus waarde 1)
    }
    else {    
        M1E = fabs(motorValue);      //de absolute snelheid wordt bepaald, de motor staat uit bij een waarde 0
    }
}

void SetMotor2(double motorValue2)
{
    if (motorValue2 >= 0) {
        M2D = 1;
    }
    else {
        M2D = 0;
    }

    if  (fabs(motorValue2) > 1) {
        M2E = 1;                    //de snelheid wordt teruggeschaald naar 8.4 rad/s (maximale snelheid, dus waarde 1)
    }
    else {    
        M2E = fabs(motorValue2);      //de absolute snelheid wordt bepaald, de motor staat uit bij een waarde 0
    }
}

void MeasureAndControl(void)
{
    SetpointRobot(); 
    // RKI aanroepen
    RKI();
    // hier the control of the 1st control system
    //double refP = GetReferencePosition();                    //KOMT UIT RKI
    double Huidigepositie = motor1.getPosition(); 
    double error = (refP - Huidigepositie);// make an error
    double motorValue = FeedBackControl(error, e_prev, e_int);
    SetMotor1(motorValue);
    // hier the control of the 2nd control system
    //double refP2 = GetReferencePosition2(); 
    double Huidigepositie2 = motor2.getPosition(); 
    double error2 = (refP2 - Huidigepositie2);// make an error
    double motorValue2 = FeedBackControl2(error2, e_prev2, e_int2);
    SetMotor2(motorValue2);
    pc.printf("refP = %f, huidigepos = %f, motorvalue = %f, refP2 = %f, huidigepos2 = %f, motorvalue2 = %f \r\n", refP, Huidigepositie, motorValue, refP2, Huidigepositie2, Huidigepositie2);
}
     
void changePosition ()    // DIT MOET NOG HEEL ERG GETUNED WORDEN !!!
{
    if (RBF>0.3) {
        goalx++;    // hoe veel verder gaat hij? 1 cm? 10 cm?
    }
    if (RTF>0.3) {
        goalx--;
    }
    if (LBF>0.3) {
        goaly++;
    }
    if (LTF>0.3) {
        goaly--;
    }
    pc.printf("goalx = %i, goaly = %i\r\n",goalx, goaly);
} 

void Loop_funtion()
{   
    pc.printf("state machine begint \r\n");
    switch(State){
        case Cal1: //Calibration motor 1
        pc.printf("cal1 \r\n");
        State=Cal2;
        // naar achteren bewegen( als voorbeeld Arvid), daarna deze waarde opslaan als offset. Dan bewegen naar home middels PID en verschil encodervalue uiterste stand en home1.
           /* motorValue1 = 0.1f; motorValue2=0;
            M2E = fabs(motorValue2);
            M1E = fabs(motorValue1);
                    
               if (Huidigepositie1== 0)
               {
                                        SetMotor1(value); //value is waarde encoder voor loodrechte hoeken,.
                    //if (fabs(Huidigepositie1<0.01) {
                        State=Cal2;
                    //}
                }
                else {
                    SetMotor1(0);
                    Loop_funtion();
                }*/
            break;
            
        case Cal2: //Calibration motor 2
        State = CalEMG;
               /*  if (Huidigepositie2== 0)
                 {
                       if (Huidigepositie2<0.01){
                       State=CalEMG;
                 }
                 else {
                     SetMotor2(0);
                     Loop_funtion();
                 } */
            break;
        case CalEMG: // Calibration EMG
            CalibrationEMG();   //calculates average EMGFiltered at rest and measures max signal EMGFiltered.
            break;    
        case SelectDevice: //Looks at the difference between current position and home. Select aansturen EMG or buttons
            if (button==1) {
                State=EMG;
            }
            if (button==0) {
                State=Demo;
            }
            break;
        case EMG: //Aansturen met EMG
        pc.printf("EMG begint met aansturen \r\n");
            Filteren();
            changePosition();
            //RKI --> output refP van motor
            MeasureAndControl();
            break;
         case Demo: // Aansturen met toetsenbord
            break;
        }
} 

/*void Tickerfunctie()
{
    //if(caldone == false)
    //{
    pc.printf("emgreadRB = %f , emgFiltered = %f, maxi = %f meanrest = %f\r\n",emgRB.read(), RBF, MVCRB, RESTMEANLB);
    pc.printf("emgreadLB = %f , emgFiltered = %f, maxi = %f, meanrest = %f emgSUMLB %f ,Timescalibration %i\r\n",emgLB.read(), LBF, MVCLB,RESTMEANRB,emgSUMLB, Timescalibration);
    pc.printf("emgreadRT = %f , emgFilteredRT = %f, maxiRT = %f meanrest = %f \r\n",emgRT.read(), RTF, MVCRT,RESTMEANRT);
    pc.printf("emgreadLT = %f , emgFilteredLT = %f, maxiLT = %f meanrest = %f \r\n",emgLT.read(), LTF, MVCLT,RESTMEANLT);
    //}
}
*/
int main()//deze moet ooit nog weg --> pas op voor errors
{
    //voor EMG filteren
    //Left Bicep
    NFLB.add( &N1LB );
    HPFLB.add( &HP1LB ).add( &HP2LB );
    LPFLB.add( &LP1LB ).add( &LP2LB );
    
    //Right Bicep
    NFRB.add( &N1RB );
    HPFRB.add( &HP1RB ).add( &HP2RB );
    LPFRB.add( &LP1RB ).add( &LP2RB );  
    
    //Left Tricep
    NFLT.add( &N1LT );
    HPFLT.add( &HP1LT ).add( &HP2LT );
    LPFLT.add( &LP1LT ).add( &LP2LT ); 
    
    //Right Tricep
    NFRT.add( &N1RT );
    HPFRT.add( &HP1RT ).add( &HP2RT );
    LPFRT.add( &LP1RT ).add( &LP2RT ); 
    
    //voor serial
    pc.baud(115200);
    pc.printf("begint met programma \r\n");
    
    //motor
   M1E.period(PwmPeriod); //set PWMposition at 5000hz
    //Ticker
    //Treecko.attach(MeasureAndControl, tijdstap);   //Elke 1 seconde zorgt de ticker voor het runnen en uitlezen van de verschillende 
                                            //functies en analoge signalen. Veranderingen worden elke 1 seconde doorgevoerd.
  //  printer.attach(Tickerfunctie,0.4);
    
    //State Machine
    State = Cal1;
    Position_controller_on = false;
    Treecko.attach(&Loop_funtion, Ts);
    while(true)
    {   }
    
 //is deze wel nodig?  
 }