Diff: main.cpp

Revision:

37:ea621fdf306a

Parent:

32:d651c23bbb77

Child:

38:fb163733c147

--- a/main.cpp	Wed Oct 30 14:33:50 2019 +0000
+++ b/main.cpp	Tue Nov 05 10:03:58 2019 +0000
@@ -3,110 +3,111 @@
 #include "QEI.h"
 #include "MODSERIAL.h"
 #include "BiQuad.h"
-#include "FastPWM.h"
+//#include "FastPWM.h"
 #define  M_PI 3.14159265358979323846  /* pi */
 #include <math.h>
-#include "Servo.h"
+//#include "Servo.h"
 #include <cmath>
-#include <complex>
+//#include <complex>
 
 Serial pc(USBTX, USBRX);
 
 // TICKERS
-Ticker loop_ticker;
+Ticker loop_ticker;                                                                     // Ticker aanmaken die ervoor zorgt dat de ProcessStateMachine met een frequentie vsn 500 Hz kan worden aangeroepen.
 
 // BENODIGD VOOR PROCESS STATE MACHINE
-enum states {Motors_off, Calib_motor, Calib_EMG, Homing, Operation_mode};
-states currentState = Motors_off;
-bool stateChanged = true; // Make sure the initialization of first state is executed
+enum states {Motors_off, Calib_motor, Calib_EMG, Homing, Operation_mode};               // Alle states definiëren.
+states currentState = Motors_off;                                                       // State waarin wordt begonnen definiëren.
+bool stateChanged = true;                                                               // Toevoegen zodat de initialisatie van de eerste state plaatsvindt.
 
 // INPUTS
-DigitalIn Power_button_pressed(D1); // Geen InterruptIn gebruiken!
+DigitalIn Power_button_pressed(D1);                                                     // Definiëren van alle buttons, we gebruiken hiervoor geen InterruptIn, maar DigitalIn.
 DigitalIn Emergency_button_pressed(D2);
 DigitalIn Motor_calib_button_pressed(SW2);
+DigitalIn Homing_button_pressed(SW3);
 
-AnalogIn EMG_biceps_right_raw (A0);
-AnalogIn EMG_biceps_left_raw (A1);
+AnalogIn EMG_biceps_right_raw (A0);                                                     // Definiëren van de ruwe EMG-signalen die binnenkomen via AnalogIn.
+AnalogIn EMG_biceps_left_raw (A1);                                                      // We gebruiken signalen van de kuit en de linker en rechter biceps.
 AnalogIn EMG_calf_raw (A2);
 
-QEI Encoder1(D12, D13, NC, 8400, QEI::X2_ENCODING); //Checken of die D12, D9 etc wel kloppen, 8400= gear ratio x 64
-QEI Encoder2(D9, D10, NC, 8400, QEI::X2_ENCODING);
-
+QEI Encoder1(D13, D12, NC, 64);                                                         // Definities voor de encoders op motor 1 (Encoder1) en 2 (Encoder2). Hiervoor wordt de QEI library gebruikt
+QEI Encoder2(D10, D9, NC, 64);                                                          // We gebruiken X2 encoding, wat standaard is en dus niet hoeft worden toegevoegd aan deze defninitie. 
+                                                                                        // Het aantal counts per omwenteling is gelijk aan 64.
 // OUTPUTS
-PwmOut motor1(D6);  // Misschien moeten we hiervoor DigitalOut gebruiken, moet
-PwmOut motor2(D5);  // samen kunnen gaan met de servo motor
+PwmOut motor1(D6);                                                                      // Definities voor de motorsnelheden door middel van PwmOut. Er kan een getal tussen 0 en 1 worden ingevoerd.
+PwmOut motor2(D5);
 
-DigitalOut motor1_dir(D7);
-DigitalOut motor2_dir(D4);
+DigitalOut motor1_dir(D7);                                                              // Definities voor de richtingen van de motoren. Het getal 0 zorgt voor de ene richting, het getal 1 voor de andere. 
+DigitalOut motor2_dir(D4);                                                              // In ons geval zijn beide motoren rechtsom draaiend vanaf de assen bekeken, wanneer de richting op 1 wordt gezet.
 
 // VARIABELEN VOOR ENCODER, MOTORHOEK ETC.
-int pulses_M1;
-int pulses_M2;
-int counts1;
-int counts2;
-const double conversion_factor = (2.0*M_PI)/(64.0*131.25*2.0);
-double theta_h_1_rad;
-double theta_h_2_rad;
+double counts1;                                                                         // Global variables definiëren voor het aantal counts dat uit de encoder komt en een begindefinitie voor 
+double counts2;                                                                         // de offset opstellen.
+double offset1 = 0.0;
+double offset2 = 0.0;
+const double conversion_factor = (2.0*M_PI)/((64.0*131.25)/2);                          // Omrekeningsfactor om de encoder counts om te zetten naar de huidige motorhoek.
+double theta_h_1_rad;                                                                   // Actuele motorhoek in radialen (motor 1).
+double theta_h_2_rad;                                                                   // Actuele motorhoek in radialen (motor 2).
 
 // DEFINITIES VOOR FILTERS
 
 // BICEPS-RECHTS
-// Definities voor eerste BiQuadChain (High-pass en Notch)
+// Definities voor eerste BiQuadChain (High-pass en Notch) opstellen
 BiQuadChain bqcbr;
-BiQuad bqbr1(0.8006, -1.6012, 0.8006, -1.5610, 0.6414); // High-pass
-BiQuad bqbr2(1, -1.6180, 1, -1.6019, 0.9801); // Notch
-// Na het nemen van de absolute waarde moet de tweede BiQuadChain worden toegepast.
-// Definieer (twee Low-pass -> vierde orde verkrijgen):
+BiQuad bqbr1(0.8006, -1.6012, 0.8006, -1.5610, 0.6414);                                 // High-pass filter met een cut off frequentie van 25 Hz.
+BiQuad bqbr2(1, -1.6180, 1, -1.6019, 0.9801);                                           // Notch filter met een frequentie van 50 Hz en een notchwidth van 0.01 Hz.
+// Na het nemen van de absolute waarde (later) moet de tweede BiQuadChain worden 
+// toegepast. Definieer (twee Low-pass filters-> vierde orde filter verkrijgen):
 BiQuadChain bqcbr2;
-BiQuad bqbr3(1.5515e-4, 3.1030e-4, 1.5515e-4, -1.9645, 0.9651); // Low-pass
-BiQuad bqbr4(1.5515e-4, 3.1030e-4, 1.5515e-4, -1.9645, 0.9651); // Low-pass
+BiQuad bqbr3(1.5515e-4, 3.1030e-4, 1.5515e-4, -1.9645, 0.9651);                         // Twee low-pass filters met een cut off frequentie van 2 Hz.
+BiQuad bqbr4(1.5515e-4, 3.1030e-4, 1.5515e-4, -1.9645, 0.9651);                    
 
 // BICEPS-LINKS
-// Definities voor eerste BiQuadChain (High-pass en Notch)
+// Definities voor eerste BiQuadChain (High-pass en Notch) opstellen
 BiQuadChain bqcbl;
-BiQuad bqbl1(0.8006, -1.6012, 0.8006, -1.5610, 0.6414); // High-pass
-BiQuad bqbl2(1, -1.6180, 1, -1.6019, 0.9801); // Notch
-// Na het nemen van de absolute waarde moet de tweede BiQuadChain worden toegepast.
-// Definieer (twee Low-pass -> vierde orde verkrijgen):
+BiQuad bqbl1(0.8006, -1.6012, 0.8006, -1.5610, 0.6414);                                 // High-pass filter met een cut off frequentie van 25 Hz.
+BiQuad bqbl2(1, -1.6180, 1, -1.6019, 0.9801);                                           // Notch filter met een frequentie van 50 Hz en een notchwidth van 0.01 Hz.
+// Na het nemen van de absolute waarde (later) moet de tweede BiQuadChain worden 
+// toegepast. Definieer (twee Low-pass filters-> vierde orde filter verkrijgen):
 BiQuadChain bqcbl2;
-BiQuad bqbl3(1.5515e-4, 3.1030e-4, 1.5515e-4, -1.9645, 0.9651); // Low-pass
-BiQuad bqbl4(1.5515e-4, 3.1030e-4, 1.5515e-4, -1.9645, 0.9651); // Low-pass
+BiQuad bqbl3(1.5515e-4, 3.1030e-4, 1.5515e-4, -1.9645, 0.9651);                         // Twee low-pass filters met een cut off frequentie van 2 Hz.
+BiQuad bqbl4(1.5515e-4, 3.1030e-4, 1.5515e-4, -1.9645, 0.9651); 
 
 // KUIT
-// Definities voor eerste BiQuadChain (High-pass en Notch)
+// Definities voor eerste BiQuadChain (High-pass en Notch) opstellen
 BiQuadChain bqck;
-BiQuad bqk1(0.8006, -1.6012, 0.8006, -1.5610, 0.6414); // High-pass
-BiQuad bqk2(1, -1.6180, 1, -1.6019, 0.9801); // Notch
-// Na het nemen van de absolute waarde moet de tweede BiQuadChain worden toegepast.
-// Definieer (twee Low-pass -> vierde orde verkrijgen):
+BiQuad bqk1(0.8006, -1.6012, 0.8006, -1.5610, 0.6414);                                  // High-pass filter met een cut off frequentie van 25 Hz.
+BiQuad bqk2(1, -1.6180, 1, -1.6019, 0.9801);                                            // Notch filter met een frequentie van 50 Hz en een notchwidth van 0.01 Hz.
+// Na het nemen van de absolute waarde (later) moet de tweede BiQuadChain worden 
+// toegepast. Definieer (twee Low-pass filters-> vierde orde filter verkrijgen):
 BiQuadChain bqck2;
-BiQuad bqk3(1.5515e-4, 3.1030e-4, 1.5515e-4, -1.9645, 0.9651); // Low-pass
-BiQuad bqk4(1.5515e-4, 3.1030e-4, 1.5515e-4, -1.9645, 0.9651); // Low-pass
+BiQuad bqk3(1.5515e-4, 3.1030e-4, 1.5515e-4, -1.9645, 0.9651);                          // Twee low-pass filters met een cut off frequentie van 2 Hz.
+BiQuad bqk4(1.5515e-4, 3.1030e-4, 1.5515e-4, -1.9645, 0.9651); 
 
 // VARIABELEN VOOR EMG + FILTEREN
-double filtered_EMG_biceps_right;
+double filtered_EMG_biceps_right_1;                                                     // Definities voor ruwe EMG-signalen, gefilterd met de high-pass en notch filter.
+double filtered_EMG_biceps_left_1;
+double filtered_EMG_calf_1;
+
+double filtered_EMG_biceps_right_abs;                                                   // Definities voor de signalen, waarbij de absolute waarden genomen zijn van de eerste filterketen.
+double filtered_EMG_biceps_left_abs;
+double filtered_EMG_calf_abs;
+
+double filtered_EMG_biceps_right;                                                       // Definities voor de gefilterde EMG-signalen, na de tweede filter keten.
 double filtered_EMG_biceps_left;
 double filtered_EMG_calf;
 
-double filtered_EMG_biceps_right_1;
-double filtered_EMG_biceps_left_1;
-double filtered_EMG_calf_1;
-
-double filtered_EMG_biceps_right_abs;
-double filtered_EMG_biceps_left_abs;
-double filtered_EMG_calf_abs;
-
-double filtered_EMG_biceps_right_total;
-double filtered_EMG_biceps_left_total;
-double filtered_EMG_calf_total;
-
 // Variabelen voor HIDScope
-HIDScope scope(3);
+HIDScope scope(3);                                                                      //  
 
 // VARIABELEN VOOR (INITIATIE VAN) EMG KALIBRATIE LOOP
 bool calib = false;
 static int i_calib = 0;
 
+double filtered_EMG_biceps_right_total;                                                 // Benodigde variabelen voor het berekenen van een gemiddelde maximale EMG-waarde tijdens de EMG-kalibratie.
+double filtered_EMG_biceps_left_total;                                                  // Dit totaal is een sommatie van de signalen over 5 seconden. 
+double filtered_EMG_calf_total;
+
 double mean_EMG_biceps_right;
 double mean_EMG_biceps_left;
 double mean_EMG_calf;
@@ -117,83 +118,53 @@
 double normalized_EMG_calf;
 
 // VARIABELEN VOOR OPERATION MODE (RKI)
-double vx; // Gelijk aan variabele die snelheid aangeeft in EMG (in x-richting)
-double vy; // Gelijk aan variabele die snelheid aangeeft in EMG (in y-richting)
+double vx; // Geeft de 'desired velocity' in x-richting
+double vy; // Geeft de 'desired velocity' in y-richting
+
+double Inverse_jacobian[2][2];
+double desired_velocity[2][1];
 
 const double delta_t = 0.002;
 
-double Joint_1_position_x = 0.0;
-double Joint_2_position_x = 0.0;
-
-double Joint_1_position_y = 0.0;
-double Joint_2_position_y = 0.0;
-
-double Joint_1_position_x_prev = 0.0;
-double Joint_2_position_x_prev = 0.0;
+double Joint_1_position = 0.0;
+double Joint_2_position = 0.0;
 
-double Joint_1_position_y_prev;
-double Joint_2_position_y_prev;
-
-double Joint_velocity_x[2][1] = {{0.0}, {0.0}};
-double Joint_velocity_y[2][1] = {{0.0}, {0.0}};
+double Joint_1_position_prev = 0.0;
+double Joint_2_position_prev = 0.0;
 
-double q1_dot_x;
-double q2_dot_x;
+double Joint_velocity[2][1] = {{0.0}, {0.0}};
 
-double q1_dot_y;
-double q2_dot_y;
+double q1_dot;
+double q2_dot;
 
 double q1;
 double q2;
 
-double Motor_1_position_x = 0.0;
-double Motor_2_position_x = 0.0;
-
-double Motor_1_position_y = 0.0;
-double Motor_2_position_y = 0.0;
+double Motor_1_position = 0.0;
+double Motor_2_position = 0.0;
 
 // VARIABELEN VOOR OPERATION MODE (PI-CONTROLLER)
 
-const double Kp = 17.5;
+const double Kp = 12.5;
 const double Ki = 0.03;
 
-// Voor x-richting
-double theta_k_1_x = 0.0;
-double theta_k_2_x = 0.0;
-
-double error_integral_1_x = 0.0;
-double error_integral_2_x = 0.0;
+double theta_k_1 = 0.0;
+double theta_k_2 = 0.0;
 
-double u_i_1_x;
-double u_i_2_x;
-
-double theta_t_1_x;
-double theta_t_2_x;
+double error_integral_1 = 0.0;
+double error_integral_2 = 0.0;
 
-double error_q1_x;
-double error_q2_x;
-
-double abs_theta_t_1_x;
-double abs_theta_t_2_x;
+double u_i_1;
+double u_i_2;
 
-// Voor y-richting
-double theta_k_1_y = 0.0;
-double theta_k_2_y = 0.0;
-
-double error_integral_1_y = 0.0;
-double error_integral_2_y = 0.0;
+double theta_t_1;
+double theta_t_2;
 
-double u_i_1_y;
-double u_i_2_y;
-
-double theta_t_1_y;
-double theta_t_2_y;
+double error_M1;
+double error_M2;
 
-double error_q1_y;
-double error_q2_y;
-
-double abs_theta_t_1_y;
-double abs_theta_t_2_y;
+double abs_theta_t_1;
+double abs_theta_t_2;
 
 // VOIDS
 
@@ -215,131 +186,163 @@
     pc.printf("Motoren uit functie\r\n");
 }
 
-// Motoren aanzetten
-void motors_on()
+void EMG_calibration()
+{
+    if (i_calib == 0) {
+        filtered_EMG_biceps_right_total=0;
+        filtered_EMG_biceps_left_total=0;
+        filtered_EMG_calf_total=0;
+    }
+    if (i_calib <= 2500) {
+        filtered_EMG_biceps_right_total+=filtered_EMG_biceps_right;
+        filtered_EMG_biceps_left_total+=filtered_EMG_biceps_left;
+        filtered_EMG_calf_total+=filtered_EMG_calf;
+        i_calib++;
+    }
+    if (i_calib > 2500) {
+        mean_EMG_biceps_right=filtered_EMG_biceps_right_total/2500.0;
+        mean_EMG_biceps_left=filtered_EMG_biceps_left_total/2500.0;
+        mean_EMG_calf=filtered_EMG_calf_total/2500.0;
+        pc.printf("Ontspan spieren\r\n");
+        pc.printf("Rechterbiceps_max = %f, Linkerbiceps_max = %f, Kuit_max = %f\r\n", mean_EMG_biceps_right, mean_EMG_biceps_left, mean_EMG_calf);
+        calib = false;
+    }
+}
+
+void Homing_function()
 {
-    motor1.write(0.3);
-    motor1_dir.write(0);
-    motor2.write(0.3);
-    motor2_dir.write(1);
-    // Door motor 1 een richting van 1 te geven en motor 2 een van 0, draaien
-    // beide motoren rechtsom
-    pc.printf("Motoren aan functie\r\n");
+    if (theta_h_1_rad != 0.0) {
+        if (theta_h_1_rad < 0) {
+            motor1.write(0.3);
+            motor1_dir.write(0);
+        } else {
+            motor1.write(0.3);
+            motor1_dir.write(1);
+        }
+    }
+    if (theta_h_1_rad == 0.0) {
+        motor1.write(0);
+    }
+    if (theta_h_2_rad != 0.0) {
+        if (theta_h_2_rad < 0) {
+            motor2.write(0.3);
+            motor2_dir.write(0);
+        } else {
+            motor2.write(0.3);
+            motor2_dir.write(1);
+        }
+    }
+    if (theta_h_2_rad == 0.0) {
+        motor2.write(0);
+    }
 }
 
 void Inverse_Kinematics()
 {
     // Defining joint velocities based on calculations of matlab
-    
-    Joint_velocity_x[0][0] = (vx*(cos(q1+3.141592653589793/6.0)*-8.5E2-sin(q1)*4.25E2+cos(q1)*cos(q2)*2.25E2+cos(q1)*sin(q2)*6.77E2+cos(q2)*sin(q1)*6.77E2-sin(q1)*sin(q2)*2.25E2+sqrt(3.0)*cos(q1)*4.25E2-sqrt(3.0)*cos(q1)*cos(q2)*4.25E2+sqrt(3.0)*sin(q1)*sin(q2)*4.25E2)*(4.0E1/1.7E1))/(cos(q1)*cos(q1+3.141592653589793/6.0)*4.25E2+sin(q1)*sin(q1+3.141592653589793/6.0)*4.25E2-cos(q1)*sin(q2)*sin(q1+3.141592653589793/6.0)*6.77E2-cos(q2)*sin(q1)*sin(q1+3.141592653589793/6.0)*6.77E2+cos(q1+3.141592653589793/6.0)*sin(q1)*sin(q2)*6.77E2+sin(q1)*sin(q2)*sin(q1+3.141592653589793/6.0)*2.25E2-sqrt(3.0)*cos(q1)*sin(q1+3.141592653589793/6.0)*4.25E2+sqrt(3.0)*cos(q1+3.141592653589793/6.0)*sin(q1)*4.25E2-cos(q1)*cos(q2)*cos(q1+3.141592653589793/6.0)*6.77E2-cos(q1)*cos(q2)*sin(q1+3.141592653589793/6.0)*2.25E2+cos(q1)*cos(q1+3.141592653589793/6.0)*sin(q2)*2.25E2+cos(q2)*cos(q1+3.141592653589793/6.0)*sin(q1)*2.25E2+sqrt(3.0)*cos(q1)*cos(q2)*sin(q1+3.141592653589793/6.0)*4.25E2-sqrt(3.0)*cos(q1)*cos(q1+3.141592653589793/6.0)*sin(q2)*4.25E2-sqrt(3.0)*cos(q2)*cos(q1+3.141592653589793/6.0)*sin(q1)*4.25E2-sqrt(3.0)*sin(q1)*sin(q2)*sin(q1+3.141592653589793/6.0)*4.25E2);
-    Joint_velocity_x[1][0] = (vx*(sin(q1)*-4.25E2+cos(q1)*cos(q2)*2.25E2+cos(q1)*sin(q2)*6.77E2+cos(q2)*sin(q1)*6.77E2-sin(q1)*sin(q2)*2.25E2+sqrt(3.0)*cos(q1)*4.25E2-sqrt(3.0)*cos(q1)*cos(q2)*4.25E2+sqrt(3.0)*sin(q1)*sin(q2)*4.25E2)*(-4.0E1/1.7E1))/(cos(q1)*cos(q1+3.141592653589793/6.0)*4.25E2+sin(q1)*sin(q1+3.141592653589793/6.0)*4.25E2-cos(q1)*sin(q2)*sin(q1+3.141592653589793/6.0)*6.77E2-cos(q2)*sin(q1)*sin(q1+3.141592653589793/6.0)*6.77E2+cos(q1+3.141592653589793/6.0)*sin(q1)*sin(q2)*6.77E2+sin(q1)*sin(q2)*sin(q1+3.141592653589793/6.0)*2.25E2-sqrt(3.0)*cos(q1)*sin(q1+3.141592653589793/6.0)*4.25E2+sqrt(3.0)*cos(q1+3.141592653589793/6.0)*sin(q1)*4.25E2-cos(q1)*cos(q2)*cos(q1+3.141592653589793/6.0)*6.77E2-cos(q1)*cos(q2)*sin(q1+3.141592653589793/6.0)*2.25E2+cos(q1)*cos(q1+3.141592653589793/6.0)*sin(q2)*2.25E2+cos(q2)*cos(q1+3.141592653589793/6.0)*sin(q1)*2.25E2+sqrt(3.0)*cos(q1)*cos(q2)*sin(q1+3.141592653589793/6.0)*4.25E2-sqrt(3.0)*cos(q1)*cos(q1+3.141592653589793/6.0)*sin(q2)*4.25E2-sqrt(3.0)*cos(q2)*cos(q1+3.141592653589793/6.0)*sin(q1)*4.25E2-sqrt(3.0)*sin(q1)*sin(q2)*sin(q1+3.141592653589793/6.0)*4.25E2);
+    Inverse_jacobian[0][0] = ((cos(q1+3.141592653589793/6.0)*-8.5E2-sin(q1)*4.25E2+cos(q1)*cos(q2)*2.25E2+cos(q1)*sin(q2)*6.77E2+cos(q2)*sin(q1)*6.77E2-sin(q1)*sin(q2)*2.25E2+sqrt(3.0)*cos(q1)*4.25E2-sqrt(3.0)*cos(q1)*cos(q2)*4.25E2+sqrt(3.0)*sin(q1)*sin(q2)*4.25E2)*(4.0E1/1.7E1))/(cos(q1)*cos(q1+3.141592653589793/6.0)*4.25E2+sin(q1)*sin(q1+3.141592653589793/6.0)*4.25E2-cos(q1)*sin(q2)*sin(q1+3.141592653589793/6.0)*6.77E2-cos(q2)*sin(q1)*sin(q1+3.141592653589793/6.0)*6.77E2+cos(q1+3.141592653589793/6.0)*sin(q1)*sin(q2)*6.77E2+sin(q1)*sin(q2)*sin(q1+3.141592653589793/6.0)*2.25E2-sqrt(3.0)*cos(q1)*sin(q1+3.141592653589793/6.0)*4.25E2+sqrt(3.0)*cos(q1+3.141592653589793/6.0)*sin(q1)*4.25E2-cos(q1)*cos(q2)*cos(q1+3.141592653589793/6.0)*6.77E2-cos(q1)*cos(q2)*sin(q1+3.141592653589793/6.0)*2.25E2+cos(q1)*cos(q1+3.141592653589793/6.0)*sin(q2)*2.25E2+cos(q2)*cos(q1+3.141592653589793/6.0)*sin(q1)*2.25E2+sqrt(3.0)*cos(q1)*cos(q2)*sin(q1+3.141592653589793/6.0)*4.25E2-sqrt(3.0)*cos(q1)*cos(q1+3.141592653589793/6.0)*sin(q2)*4.25E2-sqrt(3.0)*cos(q2)*cos(q1+3.141592653589793/6.0)*sin(q1)*4.25E2-sqrt(3.0)*sin(q1)*sin(q2)*sin(q1+3.141592653589793/6.0)*4.25E2);
+    Inverse_jacobian[0][1] = ((cos(q1)*4.25E2-sin(q1+3.141592653589793/6.0)*8.5E2-cos(q1)*cos(q2)*6.77E2+cos(q1)*sin(q2)*2.25E2+cos(q2)*sin(q1)*2.25E2+sin(q1)*sin(q2)*6.77E2+sqrt(3.0)*sin(q1)*4.25E2-sqrt(3.0)*cos(q1)*sin(q2)*4.25E2-sqrt(3.0)*cos(q2)*sin(q1)*4.25E2)*(4.0E1/1.7E1))/(cos(q1)*cos(q1+3.141592653589793/6.0)*4.25E2+sin(q1)*sin(q1+3.141592653589793/6.0)*4.25E2-cos(q1)*sin(q2)*sin(q1+3.141592653589793/6.0)*6.77E2-cos(q2)*sin(q1)*sin(q1+3.141592653589793/6.0)*6.77E2+cos(q1+3.141592653589793/6.0)*sin(q1)*sin(q2)*6.77E2+sin(q1)*sin(q2)*sin(q1+3.141592653589793/6.0)*2.25E2-sqrt(3.0)*cos(q1)*sin(q1+3.141592653589793/6.0)*4.25E2+sqrt(3.0)*cos(q1+3.141592653589793/6.0)*sin(q1)*4.25E2-cos(q1)*cos(q2)*cos(q1+3.141592653589793/6.0)*6.77E2-cos(q1)*cos(q2)*sin(q1+3.141592653589793/6.0)*2.25E2+cos(q1)*cos(q1+3.141592653589793/6.0)*sin(q2)*2.25E2+cos(q2)*cos(q1+3.141592653589793/6.0)*sin(q1)*2.25E2+sqrt(3.0)*cos(q1)*cos(q2)*sin(q1+3.141592653589793/6.0)*4.25E2-sqrt(3.0)*cos(q1)*cos(q1+3.141592653589793/6.0)*sin(q2)*4.25E2-sqrt(3.0)*cos(q2)*cos(q1+3.141592653589793/6.0)*sin(q1)*4.25E2-sqrt(3.0)*sin(q1)*sin(q2)*sin(q1+3.141592653589793/6.0)*4.25E2);
+    Inverse_jacobian[1][0] = ((sin(q1)*-4.25E2+cos(q1)*cos(q2)*2.25E2+cos(q1)*sin(q2)*6.77E2+cos(q2)*sin(q1)*6.77E2-sin(q1)*sin(q2)*2.25E2+sqrt(3.0)*cos(q1)*4.25E2-sqrt(3.0)*cos(q1)*cos(q2)*4.25E2+sqrt(3.0)*sin(q1)*sin(q2)*4.25E2)*(-4.0E1/1.7E1))/(cos(q1)*cos(q1+3.141592653589793/6.0)*4.25E2+sin(q1)*sin(q1+3.141592653589793/6.0)*4.25E2-cos(q1)*sin(q2)*sin(q1+3.141592653589793/6.0)*6.77E2-cos(q2)*sin(q1)*sin(q1+3.141592653589793/6.0)*6.77E2+cos(q1+3.141592653589793/6.0)*sin(q1)*sin(q2)*6.77E2+sin(q1)*sin(q2)*sin(q1+3.141592653589793/6.0)*2.25E2-sqrt(3.0)*cos(q1)*sin(q1+3.141592653589793/6.0)*4.25E2+sqrt(3.0)*cos(q1+3.141592653589793/6.0)*sin(q1)*4.25E2-cos(q1)*cos(q2)*cos(q1+3.141592653589793/6.0)*6.77E2-cos(q1)*cos(q2)*sin(q1+3.141592653589793/6.0)*2.25E2+cos(q1)*cos(q1+3.141592653589793/6.0)*sin(q2)*2.25E2+cos(q2)*cos(q1+3.141592653589793/6.0)*sin(q1)*2.25E2+sqrt(3.0)*cos(q1)*cos(q2)*sin(q1+3.141592653589793/6.0)*4.25E2-sqrt(3.0)*cos(q1)*cos(q1+3.141592653589793/6.0)*sin(q2)*4.25E2-sqrt(3.0)*cos(q2)*cos(q1+3.141592653589793/6.0)*sin(q1)*4.25E2-sqrt(3.0)*sin(q1)*sin(q2)*sin(q1+3.141592653589793/6.0)*4.25E2);
+    Inverse_jacobian[1][1] = ((cos(q1)*4.25E2-cos(q1)*cos(q2)*6.77E2+cos(q1)*sin(q2)*2.25E2+cos(q2)*sin(q1)*2.25E2+sin(q1)*sin(q2)*6.77E2+sqrt(3.0)*sin(q1)*4.25E2-sqrt(3.0)*cos(q1)*sin(q2)*4.25E2-sqrt(3.0)*cos(q2)*sin(q1)*4.25E2)*(-4.0E1/1.7E1))/(cos(q1)*cos(q1+3.141592653589793/6.0)*4.25E2+sin(q1)*sin(q1+3.141592653589793/6.0)*4.25E2-cos(q1)*sin(q2)*sin(q1+3.141592653589793/6.0)*6.77E2-cos(q2)*sin(q1)*sin(q1+3.141592653589793/6.0)*6.77E2+cos(q1+3.141592653589793/6.0)*sin(q1)*sin(q2)*6.77E2+sin(q1)*sin(q2)*sin(q1+3.141592653589793/6.0)*2.25E2-sqrt(3.0)*cos(q1)*sin(q1+3.141592653589793/6.0)*4.25E2+sqrt(3.0)*cos(q1+3.141592653589793/6.0)*sin(q1)*4.25E2-cos(q1)*cos(q2)*cos(q1+3.141592653589793/6.0)*6.77E2-cos(q1)*cos(q2)*sin(q1+3.141592653589793/6.0)*2.25E2+cos(q1)*cos(q1+3.141592653589793/6.0)*sin(q2)*2.25E2+cos(q2)*cos(q1+3.141592653589793/6.0)*sin(q1)*2.25E2+sqrt(3.0)*cos(q1)*cos(q2)*sin(q1+3.141592653589793/6.0)*4.25E2-sqrt(3.0)*cos(q1)*cos(q1+3.141592653589793/6.0)*sin(q2)*4.25E2-sqrt(3.0)*cos(q2)*cos(q1+3.141592653589793/6.0)*sin(q1)*4.25E2-sqrt(3.0)*sin(q1)*sin(q2)*sin(q1+3.141592653589793/6.0)*4.25E2);
 
-    Joint_velocity_y[0][0] = (vy*(cos(q1)*4.25E2-sin(q1+3.141592653589793/6.0)*8.5E2-cos(q1)*cos(q2)*6.77E2+cos(q1)*sin(q2)*2.25E2+cos(q2)*sin(q1)*2.25E2+sin(q1)*sin(q2)*6.77E2+sqrt(3.0)*sin(q1)*4.25E2-sqrt(3.0)*cos(q1)*sin(q2)*4.25E2-sqrt(3.0)*cos(q2)*sin(q1)*4.25E2)*(4.0E1/1.7E1))/(cos(q1)*cos(q1+3.141592653589793/6.0)*4.25E2+sin(q1)*sin(q1+3.141592653589793/6.0)*4.25E2-cos(q1)*sin(q2)*sin(q1+3.141592653589793/6.0)*6.77E2-cos(q2)*sin(q1)*sin(q1+3.141592653589793/6.0)*6.77E2+cos(q1+3.141592653589793/6.0)*sin(q1)*sin(q2)*6.77E2+sin(q1)*sin(q2)*sin(q1+3.141592653589793/6.0)*2.25E2-sqrt(3.0)*cos(q1)*sin(q1+3.141592653589793/6.0)*4.25E2+sqrt(3.0)*cos(q1+3.141592653589793/6.0)*sin(q1)*4.25E2-cos(q1)*cos(q2)*cos(q1+3.141592653589793/6.0)*6.77E2-cos(q1)*cos(q2)*sin(q1+3.141592653589793/6.0)*2.25E2+cos(q1)*cos(q1+3.141592653589793/6.0)*sin(q2)*2.25E2+cos(q2)*cos(q1+3.141592653589793/6.0)*sin(q1)*2.25E2+sqrt(3.0)*cos(q1)*cos(q2)*sin(q1+3.141592653589793/6.0)*4.25E2-sqrt(3.0)*cos(q1)*cos(q1+3.141592653589793/6.0)*sin(q2)*4.25E2-sqrt(3.0)*cos(q2)*cos(q1+3.141592653589793/6.0)*sin(q1)*4.25E2-sqrt(3.0)*sin(q1)*sin(q2)*sin(q1+3.141592653589793/6.0)*4.25E2);
-    Joint_velocity_y[1][0] = (vy*(cos(q1)*4.25E2-cos(q1)*cos(q2)*6.77E2+cos(q1)*sin(q2)*2.25E2+cos(q2)*sin(q1)*2.25E2+sin(q1)*sin(q2)*6.77E2+sqrt(3.0)*sin(q1)*4.25E2-sqrt(3.0)*cos(q1)*sin(q2)*4.25E2-sqrt(3.0)*cos(q2)*sin(q1)*4.25E2)*(-4.0E1/1.7E1))/(cos(q1)*cos(q1+3.141592653589793/6.0)*4.25E2+sin(q1)*sin(q1+3.141592653589793/6.0)*4.25E2-cos(q1)*sin(q2)*sin(q1+3.141592653589793/6.0)*6.77E2-cos(q2)*sin(q1)*sin(q1+3.141592653589793/6.0)*6.77E2+cos(q1+3.141592653589793/6.0)*sin(q1)*sin(q2)*6.77E2+sin(q1)*sin(q2)*sin(q1+3.141592653589793/6.0)*2.25E2-sqrt(3.0)*cos(q1)*sin(q1+3.141592653589793/6.0)*4.25E2+sqrt(3.0)*cos(q1+3.141592653589793/6.0)*sin(q1)*4.25E2-cos(q1)*cos(q2)*cos(q1+3.141592653589793/6.0)*6.77E2-cos(q1)*cos(q2)*sin(q1+3.141592653589793/6.0)*2.25E2+cos(q1)*cos(q1+3.141592653589793/6.0)*sin(q2)*2.25E2+cos(q2)*cos(q1+3.141592653589793/6.0)*sin(q1)*2.25E2+sqrt(3.0)*cos(q1)*cos(q2)*sin(q1+3.141592653589793/6.0)*4.25E2-sqrt(3.0)*cos(q1)*cos(q1+3.141592653589793/6.0)*sin(q2)*4.25E2-sqrt(3.0)*cos(q2)*cos(q1+3.141592653589793/6.0)*sin(q1)*4.25E2-sqrt(3.0)*sin(q1)*sin(q2)*sin(q1+3.141592653589793/6.0)*4.25E2);
+    desired_velocity[0][0] = vx;
+    desired_velocity[1][0] = vy;
+
+    Joint_velocity[0][0] = Inverse_jacobian[0][0]*desired_velocity[0][0] + Inverse_jacobian[0][1]*desired_velocity[1][0];
+    Joint_velocity[1][0] = Inverse_jacobian[1][0]*desired_velocity[0][0] + Inverse_jacobian[1][1]*desired_velocity[1][0];
 
     // Integratie
-    Joint_1_position_x = Joint_1_position_x_prev + Joint_velocity_x[0][0]*delta_t;
-    Joint_2_position_x = Joint_2_position_x_prev + Joint_velocity_x[1][0]*delta_t;
-
-    Joint_1_position_y = Joint_1_position_y_prev + Joint_velocity_y[0][0]*delta_t;
-    Joint_2_position_y = Joint_2_position_y_prev + Joint_velocity_y[1][0]*delta_t;
-
-    Joint_1_position_x_prev = Joint_1_position_x;
-    Joint_2_position_x_prev = Joint_2_position_x;
+    Joint_1_position = Joint_1_position_prev + Joint_velocity[0][0]*delta_t;
+    Joint_2_position = Joint_2_position_prev + Joint_velocity[1][0]*delta_t;
 
-    Joint_1_position_y_prev = Joint_1_position_y;
-    Joint_2_position_y_prev = Joint_2_position_y;
+    Joint_1_position_prev = Joint_1_position;
+    Joint_2_position_prev = Joint_2_position;
 
-    Motor_1_position_x = Joint_1_position_x;
-    Motor_2_position_x = Joint_1_position_x + Joint_2_position_x;
-
-    Motor_1_position_y = Joint_1_position_y;
-    Motor_2_position_y = Joint_1_position_y + Joint_2_position_y;
+    Motor_1_position = Joint_1_position;
+    Motor_2_position = Joint_1_position + Joint_2_position;
 }
 
-// PI-CONTROLLER X-RICHTING
-void PI_controller_x()
+// PI-CONTROLLER
+void PI_controller()
 {
 // Proportional part:
-    theta_k_1_x= Kp * error_q1_x;
-    theta_k_2_x= Kp * error_q2_x;
+    theta_k_1= Kp * error_M1;
+    theta_k_2= Kp * error_M2;
 
 // Integral part
-    error_integral_1_x = error_integral_1_x + error_q1_x *delta_t;
-    error_integral_2_x = error_integral_2_x + error_q2_x *delta_t;
-    u_i_1_x= Ki * error_integral_1_x;
-    u_i_2_x= Ki * error_integral_2_x;
+    error_integral_1 = error_integral_1+ error_M1*delta_t;
+    error_integral_2 = error_integral_2+ error_M2*delta_t;
+    u_i_1= Ki * error_integral_1;
+    u_i_2= Ki * error_integral_2;
 
-// sum all parts and return it
-    theta_t_1_x = theta_k_1_x + u_i_1_x;
-    theta_t_2_x = theta_k_2_x + u_i_2_x;
+// Sum all parts and return it
+    theta_t_1= theta_k_1 + u_i_1;
+    theta_t_2= theta_k_2 + u_i_2;
 }
 
-// PI-CONTROLLER Y-RICHTING
-void PI_controller_y()
+void Define_motor_dir()
 {
-// Proportional part:
-    theta_k_1_y= Kp * error_q1_y;
-    theta_k_2_y= Kp * error_q2_y;
-
-// Integral part
-    error_integral_1_y = error_integral_1_y + error_q1_y *delta_t;
-    error_integral_2_y = error_integral_2_y + error_q2_y *delta_t;
-    u_i_1_y= Ki * error_integral_1_y;
-    u_i_2_y= Ki * error_integral_2_y;
-
-// sum all parts and return it
-    theta_t_1_y = theta_k_1_y + u_i_1_y;
-    theta_t_2_y = theta_k_2_y + u_i_2_y;
-}
-
-void Define_motor_dir_x()
-{
-    if (theta_t_1_x >= 0 && theta_t_2_x >= 0) {
+    if (theta_t_1 >= 0 && theta_t_2 >= 0) {
+        motor1_dir.write(0);
+        motor2_dir.write(0);
+    }
+    if (theta_t_1 < 0 && theta_t_2 >= 0) {
+        motor1_dir.write(1);
+        motor1_dir.write(0);
+    }
+    if (theta_t_1 >= 0 && theta_t_2 < 0) {
         motor1_dir.write(0);
         motor2_dir.write(1);
-    }
-    if (theta_t_1_x < 0 && theta_t_2_x >= 0) {
-        motor1_dir.write(1);
-        motor1_dir.write(1);
-    }
-    if (theta_t_1_x >= 0 && theta_t_2_x < 0) {
-        motor1_dir.write(0);
-        motor2_dir.write(0);
     } else {
         motor1_dir.write(1);
-        motor2_dir.write(0);
+        motor2_dir.write(1);
     }
 }
 
-void Define_motor_dir_y()
+void Controlling_system()
 {
-    if (theta_t_1_y >= 0 && theta_t_2_y >= 0) {
-        motor1_dir.write(0);
-        motor2_dir.write(1);
-    }
-    if (theta_t_1_y < 0 && theta_t_2_y >= 0) {
-        motor1_dir.write(1);
-        motor1_dir.write(1);
-    }
-    if (theta_t_1_y >= 0 && theta_t_2_y < 0) {
-        motor1_dir.write(0);
-        motor2_dir.write(0);
-    } else {
-        motor1_dir.write(1);
-        motor2_dir.write(0);
-    }
+    Inverse_Kinematics();
+
+    error_M1 = Motor_1_position + theta_h_1_rad;
+    error_M2 = Motor_2_position + theta_h_2_rad;
+
+    PI_controller();
+
+    abs_theta_t_1 = abs(theta_t_1);
+    abs_theta_t_2 = abs(theta_t_2);
+
+    motor1.write(abs_theta_t_1);
+    motor2.write(abs_theta_t_2);
+    Define_motor_dir();
 }
+
+// Aanmaken van een bool om te testen of de berekeningen in the ProcessStatemachine
+// meer tijd kosten dan wordt gegeven door de ticker. Dit zou mogelijk het encoder
+// probleem kunnen verklaren. Indien er te weinig tijd is, zou de loop zichzelf in moeten
+// halen. Start met een bool die true is, stel deze gelijk aan false in het begin van de loop
+// en verander deze weer in true wanneer de hele loop voltooid is. In het geval dat de loop zichzelf
+// inhaalt, blijft de bool false en wordt een string (There is a timing problem) geprint.
+// RESULTAAT: de string wordt niet geprint, er zouden geen timing issues moeten zijn.
+// Het script spreekt zichzelf dus tegen, experts komen ook niet uit dit probleem.
+
+volatile bool loop_done = true;
+
 // Finite state machine programming (calibration servo motor?)
 void ProcessStateMachine(void)
 {
+    if (!loop_done) {
+        pc.printf("There is a timing problem\r\n");
+
+        return;
+    }
+
+    loop_done = false;
+
     // Berekenen van de motorhoeken (in radialen)
-    pulses_M1 = Encoder1.getPulses();
-    pulses_M2 = Encoder2.getPulses();
-    counts1 = pulses_M1*2;
-    counts2 = pulses_M2*2;
-    theta_h_1_rad = conversion_factor*counts1;
-    theta_h_2_rad = conversion_factor*counts2;
+    counts1 = Encoder1.getPulses();
+    counts2 = Encoder2.getPulses();
+    theta_h_1_rad = conversion_factor*(counts1-offset1);
+    theta_h_2_rad = conversion_factor*(counts2-offset2);
 
     // Calculating joint angles based on motor angles (current encoder values)
     q1 = theta_h_1_rad;                       // Relative angle joint 1 (rad)
@@ -373,25 +376,9 @@
     // 5 seconden maximaal aangespannen. De EMG waarden worden bij elkaar opgeteld,
     // waarna het gemiddelde wordt bepaald.
     if (calib) {
-        if (i_calib == 0) {
-            filtered_EMG_biceps_right_total=0;
-            filtered_EMG_biceps_left_total=0;
-            filtered_EMG_calf_total=0;
-        }
-        if (i_calib <= 2500) {
-            filtered_EMG_biceps_right_total+=filtered_EMG_biceps_right;
-            filtered_EMG_biceps_left_total+=filtered_EMG_biceps_left;
-            filtered_EMG_calf_total+=filtered_EMG_calf;
-            i_calib++;
-        }
-        if (i_calib > 2500) {
-            mean_EMG_biceps_right=filtered_EMG_biceps_right_total/2500.0;
-            mean_EMG_biceps_left=filtered_EMG_biceps_left_total/2500.0;
-            mean_EMG_calf=filtered_EMG_calf_total/2500.0;
-            pc.printf("Ontspan spieren\r\n");
-            pc.printf("Rechterbiceps_max = %f, Linkerbiceps_max = %f, Kuit_max = %f\r\n", mean_EMG_biceps_right, mean_EMG_biceps_left, mean_EMG_calf);
-            calib = false;
-        }
+        
+        EMG_calibration();
+        
     }
 
     // Genormaliseerde EMG's berekenen
@@ -429,8 +416,8 @@
                 emergency();
             }
             if (Motor_calib_button_pressed.read() == false) {
-                theta_h_1_rad = 0;
-                theta_h_2_rad = 0;
+                offset1 = counts1;
+                offset2 = counts2;
                 pc.printf("Huidige hoek in radialen motor 1:%f en motor 2: %f (moet 0 zijn) \r\n", theta_h_1_rad, theta_h_2_rad);
                 currentState = Calib_EMG;
                 stateChanged = true;
@@ -459,7 +446,7 @@
             }
             break;
 
-        case Homing: 
+        case Homing:
 
             if (stateChanged) {
                 // Ervoor zorgen dat de motoren zo bewegen dat de robotarm
@@ -470,34 +457,9 @@
                 emergency();
             }
 
-            if (theta_h_1_rad != 0.0) {
-                if (theta_h_1_rad < 0) {
-                    motor1.write(0.3);
-                    motor1_dir.write(0);
-                } else {
-                    motor1.write(0.3);
-                    motor1_dir.write(1);
-                }
-            }
-            if (theta_h_1_rad == 0.0) {
-                motor1.write(0);
-            }
-            if (theta_h_2_rad != 0.0) {
-                if (theta_h_2_rad < 0) {
-                    motor2.write(0.3);
-                    motor2_dir.write(1);
-                } else {
-                    motor2.write(0.3);
-                    motor2_dir.write(0);
-                }
-            }
-            if (theta_h_2_rad == 0.0) {
-                motor2.write(0);
-            }
+            Homing_function();
 
-            if (theta_h_1_rad == 0.0 && theta_h_2_rad == 0.0)
-                // Veranderen wanneer encoders werken en motor in elkaar zit
-            {
+            if (theta_h_1_rad == 0.0 && theta_h_2_rad == 0.0) {
                 currentState = Operation_mode;
                 stateChanged = true;
                 pc.printf("Moving to operation mode \r\n");
@@ -505,48 +467,35 @@
 
             break;
 
-        case Operation_mode: // Overgaan tot emergency wanneer referentie niet
-            // overeenkomt met werkelijkheid
+        case Operation_mode:
 
-            // pc.printf("normalized_EMG_biceps_right= %f, mean_EMG_biceps_right = %f, filtered_EMG_biceps_right = %f\r\n", normalized_EMG_biceps_right, mean_EMG_biceps_right, filtered_EMG_biceps_right);
             if (stateChanged) {
                 motors_off();
-                Joint_1_position_x = 0;
-                Joint_2_position_x = 0;
-                Joint_1_position_y = 0;
-                Joint_2_position_y = 0;
-                Joint_1_position_x_prev = Joint_1_position_x;
-                Joint_2_position_x_prev = Joint_2_position_x;
-                Joint_1_position_y_prev = Joint_1_position_y;
-                Joint_2_position_y_prev = Joint_2_position_y;
-                Joint_velocity_x[0][0] = 0;
-                Joint_velocity_x[1][0] = 0;
-                Joint_velocity_y[0][0] = 0;
-                Joint_velocity_y[1][0] = 0;
-                Motor_1_position_x = 0;
-                Motor_2_position_x = 0;
-                Motor_1_position_y = 0;
-                Motor_2_position_y = 0;
-                theta_k_1_x = 0.0;
-                theta_k_2_x = 0.0;
-                error_integral_1_x = 0.0;
-                error_integral_2_x = 0.0;
-                theta_k_1_y = 0.0;
-                theta_k_2_y = 0.0;
-                error_integral_1_y = 0.0;
-                error_integral_2_y = 0.0;
+                Joint_1_position = 0;
+                Joint_2_position = 0;
+                Joint_1_position_prev = Joint_1_position;
+                Joint_2_position_prev = Joint_2_position;
+                Joint_velocity[0][0] = 0;
+                Joint_velocity[1][0] = 0;
+                Motor_1_position = 0;
+                Motor_2_position = 0;
+                theta_k_1 = 0.0;
+                theta_k_2 = 0.0;
+                error_integral_1 = 0.0;
+                error_integral_2 = 0.0;
                 stateChanged = false;
+                pc.printf("einde operation mode init");
             }
             // Hier moet een functie worden aangeroepen die ervoor zorgt dat
             // aan de hand van EMG-signalen de motoren kunnen worden aangestuurd,
             // zodat de robotarm kan bewegen
 
-            // if (Power_button_pressed.read() == false) { // Normaal waarde 1 bij indrukken, nu nul -> false
-            // motors_off();
-            // currentState = Motors_off;
-            // stateChanged = true;
-            // pc.printf("Terug naar de state Motors_off\r\n");
-            // }
+            if (Power_button_pressed.read() == false) { // Normaal waarde 1 bij indrukken, nu nul -> false
+                motors_off();
+                currentState = Motors_off;
+                stateChanged = true;
+                pc.printf("Terug naar de state Motors_off\r\n");
+            }
             if (Emergency_button_pressed.read() == false) {
                 emergency();
             }
@@ -560,23 +509,14 @@
 
                 if (normalized_EMG_calf < 0.3) {
                     vx = 0.0;
-                    vy = 0.05;
+                    vy = 0.02;
                 }
                 if (normalized_EMG_calf >= 0.3) {
                     vx = 0.0;
-                    vy = -0.05;
+                    vy = -0.02;
                 }
-                Inverse_Kinematics();
-                error_q1_y = Motor_1_position_y - theta_h_1_rad;
-                error_q2_y = Motor_2_position_y - theta_h_2_rad;
-                PI_controller_y();
 
-                abs_theta_t_1_y = abs(theta_t_1_y);
-                abs_theta_t_2_y = abs(theta_t_2_y);
-
-                motor1.write(abs_theta_t_1_y);
-                motor2.write(abs_theta_t_2_y);
-                Define_motor_dir_y();
+                Controlling_system();
 
             } else if (normalized_EMG_biceps_left >= 0.3) {
                 if (normalized_EMG_calf < 0.3) {
@@ -587,23 +527,16 @@
                     vx = -0.05;
                     vy = 0.0;
                 }
-                Inverse_Kinematics();
-                error_q1_x = Motor_1_position_x - theta_h_1_rad;
-                error_q2_x = Motor_2_position_x - theta_h_2_rad;
-                PI_controller_x();
 
-                abs_theta_t_1_x = abs(theta_t_1_x);
-                abs_theta_t_2_x = abs(theta_t_2_x);
-
-                motor1.write(abs_theta_t_1_x);
-                motor2.write(abs_theta_t_2_x);
-                Define_motor_dir_x();
+                Controlling_system();
 
             } else {
-                motor1.write(0);
-                motor2.write(0);
+                vx = 0.0;
+                vy = 0.0;
+
+                Controlling_system();
+
             }
-
             break;
 
         default:
@@ -612,10 +545,16 @@
             pc.printf("Unknown or uninplemented state reached!\r\n");
 
     }
+    loop_done = true;
 }
 
 int main(void)
 {
+    pc.baud(115200);
+
+    motor1.period_us(56);
+    motor2.period_us(56);
+
     pc.printf("Opstarten\r\n");
 
     // Chain voor rechter biceps
@@ -631,7 +570,6 @@
     loop_ticker.attach(&ProcessStateMachine, 0.002f);
 
     while(true) {
-        // wait(0.2);
         /* do nothing */
     }
 }
\ No newline at end of file