File content as of revision 37:ea621fdf306a:

#include "mbed.h"
#include "HIDScope.h"
#include "QEI.h"
#include "MODSERIAL.h"
#include "BiQuad.h"
//#include "FastPWM.h"
#define  M_PI 3.14159265358979323846  /* pi */
#include <math.h>
//#include "Servo.h"
#include <cmath>
//#include <complex>

Serial pc(USBTX, USBRX);

// TICKERS
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};               // 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);                                                     // 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);                                                     // 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(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);                                                                      // 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);                                                              // 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.
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) opstellen
BiQuadChain bqcbr;
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);                         // 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) opstellen
BiQuadChain bqcbl;
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);                         // 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) opstellen
BiQuadChain bqck;
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);                          // 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_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;

// Variabelen voor HIDScope
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;

// VARIABELEN VOOR OPERATION MODE (EMG)
double normalized_EMG_biceps_right;
double normalized_EMG_biceps_left;
double normalized_EMG_calf;

// VARIABELEN VOOR OPERATION MODE (RKI)
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 = 0.0;
double Joint_2_position = 0.0;

double Joint_1_position_prev = 0.0;
double Joint_2_position_prev = 0.0;

double Joint_velocity[2][1] = {{0.0}, {0.0}};

double q1_dot;
double q2_dot;

double q1;
double q2;

double Motor_1_position = 0.0;
double Motor_2_position = 0.0;

// VARIABELEN VOOR OPERATION MODE (PI-CONTROLLER)

const double Kp = 12.5;
const double Ki = 0.03;

double theta_k_1 = 0.0;
double theta_k_2 = 0.0;

double error_integral_1 = 0.0;
double error_integral_2 = 0.0;

double u_i_1;
double u_i_2;

double theta_t_1;
double theta_t_2;

double error_M1;
double error_M2;

double abs_theta_t_1;
double abs_theta_t_2;

// VOIDS

// Noodfunctie waarbij alles uitgaat (evt. nog een rood LEDje laten branden).
// Enige optie is resetten, dan wordt het script opnieuw opgestart.
void emergency()
{
    loop_ticker.detach();
    motor1.write(0);
    motor2.write(0);
    pc.printf("Ik ga exploderen!!!\r\n");
}

// Motoren uitzetten
void motors_off()
{
    motor1.write(0);
    motor2.write(0);
    pc.printf("Motoren uit functie\r\n");
}

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()
{
    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
    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);

    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 = 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_prev = Joint_1_position;
    Joint_2_position_prev = Joint_2_position;

    Motor_1_position = Joint_1_position;
    Motor_2_position = Joint_1_position + Joint_2_position;
}

// PI-CONTROLLER
void PI_controller()
{
// Proportional part:
    theta_k_1= Kp * error_M1;
    theta_k_2= Kp * error_M2;

// Integral part
    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= theta_k_1 + u_i_1;
    theta_t_2= theta_k_2 + u_i_2;
}

void Define_motor_dir()
{
    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);
    } else {
        motor1_dir.write(1);
        motor2_dir.write(1);
    }
}

void Controlling_system()
{
    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)
    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)
    q2 = theta_h_2_rad - theta_h_1_rad;       // Relative angle joint 2 (rad)

    // Eerste deel van de filters (High-pass + Notch) over het ruwe EMG signaal
    // doen. Het ruwe signaal wordt gelezen binnen een ticker en wordt daardoor 'gesampled'
    filtered_EMG_biceps_right_1=bqbr1.step(EMG_biceps_right_raw.read());
    filtered_EMG_biceps_left_1=bqcbl.step(EMG_biceps_left_raw.read());
    filtered_EMG_calf_1=bqck.step(EMG_calf_raw.read());

    // Vervolgens wordt de absolute waarde hiervan genomen
    filtered_EMG_biceps_right_abs=abs(filtered_EMG_biceps_right_1);
    filtered_EMG_biceps_left_abs=abs(filtered_EMG_biceps_left_1);
    filtered_EMG_calf_abs=abs(filtered_EMG_calf_1);

    // Tenslotte wordt het tweede deel van de filters (twee low-pass, voor 4e orde filter)
    // over het signaal gedaan
    filtered_EMG_biceps_right=bqcbr2.step(filtered_EMG_biceps_right_abs);
    filtered_EMG_biceps_left=bqcbl2.step(filtered_EMG_biceps_left_abs);
    filtered_EMG_calf=bqck2.step(filtered_EMG_calf_abs);

    // De gefilterde EMG-signalen kunnen tevens visueel worden weergegeven in de HIDScope
    scope.set(0, normalized_EMG_biceps_right);
    scope.set(1, normalized_EMG_biceps_left);
    scope.set(2, normalized_EMG_calf);
    scope.send();

    // Tijdens de kalibratie moet vervolgens een maximale spierspanning worden bepaald, die
    // later kan worden gebruikt voor een normalisatie. De spieren worden hiertoe gedurende
    // 5 seconden maximaal aangespannen. De EMG waarden worden bij elkaar opgeteld,
    // waarna het gemiddelde wordt bepaald.
    if (calib) {
        
        EMG_calibration();
        
    }

    // Genormaliseerde EMG's berekenen
    normalized_EMG_biceps_right=filtered_EMG_biceps_right/mean_EMG_biceps_right;
    normalized_EMG_biceps_left=filtered_EMG_biceps_left/mean_EMG_biceps_left;
    normalized_EMG_calf=filtered_EMG_calf/mean_EMG_calf;

    // Finite state machine
    switch (currentState) {
        case Motors_off:

            if (stateChanged) {
                motors_off(); // functie waarbij motoren uitgaan
                stateChanged = false;
                pc.printf("Motors off state\r\n");
            }
            if  (Emergency_button_pressed.read() == false) { // Normaal waarde 1 bij indrukken, nu nul -> false
                emergency();
            }
            if  (Power_button_pressed.read() == false) { // Normaal waarde 1 bij indrukken, nu nul -> false
                currentState = Calib_motor;
                stateChanged = true;
                pc.printf("Moving to Calib_motor state\r\n");
            }
            break;

        case Calib_motor:

            if (stateChanged) {
                pc.printf("Zet motoren handmatig in home positie\r\n");
                stateChanged = false;
            }

            if  (Emergency_button_pressed.read() == false) {
                emergency();
            }
            if (Motor_calib_button_pressed.read() == false) {
                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;
                pc.printf("Moving to Calib_EMG state\r\n");
            }
            break;

        case Calib_EMG:

            if (stateChanged) {
                i_calib = 0;
                calib = true;
                pc.printf("Span spieren aan\r\n");
                stateChanged = false;
            }

            if  (Emergency_button_pressed.read() == false) {
                emergency();
            }

            if (i_calib > 2500) {
                calib = false;
                currentState = Homing;
                stateChanged = true;
                pc.printf("Moving to Homing state\r\n");
            }
            break;

        case Homing:

            if (stateChanged) {
                // Ervoor zorgen dat de motoren zo bewegen dat de robotarm
                // (inclusief de end-effector) in de juiste home positie wordt gezet
                stateChanged = false;
            }
            if  (Emergency_button_pressed.read() == false) {
                emergency();
            }

            Homing_function();

            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");
            }

            break;

        case Operation_mode:

            if (stateChanged) {
                motors_off();
                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 (Emergency_button_pressed.read() == false) {
                emergency();
            }
            if (Motor_calib_button_pressed.read() == false) { // Is nu voor de homing
                motors_off();
                currentState = Homing;
                stateChanged = true;
                pc.printf("Terug naar de state Homing\r\n");
            }
            if (normalized_EMG_biceps_right >= 0.3) {

                if (normalized_EMG_calf < 0.3) {
                    vx = 0.0;
                    vy = 0.02;
                }
                if (normalized_EMG_calf >= 0.3) {
                    vx = 0.0;
                    vy = -0.02;
                }

                Controlling_system();

            } else if (normalized_EMG_biceps_left >= 0.3) {
                if (normalized_EMG_calf < 0.3) {
                    vx = 0.05;
                    vy = 0.0;
                }
                if (normalized_EMG_calf >= 0.3) {
                    vx = -0.05;
                    vy = 0.0;
                }

                Controlling_system();

            } else {
                vx = 0.0;
                vy = 0.0;

                Controlling_system();

            }
            break;

        default:
            // Zelfde functie als die eerder is toegepast om motoren uit te schakelen -> safety!
            motors_off();
            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
    bqcbr.add(&bqbr1).add(&bqbr2);
    bqcbr2.add(&bqbr3).add(&bqbr4);
    // Chain voor linker biceps
    bqcbl.add(&bqbl1).add(&bqbl2);
    bqcbl2.add(&bqbl3).add(&bqbl4);
    // Chain voor kuit
    bqck.add(&bqk1).add(&bqk2);
    bqck2.add(&bqk3).add(&bqk4);

    loop_ticker.attach(&ProcessStateMachine, 0.002f);

    while(true) {
        /* do nothing */
    }
}