Diff: main.cpp

Revision:

0:73dcc8fb9900

Child:

1:1c22ce9f370b

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/main.cpp	Fri Nov 01 08:24:02 2013 +0000
@@ -0,0 +1,536 @@
+#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/  *
+*                                                                              *
+********************************************************************************/
+
+// dit is voor schakelaar die aan en uit wil gaan
+DigitalIn toggle(PTD7);
+
+void toggle_on()
+{
+}
+
+void toggle_off()
+{
+    // do nothing
+}
+
+/** 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 vis-*/
+/** ible 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
+    float emg_value_biceps, emg_value_triceps, emg_value_flexoren, emg_value_extensoren, dy;
+    AnalogIn    emg_biceps(PTB0); //Analog input
+    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 definieren
+    
+    //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
+    /*Potmeter input*/
+    AnalogIn potmeterA(PTC2);
+    AnalogIn potmeterB(PTB2);
+    /* Encoder, using my encoder library */
+    /* First pin should be PTDx or PTAx  */
+    /* because those pins can be used as */
+    /* InterruptIn                       */
+    Encoder motorA(PTD4,PTC8);
+    Encoder motorB(PTD0,PTD2);
+    /* MODSERIAL to get non-blocking Serial*/
+    MODSERIAL pc(USBTX,USBRX);
+    /* PWM control to motor */
+    PwmOut pwm_motorA(PTA12);
+    PwmOut pwm_motorB(PTA5);
+    /* Direction pin */
+    DigitalOut motordirA(PTD3);
+    DigitalOut motordirB(PTD1);
+    /* 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;
+
+    const float dt = 0.002;
+    float Kp = 0.001;  //0.0113
+    float Ki = 0.0759;
+    float Kd = 0.0004342;
+    float error_t0_A = 0;
+    float error_t0_B = 0;
+    float error_ti_A;
+    float error_ti_B;
+    float error_t_1_A;
+    float error_t_1_B;
+    float P_regelaar_A;
+    float P_regelaar_B;
+    float I_regelaar_A;
+    float I_regelaar_B;
+    float D_regelaar_A;
+    float D_regelaar_B;
+    float output_regelaar_A;
+    float output_regelaar_B;
+    float integral_i_A;
+    float integral_i_B;
+    float integral_0_A = 0;
+    float integral_0_B = 0;    
+
+    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;
+
+    //START OF CODE
+
+    while(1) {
+        while(!toggle);
+        { // wait while toggle == 0
+            toggle_on();
+
+            /*Set the baudrate (use this number in RealTerm too!) */
+            pc.baud(115200);
+
+            // 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.
+
+            motordirA.write(0);
+            pwm_motorA.write(0.08);
+            do {
+                setpoint_beginA = -532;     // rechtsonder positie A4
+                pwm_to_begin_motorA = abs((setpoint_beginA + motorA.getPosition()) *.001);
+                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);
+            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 460.
+
+            motordirB.write(1);
+            pwm_motorB.write(0.08);
+            do {
+                setpoint_beginB = 460;      // rechtsonder positie A4
+                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);
+            pwm_motorB.write(0);
+
+            wait(1);
+
+            // Nu zijn de motoren gekalibreed en staan ze op de startpositie.
+            // 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;
+    emg_value_biceps = ((100*(filter(1))-0.18)/0.49);
+    emg_value_triceps = ((100*(filter(2))-0.18)/0.35);
+    //emg_value_flexoren = 100*filter(3);
+    //emg_value_extensoren = 100*filter(4);
+    
+    if(emg_value_biceps < 0.10){
+        emg_value_biceps=0;
+    }
+    else {
+        emg_value_biceps = emg_value_biceps;
+    }
+    if(emg_value_triceps < 0.20){
+        emg_value_triceps=0;
+    }
+    else {
+        emg_value_triceps=emg_value_triceps;
+    }
+
+
+ 
+    dy = emg_value_biceps-emg_value_triceps;
+    dy=dy*10;
+                if(pc.rxBufferGetSize(0)-pc.rxBufferGetCount() > 30)
+                    pc.printf("%.6f\n",dy);
+                
+                
+                
+                
+                //setpointA = (potmeterA.read()-0.09027)*(631); // bereik van 71 graden             dit afhankelijk van waar nul punt zit en waar heel wil. Dus afh. van EMG lezen bij EMG wordt 0.5 - 0.09027
+                //setpointB = (potmeterB.read())*(415);           // bereik van 46.7 graden
+                //pc.printf("s: %f, %d ", setpointA, motorA.getPosition());
+                //pc.printf("s: %f, %d ", setpointB, motorB.getPosition());
+                
+                setpointB = (dy);
+                //setpointB = (potmeterB.read() - 0.5) * (871/2);
+                // 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(&setpointA, -1105, -474);     // voor motor moet bereik zijn -1105 tot -474
+                keep_in_range(&setpointB, -147, 269);       // voor motor moet bereik zijn -147 tot 269
+
+                // PID regelaar voor motor A
+                //wait(dt);
+                //error_ti_A = setpointA - motorA.getPosition();
+                //P_regelaar_A = Kp * error_ti_A;
+                //D_regelaar_A = Kd * ((error_ti_A - error_t0_A) / dt);
+                //integral_i_A = integral_0_A + (error_ti_A * dt);
+                //I_regelaar_A = Ki * integral_i_A;
+                //integral_0_A = integral_i_A;
+                //error_t0_A = error_ti_A;
+                //output_regelaar_A = P_regelaar_A;
+
+                // PID regelaar voor motor B
+                //wait(dt);
+                //error_ti_B = setpointB - motorB.getPosition();
+                //P_regelaar_B = Kp * error_ti_B;
+                //D_regelaar_B = Kd * ((error_ti_B - error_t0_B) / dt);
+                //integral_i_B = integral_0_B + (error_ti_B * dt);
+                //I_regelaar_B = Ki * integral_i_B;
+                //integral_0_B = integral_i_B;
+                //error_t0_B = error_ti_B;
+                //output_regelaar_B = P_regelaar_B;
+
+                /* This is a PID-action! store in pwm_to_motor */
+                pwm_to_motorA = (setpointA - motorA.getPosition())*.001;        //output_regelaar_A;
+                pwm_to_motorB = (setpointB); //- motorB.getPosition())*.001;        //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));
+            }
+        }
+        while(toggle);
+        {  // wait while toggle == 1
+            toggle_off();
+            pwm_motorA.write(0);
+            pwm_motorB.write(0);
+        }
+    }
+}
+
+
+void keep_in_range(float * in, float min, float max)
+{
+*in > min ? *in < max? : *in = max: *in = min;
+}
+
+
+
+
+
+