File content as of revision 2:55bff07c1058:

#include "mbed.h"
#include "encoder.h"
#include "MODSERIAL.h"

//high pass filter constantes 15Hz cutoff 4e orde
#define NUM0 0.2754     // constante
#define NUM1 -1.1017    // z^-1
#define NUM2 1.6525     // z^-2etc.
#define NUM3 -1.1017
#define NUM4 0.2754

#define DEN0 1          // constante
#define DEN1 -1.5704
#define DEN2 1.2756
#define DEN3 -0.4844
#define DEN4 0.0762

//lowpass filter constantes 40 Hz 4e orde
#define NUM0_2 0.4328   // constante
#define NUM1_2 1.7314   // z^-1
#define NUM2_2 2.5971   // z^-2etc.
#define NUM3_2 1.7314
#define NUM4_2 0.4328


#define DEN0_2 1        // constante
#define DEN1_2 2.3695
#define DEN2_2 2.3140
#define DEN3_2 1.0547
#define DEN4_2 0.1874

//lowpass filter  constantes 3Hz 4e orde
#define NUM0_3 0.0000624 // constante
#define NUM1_3 0.0002495 // z^-1
#define NUM2_3 0.0003743 // z^-2etc.
#define NUM3_3 0.0002495
#define NUM4_3 0.0000624

#define DEN0_3 1        // constante
#define DEN1_3 -3.5078
#define DEN2_3 4.6409
#define DEN3_3 -2.7427
#define DEN4_3 0.6105

/*******************************************************************************
*                                                                              *
*   Code can be found at http://mbed.org/users/vsluiter/code/BMT-K9-Regelaar/  *
*                                                                              *
********************************************************************************/

/** keep_in_range -> float in, and keep_in_range if less than min, or larger than max **/
void keep_in_range(float * in, float min, float max);

/** variable to show when a new loop can be started */
/** volatile means that it can be changed in an interrupt routine, and that that change is visible in the main loop. */

volatile bool looptimerflag;

/** function called by Ticker "looptimer"     */
/** variable 'looptimerflag' is set to 'true' each time the looptimer expires.*/
void setlooptimerflag(void)
{
    looptimerflag = true;
}

//emg variabelen -> analoge input
float emg_value_biceps, emg_value_triceps, emg_value_flexoren, emg_value_extensoren, dy;
AnalogIn    emg_biceps(PTB0);
AnalogIn    emg_triceps(PTB1);
AnalogIn    emg_flexoren(PTB2);
AnalogIn    emg_extensoren(PTB3);

/*
DIT IS DE FILTER FUNCTIE! aanroepen door "filter(signaal nummer)"
    filter(1): biceps meten
    filter(2): triceps meten
    filter(3): flexoren meten
    filter(3): extensoren meten
*/
float filter(int sig_number)
{
    float sig_out;

    // eerst variabelen definiëren

    //biceps
    //filter 1
    float in0_biceps =0;
    static float in1_biceps =0, in2_biceps = 0, in3_biceps = 0, in4_biceps = 0;
    static float out0_biceps = 0, out1_biceps = 0 , out2_biceps = 0, out3_biceps = 0, out4_biceps = 0;
    //filter 2
    float in0_2_biceps =0;
    static float in1_2_biceps =0, in2_2_biceps = 0, in3_2_biceps = 0, in4_2_biceps = 0;
    static float out0_2_biceps = 0, out1_2_biceps = 0 , out2_2_biceps = 0, out3_2_biceps = 0, out4_2_biceps = 0;
    //filter 3
    float in0_3_biceps =0;
    static float in1_3_biceps =0, in2_3_biceps = 0, in3_3_biceps = 0, in4_3_biceps = 0;
    static float out0_3_biceps = 0, out1_3_biceps = 0 , out2_3_biceps = 0, out3_3_biceps = 0, out4_3_biceps = 0;

    //triceps
    //filter 1
    float in0_triceps =0;
    static float in1_triceps =0, in2_triceps = 0, in3_triceps = 0, in4_triceps = 0;
    static float out0_triceps = 0, out1_triceps = 0 , out2_triceps = 0, out3_triceps = 0, out4_triceps = 0;
    //filter 2
    float in0_2_triceps =0;
    static float in1_2_triceps =0, in2_2_triceps = 0, in3_2_triceps = 0, in4_2_triceps = 0;
    static float out0_2_triceps = 0, out1_2_triceps = 0 , out2_2_triceps = 0, out3_2_triceps = 0, out4_2_triceps = 0;
    //filter 3
    float in0_3_triceps =0;
    static float in1_3_triceps =0, in2_3_triceps = 0, in3_3_triceps = 0, in4_3_triceps = 0;
    static float out0_3_triceps = 0, out1_3_triceps = 0 , out2_3_triceps = 0, out3_3_triceps = 0, out4_3_triceps = 0;

    //flexoren
    //filter 1
    float in0_flexoren =0;
    static float in1_flexoren =0, in2_flexoren = 0, in3_flexoren = 0, in4_flexoren = 0;
    static float out0_flexoren = 0, out1_flexoren = 0 , out2_flexoren = 0, out3_flexoren = 0, out4_flexoren = 0;
    //filter 2
    float in0_2_flexoren =0;
    static float in1_2_flexoren =0, in2_2_flexoren = 0, in3_2_flexoren = 0, in4_2_flexoren = 0;
    static float out0_2_flexoren = 0, out1_2_flexoren = 0 , out2_2_flexoren = 0, out3_2_flexoren = 0, out4_2_flexoren = 0;
    //filter 3
    float in0_3_flexoren =0;
    static float in1_3_flexoren =0, in2_3_flexoren = 0, in3_3_flexoren = 0, in4_3_flexoren = 0;
    static float out0_3_flexoren = 0, out1_3_flexoren = 0 , out2_3_flexoren = 0, out3_3_flexoren = 0, out4_3_flexoren = 0;

    //extensoren
    //filter 1
    float in0_extensoren =0;
    static float in1_extensoren =0, in2_extensoren = 0, in3_extensoren = 0, in4_extensoren = 0;
    static float out0_extensoren = 0, out1_extensoren = 0 , out2_extensoren = 0, out3_extensoren = 0, out4_extensoren = 0;
    //filter 2
    float in0_2_extensoren =0;
    static float in1_2_extensoren =0, in2_2_extensoren = 0, in3_2_extensoren = 0, in4_2_extensoren = 0;
    static float out0_2_extensoren = 0, out1_2_extensoren = 0 , out2_2_extensoren = 0, out3_2_extensoren = 0, out4_2_extensoren = 0;
    //filter 3
    float in0_3_extensoren =0;
    static float in1_3_extensoren =0, in2_3_extensoren = 0, in3_3_extensoren = 0, in4_3_extensoren = 0;
    static float out0_3_extensoren = 0, out1_3_extensoren = 0 , out2_3_extensoren = 0, out3_3_extensoren = 0, out4_3_extensoren = 0;


    switch(sig_number) {
        case 1:
            // signaal filteren op 15 Hz HIGHPASS
            in4_biceps = in3_biceps;
            in3_biceps = in2_biceps;
            in2_biceps = in1_biceps;
            in1_biceps = in0_biceps;
            in0_biceps = emg_biceps.read();
            out4_biceps = out3_biceps;
            out3_biceps = out2_biceps;
            out2_biceps = out1_biceps;
            out1_biceps = out0_biceps;
            out0_biceps = (NUM0*in0_biceps + NUM1*in1_biceps + NUM2*in2_biceps + NUM3*in3_biceps + NUM4*in4_biceps - DEN1*out1_biceps - DEN2*out2_biceps - DEN3*out3_biceps - DEN4*out4_biceps ) / DEN0;

            //signaal filteren op 40 HZ LOWPASS
            in4_2_biceps = in3_2_biceps;
            in3_2_biceps = in2_2_biceps;
            in2_2_biceps = in1_2_biceps;
            in1_2_biceps = in0_2_biceps;
            in0_2_biceps = out0_biceps;
            out4_2_biceps = out3_2_biceps;
            out3_2_biceps = out2_2_biceps;
            out2_2_biceps = out1_2_biceps;
            out1_2_biceps = out0_2_biceps;
            out0_2_biceps = (NUM0_2*in0_2_biceps + NUM1_2*in1_2_biceps + NUM2_2*in2_2_biceps + NUM3_2*in3_2_biceps + NUM4_2*in4_2_biceps - DEN1_2*out1_2_biceps - DEN2_2*out2_2_biceps - DEN3_2*out3_2_biceps - DEN4_2*out4_2_biceps ) / DEN0_2;

            //signaal filteren op 5Hz LOWPASS
            in4_3_biceps = in3_3_biceps;
            in3_3_biceps = in2_3_biceps;
            in2_3_biceps = in1_3_biceps;
            in1_3_biceps = in0_3_biceps;
            in0_3_biceps = abs(out0_2_biceps);
            out4_3_biceps = out3_3_biceps;
            out3_3_biceps = out2_3_biceps;
            out2_3_biceps = out1_3_biceps;
            out1_3_biceps = out0_3_biceps;
            out0_3_biceps = (NUM0_3*in0_3_biceps + NUM1_3*in1_3_biceps + NUM2_3*in2_3_biceps + NUM3_3*in3_3_biceps + NUM4_3*in4_3_biceps - DEN1_3*out1_3_biceps - DEN2_3*out2_3_biceps - DEN3_3*out3_3_biceps - DEN4_3*out4_3_biceps ) / DEN0_3;
            sig_out = out0_3_biceps;
            break;
        case 2:
            // signaal filteren op 15 Hz HIGHPASS
            in4_triceps = in3_triceps;
            in3_triceps = in2_triceps;
            in2_triceps = in1_triceps;
            in1_triceps = in0_triceps;
            in0_triceps = emg_triceps.read();
            out4_triceps = out3_triceps;
            out3_triceps = out2_triceps;
            out2_triceps = out1_triceps;
            out1_triceps = out0_triceps;
            out0_triceps = (NUM0*in0_triceps + NUM1*in1_triceps + NUM2*in2_triceps + NUM3*in3_triceps + NUM4*in4_triceps - DEN1*out1_triceps - DEN2*out2_triceps - DEN3*out3_triceps - DEN4*out4_triceps ) / DEN0;

            //signaal filteren op 40 HZ LOWPASS
            in4_2_triceps = in3_2_triceps;
            in3_2_triceps = in2_2_triceps;
            in2_2_triceps = in1_2_triceps;
            in1_2_triceps = in0_2_triceps;
            in0_2_triceps = out0_triceps;
            out4_2_triceps = out3_2_triceps;
            out3_2_triceps = out2_2_triceps;
            out2_2_triceps = out1_2_triceps;
            out1_2_triceps = out0_2_triceps;
            out0_2_triceps = (NUM0_2*in0_2_triceps + NUM1_2*in1_2_triceps + NUM2_2*in2_2_triceps + NUM3_2*in3_2_triceps + NUM4_2*in4_2_triceps - DEN1_2*out1_2_triceps - DEN2_2*out2_2_triceps - DEN3_2*out3_2_triceps - DEN4_2*out4_2_triceps ) / DEN0_2;

            //signaal filteren op 5Hz LOWPASS
            in4_3_triceps = in3_3_triceps;
            in3_3_triceps = in2_3_triceps;
            in2_3_triceps = in1_3_triceps;
            in1_3_triceps = in0_3_triceps;
            in0_3_triceps = abs(out0_2_triceps);
            out4_3_triceps = out3_3_triceps;
            out3_3_triceps = out2_3_triceps;
            out2_3_triceps = out1_3_triceps;
            out1_3_triceps = out0_3_triceps;
            out0_3_triceps = (NUM0_3*in0_3_triceps + NUM1_3*in1_3_triceps + NUM2_3*in2_3_triceps + NUM3_3*in3_3_triceps + NUM4_3*in4_3_triceps - DEN1_3*out1_3_triceps - DEN2_3*out2_3_triceps - DEN3_3*out3_3_triceps - DEN4_3*out4_3_triceps ) / DEN0_3;
            sig_out = out0_3_triceps;
            break;
        case 3:
            // signaal filteren op 15 Hz HIGHPASS
            in4_flexoren = in3_flexoren;
            in3_flexoren = in2_flexoren;
            in2_flexoren = in1_flexoren;
            in1_flexoren = in0_flexoren;
            in0_flexoren = emg_flexoren.read();
            out4_flexoren = out3_flexoren;
            out3_flexoren = out2_flexoren;
            out2_flexoren = out1_flexoren;
            out1_flexoren = out0_flexoren;
            out0_flexoren = (NUM0*in0_flexoren + NUM1*in1_flexoren + NUM2*in2_flexoren + NUM3*in3_flexoren + NUM4*in4_flexoren - DEN1*out1_flexoren - DEN2*out2_flexoren - DEN3*out3_flexoren - DEN4*out4_flexoren ) / DEN0;

            //signaal filteren op 40 HZ LOWPASS
            in4_2_flexoren = in3_2_flexoren;
            in3_2_flexoren = in2_2_flexoren;
            in2_2_flexoren = in1_2_flexoren;
            in1_2_flexoren = in0_2_flexoren;
            in0_2_flexoren = out0_flexoren;
            out4_2_flexoren = out3_2_flexoren;
            out3_2_flexoren = out2_2_flexoren;
            out2_2_flexoren = out1_2_flexoren;
            out1_2_flexoren = out0_2_flexoren;
            out0_2_flexoren = (NUM0_2*in0_2_flexoren + NUM1_2*in1_2_flexoren + NUM2_2*in2_2_flexoren + NUM3_2*in3_2_flexoren + NUM4_2*in4_2_flexoren - DEN1_2*out1_2_flexoren - DEN2_2*out2_2_flexoren - DEN3_2*out3_2_flexoren - DEN4_2*out4_2_flexoren ) / DEN0_2;

            //signaal filteren op 5Hz LOWPASS
            in4_3_flexoren = in3_3_flexoren;
            in3_3_flexoren = in2_3_flexoren;
            in2_3_flexoren = in1_3_flexoren;
            in1_3_flexoren = in0_3_flexoren;
            in0_3_flexoren = abs(out0_2_flexoren);
            out4_3_flexoren = out3_3_flexoren;
            out3_3_flexoren = out2_3_flexoren;
            out2_3_flexoren = out1_3_flexoren;
            out1_3_flexoren = out0_3_flexoren;
            out0_3_flexoren = (NUM0_3*in0_3_flexoren + NUM1_3*in1_3_flexoren + NUM2_3*in2_3_flexoren + NUM3_3*in3_3_flexoren + NUM4_3*in4_3_flexoren - DEN1_3*out1_3_flexoren - DEN2_3*out2_3_flexoren - DEN3_3*out3_3_flexoren - DEN4_3*out4_3_flexoren ) / DEN0_3;
            sig_out = out0_3_flexoren;
            break;
        case 4:
            // signaal filteren op 15 Hz HIGHPASS
            in4_extensoren = in3_extensoren;
            in3_extensoren = in2_extensoren;
            in2_extensoren = in1_extensoren;
            in1_extensoren = in0_extensoren;
            in0_extensoren = emg_extensoren.read();
            out4_extensoren = out3_extensoren;
            out3_extensoren = out2_extensoren;
            out2_extensoren = out1_extensoren;
            out1_extensoren = out0_extensoren;
            out0_extensoren = (NUM0*in0_extensoren + NUM1*in1_extensoren + NUM2*in2_extensoren + NUM3*in3_extensoren + NUM4*in4_extensoren - DEN1*out1_extensoren - DEN2*out2_extensoren - DEN3*out3_extensoren - DEN4*out4_extensoren ) / DEN0;

            //signaal filteren op 40 HZ LOWPASS
            in4_2_extensoren = in3_2_extensoren;
            in3_2_extensoren = in2_2_extensoren;
            in2_2_extensoren = in1_2_extensoren;
            in1_2_extensoren = in0_2_extensoren;
            in0_2_extensoren = out0_extensoren;
            out4_2_extensoren = out3_2_extensoren;
            out3_2_extensoren = out2_2_extensoren;
            out2_2_extensoren = out1_2_extensoren;
            out1_2_extensoren = out0_2_extensoren;
            out0_2_extensoren = (NUM0_2*in0_2_extensoren + NUM1_2*in1_2_extensoren + NUM2_2*in2_2_extensoren + NUM3_2*in3_2_extensoren + NUM4_2*in4_2_extensoren - DEN1_2*out1_2_extensoren - DEN2_2*out2_2_extensoren - DEN3_2*out3_2_extensoren - DEN4_2*out4_2_extensoren ) / DEN0_2;

            //signaal filteren op 5Hz LOWPASS
            in4_3_extensoren = in3_3_extensoren;
            in3_3_extensoren = in2_3_extensoren;
            in2_3_extensoren = in1_3_extensoren;
            in1_3_extensoren = in0_3_extensoren;
            in0_3_extensoren = abs(out0_2_extensoren);
            out4_3_extensoren = out3_3_extensoren;
            out3_3_extensoren = out2_3_extensoren;
            out2_3_extensoren = out1_3_extensoren;
            out1_3_extensoren = out0_3_extensoren;
            out0_3_extensoren = (NUM0_3*in0_3_extensoren + NUM1_3*in1_3_extensoren + NUM2_3*in2_3_extensoren + NUM3_3*in3_3_extensoren + NUM4_3*in4_3_extensoren - DEN1_3*out1_3_extensoren - DEN2_3*out2_3_extensoren - DEN3_3*out3_3_extensoren - DEN4_3*out4_3_extensoren ) / DEN0_3;
            sig_out = out0_3_extensoren;
            break;
    }
    return sig_out;
}

int main()
{
    //LOCAL VARIABLES
    Encoder motorA(PTD4,PTC8);      // encoder, using my encoder library
    Encoder motorB(PTD0,PTD2);
    MODSERIAL pc(USBTX,USBRX);      // MODSERIAL to get non-blocking Serial
    PwmOut pwm_motorA(PTA12);       // PWM control to motor
    PwmOut pwm_motorB(PTA5);        // PWM control to motor
    DigitalOut motordirA(PTD3);     // Direction pin
    DigitalOut motordirB(PTD1);     // Direction pin

    /* variable to store setpoint in */
    float setpointA;
    float setpointB;
    float setpoint_beginA;
    float setpoint_beginB;
    float setpoint_rechtsonderA;
    float setpoint_rechtsonderB;

    /* variable to store pwm value in*/
    float pwm_to_motorA;
    float pwm_to_begin_motorA = 0;
    float pwm_to_begin_motorB = 0;
    float pwm_to_motorB;
    float pwm_to_rechtsonder_motorA;
    float pwm_to_rechtsonder_motorB;

    /* variable for PD controller*/
    const float dt = 0.002;
    float Kp = 0.001;               //0.0208
    float Kd = 0.00004342;          //0.0006897
    float error_t0_A = 0;
    float error_t0_B = 0;
    float error_ti_A;
    float error_ti_B;
    float P_regelaar_A;
    float P_regelaar_B;
    float D_regelaar_A;
    float D_regelaar_B;
    float output_regelaar_A;
    float output_regelaar_B;

    /* variable to store positions in*/
    int32_t positionmotorA_t0;
    int32_t positionmotorB_t0;
    int32_t positionmotorA_t_1;
    int32_t positionmotorB_t_1;
    int32_t positiondifference_motorA;
    int32_t positiondifference_motorB;

    /* inverse kinematica */
    float dy;       //dy waarde tussen -1 en 1     -1 -vmax; 1 vmax
    float dx;       //dx waarde tussen -1 en 1     -1 -vmax; 1 vmax
    const float vmax = 2;          // m/s
    const float delta_t = 0.005;  // 1/samplefrequentie, dus tijd tussen twee meetpunten
    float X_positie;
    float Y_positie;
    float X_positie_begin;
    float Y_positie_begin;
    float puls_motorA;
    float puls_motorB;
    float kwadraat_X_positie;
    float kwadraat_Y_positie;
    float phi_A_pulsen_positie_begin;
    float phi_B_pulsen_positie_begin;
    float phi_A_positie_begin;
    float phi_B_positie_begin;
    float phi_1;
    float lengte_arm = 276;     // in mm anders rare imaginaire getallen
    float phi_A;
    float phi_B;
    float Puls_motorA;
    float Puls_motorB;
    float phi_A_pulsen;
    float phi_B_pulsen;

    //START OF CODE

    pc.baud(115200);                // Set the baudrate (use this number in RealTerm too!)

    // In dit stukje code zorgen we ervoor dat de arm gaat draaien naar rechts en stopt als het tegen het frame komt. Eerst motor B botsen dan motor A botsen.
    // motor B zit onder en motor A zit boven en dus op zijn kop (en dus setpoint moet - zijn).

    motordirB.write(0);
    pwm_motorB.write(.08);
    positionmotorB_t0 = motorB.getPosition();
    do {
        wait(0.2);
        positionmotorB_t_1 = positionmotorB_t0 ;
        positionmotorB_t0 = motorB.getPosition();
        positiondifference_motorB = abs(positionmotorB_t0 - positionmotorB_t_1);
    } while(positiondifference_motorB > 10);
    motorB.setPosition(0);
    pwm_motorB.write(0);

    wait(1);            // willen nu even dat tussen ene actie en andere actie 1 seconde wacht.

    motordirA.write(1);
    pwm_motorA.write(.08);
    positionmotorA_t0 = motorA.getPosition();
    do {
        wait(0.2);
        positionmotorA_t_1 = positionmotorA_t0 ;
        positionmotorA_t0 = motorA.getPosition();
        positiondifference_motorA = abs(positionmotorA_t0 - positionmotorA_t_1);
    } while(positiondifference_motorA > 10);
    motorA.setPosition(0);
    pwm_motorA.write(0);

    wait(1);            // willen nu even dat tussen ene actie en andere actie 1 seconde wacht.

    // Hierna willen we de motor van zijn alleruiterste positie naar de x-as hebben. Hiervoor moet motor A eerst op de x-as worden gezet. Hiervoor moet motor A 4.11 graden (63) naar links.

    motordirA.write(0);
    pwm_motorA.write(.08);
    do {
        setpoint_beginA = -63;      // x-as
        pwm_to_begin_motorA = abs((setpoint_beginA + motorA.getPosition()) *.001);   // + omdat men met een negatieve hoekverdraaiing werkt.
        wait(0.2);
        keep_in_range(&pwm_to_begin_motorA, -1, 1 );
        motordirA.write(0);
        pwm_motorA.write(pwm_to_begin_motorA);
    } while(pwm_to_begin_motorA <= 0);
    motorA.setPosition(0);
    pwm_motorA.write(0);

    wait(1);            // willen nu even dat tussen ene actie en andere actie 1 seconde wacht.

    // hierna moet motor A naar de rechtsonder A4. Motor A 532 (hoek 59.8 graden).

    motordirA.write(0);
    pwm_motorA.write(0.08);
    do {
        setpoint_rechtsonderA = -532;     // rechtsonder positie A4
        pwm_to_rechtsonder_motorA = abs((setpoint_rechtsonderA + motorA.getPosition()) *.001);
        wait(0.2);
        keep_in_range(&pwm_to_rechtsonder_motorA, -1, 1 );
        motordirA.write(0);
        pwm_motorA.write(pwm_to_rechtsonder_motorA);
    } while(pwm_to_rechtsonder_motorA <= 0);
    pwm_motorA.write(0);

    wait(1);

    // Hierna moet motor B 21.6 (192) graden naar links om naar x-as te gaan.

    motordirB.write(1);
    pwm_motorB.write(.08);
    do {
        setpoint_beginB = 192;      // x-as
        pwm_to_begin_motorB = abs((setpoint_beginB - motorB.getPosition()) *.001);
        wait(0.2);
        keep_in_range(&pwm_to_begin_motorB, -1, 1 );
        motordirB.write(1);
        pwm_motorB.write(pwm_to_begin_motorB);
    } while(pwm_to_begin_motorB <= 0);
    motorB.setPosition(0);
    pwm_motorB.write(0);

    wait(1);            // willen nu even dat tussen ene actie en andere actie 1 seconde wacht.

    // Hierna moet motor B van x-as naar de rechtsonder A4 positie. Motor B 268 (30.2 graden).

    motordirB.write(1);
    pwm_motorB.write(0.08);
    do {
        setpoint_rechtsonderB = 268;      // rechtsonder positie A4
        pwm_to_rechtsonder_motorB = abs((setpoint_rechtsonderB - motorB.getPosition()) *.001);
        wait(0.2);
        keep_in_range(&pwm_to_rechtsonder_motorB, -1, 1 );
        motordirB.write(1);
        pwm_motorB.write(pwm_to_rechtsonder_motorB);
    } while(pwm_to_rechtsonder_motorB <= 0);
    pwm_motorB.write(0);

    wait(1);

    // Nu zijn de motoren gekalibreed en staan ze op de startpositie (rechtsonderhoek van A4).
    // Hierna het script dat EMG wordt omgezet in een positie verandering

    /*Create a ticker, and let it call the function 'setlooptimerflag' every 0.01s  */
    Ticker looptimer;
    looptimer.attach(setlooptimerflag,0.01);

    //INFINITE LOOP
    while(1) {

        while(looptimerflag != true);
        looptimerflag = false;

        // HIER EMG!!
        float emg_value_biceps;
        float emg_value_triceps;
        float emg_value_flexoren;
        float emg_value_extensoren;
        float dy;                                               // verschil tussen biceps en triceps, daarmee snelheid en richting aangeven.
        float dx;

        emg_value_biceps = ((100*(filter(1))-0.18)/0.49);       // dit is om waarde tussen 0 en 1 te krijgen. filter(1) zegt biceps, 0.18 offset aftrekken, 0.49 maximale waarde, 100 omdat procent
        emg_value_triceps = ((100*(filter(2))-0.18)/0.35);      // 0.35 maximale waarde van triceps
        //emg_value_flexoren = ((100*filter(3))-0.00000)/0.000);
        //emg_value_extensoren = ((100*filter(4))-0.00000)/0.0000);

        if(emg_value_biceps < 0.10) {                           // lager dan 10% doe dan niks - threshold
            emg_value_biceps=0;
        } else {
            emg_value_biceps = emg_value_biceps;
        }
        if(emg_value_triceps < 0.20) {                          // lager dan 20% doe dan niks - threshold - triceps fluctueren meer
            emg_value_triceps=0;
        } else {
            emg_value_triceps=emg_value_triceps;
        }
        /*if(emg_value_flexoren < 0.10){
            emg_value_flexoren = 0;
            } else {
            emg_value_flexoren = emg_value_flexoren;
            }
            if(emg_value_extensoren<0.20){
            emg_value_extensoren = 0;
            } else {
            emg_value_extensoren = emg_value_extensoren;
            }*/

        dy = emg_value_biceps - emg_value_triceps;
        // dx =  emg_value_flexoren - emg_value_extensoren;
        if(pc.rxBufferGetSize(0)-pc.rxBufferGetCount() > 30)
            pc.printf("%.6f\n",dy);

        // inverse kinematica
        phi_A_pulsen_positie_begin = motorA.getPosition();
        phi_B_pulsen_positie_begin = motorB.getPosition();
        
        phi_A_positie_begin = (360/3200) * phi_A_pulsen_positie_begin;
        phi_B_positie_begin = (360/3200) * phi_B_pulsen_positie_begin;
        
        phi_1 = phi_A_positie_begin - phi_B_positie_begin;
       
        X_positie_begin = 2 * lengte_arm * sin(0.5 * phi_1) * cos(90 - 0.5 * phi_A_positie_begin - 0.5 * phi_B_positie_begin);
        Y_positie_begin = 2 * lengte_arm * sin(0.5 * phi_1) * sin(90 - 0.5 * phi_A_positie_begin - 0.5 * phi_B_positie_begin);
               
        X_positie = dx * vmax * delta_t + X_positie_begin;
        Y_positie = dy * vmax * delta_t + Y_positie_begin;

        kwadraat_X_positie = pow(X_positie,2);
        kwadraat_Y_positie = pow(Y_positie,2);

        phi_A = 180 - acos(sqrt(kwadraat_X_positie+kwadraat_Y_positie)/(2*lengte_arm)) - atan(Y_positie/X_positie);
        phi_B = 180 - phi_A - acos(-(kwadraat_X_positie + kwadraat_Y_positie) / (2 * pow(lengte_arm,2))+1);
        
        phi_A_pulsen = (3200/360) * phi_A;
        phi_B_pulsen = (3200/360) * phi_B;
        
        // motor A moet de hoek altijd binnen 53.4 tot en met 124.3 graden liggen
        // motor B moet de hoek altijd binnen 30.2 tot en met -16.5 graden liggen
        keep_in_range(&phi_A_pulsen, -1104, -474);     // voor motor moet bereik zijn -1104 tot -474
        keep_in_range(&phi_B_pulsen, -146, 268);       // voor motor moet bereik zijn -146 tot 268
        
        Puls_motorA = phi_A_pulsen - phi_A_pulsen_positie_begin;
        Puls_motorB = phi_B_pulsen - phi_B_pulsen_positie_begin;

        //PD regelaar voor motor A
        wait(dt);
        error_ti_A = puls_motorA - motorA.getPosition();
        P_regelaar_A = Kp * error_ti_A;
        D_regelaar_A = Kd * ((error_ti_A - error_t0_A) / dt);
        error_t0_A = error_ti_A;
        output_regelaar_A = P_regelaar_A + D_regelaar_A;

        //PD regelaar voor motor B
        wait(dt);
        error_ti_B = puls_motorB - motorB.getPosition();
        P_regelaar_B = Kp * error_ti_B;
        D_regelaar_B = Kd * ((error_ti_B - error_t0_B) / dt);
        error_t0_B = error_ti_B;
        output_regelaar_B = P_regelaar_B + D_regelaar_B;

        /* This is a PD-action! store in pwm_to_motor */
        pwm_to_motorA = output_regelaar_A;
        pwm_to_motorB = output_regelaar_B;

        keep_in_range(&pwm_to_motorA, -1,1);
        keep_in_range(&pwm_to_motorB, -1,1);

        if(pwm_to_motorA > 0)
            motordirA.write(1);
        else
            motordirA.write(0);
        if(pwm_to_motorB > 0)
            motordirB.write(1);
        else
            motordirB.write(0);

        pwm_motorA.write(abs(pwm_to_motorA));
        pwm_motorB.write(abs(pwm_to_motorB));
    }
}

void keep_in_range(float * in, float min, float max)
{
*in > min ? *in < max? : *in = max: *in = min;
}