Revision 0:aa126aa2ff8b, committed 2018-11-02

Comitter:

EvaKrolis

Date:

Fri Nov 02 08:53:55 2018 +0000

Commit message:

First try controlling the motors from the EMG without kinematics

Changed in this revision

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/BiQuad.lib	Fri Nov 02 08:53:55 2018 +0000
@@ -0,0 +1,1 @@
+http://os.mbed.com/users/tomlankhorst/code/biquadFilter/#26861979d305

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/FastPWM.lib	Fri Nov 02 08:53:55 2018 +0000
@@ -0,0 +1,1 @@
+http://os.mbed.com/users/Sissors/code/FastPWM/#c0b2265cff9c

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/MODSERIAL.lib	Fri Nov 02 08:53:55 2018 +0000
@@ -0,0 +1,1 @@
+http://os.mbed.com/users/Sissors/code/MODSERIAL/#da0788f0bd77

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/QEI.lib	Fri Nov 02 08:53:55 2018 +0000
@@ -0,0 +1,1 @@
+https://os.mbed.com/users/aberk/code/QEI/#5c2ad81551aa

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/main.cpp	Fri Nov 02 08:53:55 2018 +0000
@@ -0,0 +1,882 @@
+// ≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡ PREPARATION ≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡
+// Libraries
+#include "mbed.h"
+#include "BiQuad.h"
+#include "FastPWM.h"
+#include "MODSERIAL.h"
+#include "QEI.h"
+#include <algorithm>
+#include <math.h>
+#include <cmath>
+#define PI 3.14159265
+
+// LEDs
+DigitalOut ledRed(LED_RED,1);                   // red LED K64F
+DigitalOut ledGreen(LED_GREEN,1);               // green LED K64F
+DigitalOut ledBlue(LED_BLUE,1);                 // blue LED K64F
+
+Ticker blinkTimer;                              // LED ticker
+Timer EMGtransition_timer;                      // Timer for the transition from EMG calibration to homing
+
+// Buttons/inputs
+InterruptIn buttonBio1(D0);                     // button 1 BioRobotics shield
+InterruptIn buttonBio2(D1);                     // button 2 BioRobotics shield
+InterruptIn buttonEmergency(SW2);               // emergency button on K64F
+InterruptIn buttonHome(SW3);                    // button on K64F
+
+// Potmeters
+AnalogIn    pot2(A2);
+
+// Motor pins input
+DigitalIn   pin8(D8);       // Encoder L B
+DigitalIn   pin9(D9);       // Encoder L A
+DigitalIn   pin10(D10);     // Encoder R B
+DigitalIn   pin11(D11);     // Encoder R A
+DigitalIn   pin12(D12);     // Encoder F B
+DigitalIn   pin13(D13);     // Encoder F A
+
+// Motor pins output
+DigitalOut  pin2(D2);       // Motor F direction = motor flip
+FastPWM     pin3(A5);       // Motor F pwm = motor flip
+DigitalOut  pin4(D4);       // Motor R direction = motor right
+FastPWM     pin5(D5);       // Motor R pwm = motor right
+FastPWM     pin6(D6);       // Motor L pwm = motor left
+DigitalOut  pin7(D7);       // Motor L direction = motor left
+
+// Utilisation of libraries
+MODSERIAL  pc(USBTX, USBRX);                     // communication with pc
+QEI         encoderL(D9,D8,NC,4200);            // Encoder input of motor L
+QEI         encoderR(D10,D11,NC,4200);          // Encoder input of motor R
+QEI         encoderF(D12,D13,NC,4200);          // Encoder input of motor F
+Ticker      motorL;
+Ticker      motorR;
+Ticker      motorF;
+
+// Define & initialise state machine
+const float dt = 0.002f;
+enum states {   calibratingMotorL, calibratingMotorR, calibratingMotorF,
+                calibratingEMG, homing, reading, operating, failing, demoing
+            };
+
+states currentState = calibratingMotorL;        // start in motor L mode
+bool changeState = true;                        // initialise the first state
+
+Ticker stateTimer;                              // state machine ticker
+
+//------------------------ Parameters for the EMG ----------------------------
+
+// EMG inputs
+AnalogIn EMG0In(A0);                            // EMG input 0
+AnalogIn EMG1In(A1);                            // EMG input 1
+
+// Timers
+Ticker FindMax0_timer;          // Timer for finding the max muscle
+Ticker FindMax1_timer;          // Timer for finding the max muscle
+
+// Constants
+const int Length = 10000;       // Length of the array for the calibration
+const int Parts = 50;           // Mean average filter over 50 values
+
+// Parameters for the first EMG signal
+float EMG0;                     // float for EMG input
+float EMG0filt;                 // float for filtered EMG
+float EMG0filtArray[Parts];     // Array for the filtered array
+float EMG0Average;              // float for the value after Moving Average Filter
+float Sum0 = 0;                 // Sum0 for the moving average filter
+float EMG0Calibrate[Length];    // Array for the calibration
+int ReadCal0 = 0;               // Integer to read over the calibration array
+float MaxValue0 = 0;            // float to save the max muscle
+float Threshold0 = 0;           // Threshold for the first EMG signal
+
+// Parameters for the second EMG signal
+float EMG1;                     // float for EMG input
+float EMG1filt;                 // float for filtered EMG
+float EMG1filtArray[Parts];     // Array for the filtered array
+float EMG1Average;              // float for the value after Moving Average Filter
+float Sum1 = 0;                 // Sum0 for the moving average filter
+float EMG1Calibrate[Length];    // Array for the calibration
+int ReadCal1 = 0;               // Integer to read over the calibration array
+float MaxValue1 = 0;            // float to save the max muscle
+float Threshold1 = 0;           // Threshold for the second EMG signal
+
+//Filter variables
+BiQuad Notch50_0(0.9049,-1.4641,0.9049,-1.4641,0.8098);         //Make Notch filter around 50 Hz
+BiQuad Notch50_1(0.9049,-1.4641,0.9049,-1.4641,0.8098);         //Make Notch filter around 50 Hz
+BiQuad Notch100_0(0.8427,-0.5097,0.8247,-0.5097,0.6494);        //Make Notch filter around 100 Hz
+BiQuad Notch100_1(0.8427,-0.5097,0.8247,-0.5097,0.6494);        //Make Notch filter around 100 Hz
+BiQuad Notch150_0(0.7548,0.4665,0.7544,0.4665,0.5095);          //Make Notch filter around 150 Hz
+BiQuad Notch150_1(0.7548,0.4665,0.7544,0.4665,0.5095);          //Make Notch filter around 150 Hz
+BiQuad Notch200_0(0.6919,1.1196,0.6919,1.1196,0.3839);          //Make Notch filter around 200 Hz
+BiQuad Notch200_1(0.6919,1.1196,0.6919,1.1196,0.3839);          //Make Notch filter around 200 Hz
+BiQuad High_0(0.9150,-1.8299,0.9150,-1.8227,0.8372);            //Make high-pass filter
+BiQuad High_1(0.9150,-1.8299,0.9150,-1.8227,0.8372);            //Make high-pass filter
+BiQuad Low_0(0.6389,1.2779,0.6389,1.143,0.4128);                //Make low-pass filter
+BiQuad Low_1(0.6389,1.2779,0.6389,1.143,0.4128);                //Make low-pass filter
+BiQuadChain filter0;                                            //Make chain of filters for the first EMG signal
+BiQuadChain filter1;                                            //Make chain of filters for the second EMG signal
+
+//Bool for movement
+bool xMove = false;                                             //Bool for the x-movement
+bool yMove = false;                                             //Bool for the y-movement
+
+// -------------------- Parameters for the kinematics -------------------------
+double theta1 = PI*0.40;                                        //Angle of the left motor
+double theta4 = PI*0.40;                                        //Angle of the right motor
+// ---------------------- Parameters for the motors ---------------------------
+const float     countsRad = 4200.0f;
+int     countsL;
+int     countsR;
+int     countsF;
+int     countsCalibratedL;
+int     countsCalibratedR;
+int     countsCalibratedF;
+int     countsOffsetL;
+int     countsOffsetR;
+int     countsOffsetF;
+float   angleCurrentL;
+float   angleCurrentR;
+float   angleCurrentF;
+float   errorL;
+float   errorR;
+float   errorF;
+float   errorCalibratedL;
+float   errorCalibratedR;
+float   errorCalibratedF;
+
+int     countsCalibration = 4200/4;
+
+// ≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡ FUNCTIONS ≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡
+// ============================ GENERAL FUNCTIONS =============================
+void stopProgram(void)
+{
+    // Error message
+    pc.printf("[ERROR] emergency button pressed\r\n");
+    currentState = failing;             // change to state
+    changeState = true;                 // next loop, switch states
+}
+
+void blinkLedRed(void)
+{
+    ledRed =! ledRed;                   // toggle LED
+}
+void blinkLedGreen(void)
+{
+    ledGreen =! ledGreen;               // toggle LED
+}
+void blinkLedBlue(void)
+{
+    ledBlue =! ledBlue;                 // toggle LED
+}
+
+float   angleCurrent(float counts)      // Calculates the current angle of the motor (between -2*PI to 2*PI) based on the counts from the encoder
+{
+    float   angle = ((float)counts*2.0f*PI)/countsRad;
+    while   (angle > 2.0f*PI) {
+        angle = angle-2.0f*PI;
+    }
+    while   (angle < -2.0f*PI) {
+        angle = angle+2.0f*PI;
+    }
+    return angle;
+}
+
+//  ============================= EMG FUNCTIONS ===============================
+
+//Function to read and filter the EMG
+void ReadUseEMG0()
+{
+    for(int i = Parts ; i > 0 ; i--) {                          //Make a first in, first out array
+        EMG0filtArray[i] = EMG0filtArray[i-1];                  //Every value moves one up
+    }
+
+    Sum0 = 0;
+    EMG0 = EMG0In;                                              //Save EMG input in float
+    EMG0filt = filter0.step(EMG0);                              //Filter the signal
+    EMG0filt = abs(EMG0filt);                                   //Take the absolute value
+    EMG0filtArray[0] = EMG0filt;                                //Save the filtered signal on the first place in the array
+
+    for(int i = 0 ; i < Parts ; i++) {                          //Moving Average filter
+        Sum0 += EMG0filtArray[i];                               //Sum the new value and the previous 49
+    }
+    EMG0Average = (float)Sum0/Parts;                            //Divide the sum by 50
+
+    if (EMG0Average > Threshold0) {                             //If the value is higher than the threshold value
+        xMove = true;                                           //Set movement to true
+    } else {
+        xMove = false;                                          //Otherwise set movement to false
+    }
+}
+
+//Function to read and filter the EMG
+void ReadUseEMG1()
+{
+    for(int i = Parts ; i > 0 ; i--) {                          //Make a first in, first out array
+        EMG1filtArray[i] = EMG1filtArray[i-1];                  //Every value moves one up
+    }
+
+    Sum1 = 0;
+    EMG1 = EMG1In;                                              //Save EMG input in float
+    EMG1filt = filter1.step(EMG1);                              //Filter the signal
+    EMG1filt = abs(EMG1filt);                                   //Take the absolute value
+    EMG1filtArray[0] = EMG1filt;                                //Save the filtered signal on the first place in the array
+
+    for(int i = 0 ; i < Parts ; i++) {                          //Moving Average filter
+        Sum1 += EMG1filtArray[i];                               //Sum the new value and the previous 49
+    }
+    EMG1Average = (float)Sum1/Parts;                            //Divide the sum by 50
+
+    if (EMG1Average > Threshold1) {                             //If the value is higher than the threshold value
+        yMove = true;                                           //Set y movement to true
+    } else {
+        yMove = false;                                           //Otherwise set y movement to false
+    }
+}
+
+//Function to make an array during the calibration
+void CalibrateEMG0()
+{
+    for(int i = Parts ; i > 0 ; i--) {                          //Make a first in, first out array
+        EMG0filtArray[i] = EMG0filtArray[i-1];                  //Every value moves one up
+    }
+
+    Sum0 = 0;
+    EMG0 = EMG0In;                                              //Save EMG input in float
+    EMG0filt = filter0.step(EMG0);                              //Filter the signal
+    EMG0filt = abs(EMG0filt);                                   //Take the absolute value
+    EMG0filtArray[0] = EMG0filt;                                //Save the filtered signal on the first place in the array
+
+    for(int i = 0 ; i < Parts ; i++) {                          //Moving Average filter
+        Sum0 += EMG0filtArray[i];                               //Sum the new value and the previous 49
+    }
+    EMG0Calibrate[ReadCal0] = (float)Sum0/Parts;                //Divide the sum by 50
+
+    ReadCal0++;
+}
+
+//Function to make an array during the calibration
+void CalibrateEMG1()
+{
+    for(int i = Parts ; i > 0 ; i--) {                          //Make a first in, first out array
+        EMG1filtArray[i] = EMG1filtArray[i-1];                  //Every value moves one up
+    }
+
+    Sum1 = 0;
+    EMG1 = EMG1In;                                              //Save EMG input in float
+    EMG1filt = filter1.step(EMG1);                              //Filter the signal
+    EMG1filt = abs(EMG1filt);                                   //Take the absolute value
+    EMG1filtArray[0] = EMG1filt;                                //Save the filtered signal on the first place in the array
+
+    for(int i = 0 ; i < Parts ; i++) {                          //Moving Average filter
+        Sum1 += EMG1filtArray[i];                               //Sum the new value and the previous 49
+    }
+    EMG1Calibrate[ReadCal1] = (float)Sum1/Parts;                //Divide the sum by 50
+
+    ReadCal1++;
+}
+
+//Function to find the max value during the calibration
+void FindMax0()
+{
+    MaxValue0 = *max_element(EMG0Calibrate+500,EMG0Calibrate+Length);    //Find max value, but discard the first 100 values
+    Threshold0 = 0.30f*MaxValue0;                               //The threshold is a percentage of the max value
+    pc.printf("The calibration value of the first EMG is %f.\n\r The threshold is %f. \n\r",MaxValue0,Threshold0);              //Print the max value and the threshold
+    FindMax0_timer.detach();                                    //Detach the timer, so you only use this once
+}
+
+//Function to find the max value during the calibration
+void FindMax1()
+{
+    MaxValue1 = *max_element(EMG1Calibrate+500,EMG1Calibrate+Length);    //Find max value, but discard the first 100 values
+    Threshold1 = 0.30f*MaxValue1;                               //The threshold is a percentage of the max value
+    pc.printf("The calibration value of the second EMG is %f.\n\r The threshold is %f. \n\r",MaxValue1,Threshold1); //Print the Max value and the threshold
+    FindMax1_timer.detach();                                    //Detach the timer, so you only use this once
+}
+
+//  ================ FUNCTIONS TO LINK THE EMG TO THE MOTORS===================
+void EMGMotorLink(){
+    if (xMove == true){
+        theta1 = theta1*0.0001;
+        theta4 = -theta4*0.0001;
+    }
+    if(yMove == true){
+        theta1 = -theta1*0.0001;
+        theta4 = theta4*0.0001;
+    }
+}
+
+//  ============================ MOTOR FUNCTIONS ===============================
+// angleDesired now defined as angle of potmeter and not the angle as output from the kinematics
+// So, the angleDesired needs to be defined again and the part in which the potmeter value is calculated needs to be commented
+
+// ------------------------ General motor functions ----------------------------
+int countsInputL()      // Gets the counts from encoder 1
+{
+    countsL = encoderL.getPulses();
+    return  countsL;
+}
+int countsInputR()      // Gets the counts from encoder 2
+{
+    countsR = encoderR.getPulses();
+    return  countsR;
+}
+int countsInputF()      // Gets the counts from encoder 3
+{
+    countsF = encoderF.getPulses();
+    return  countsF;
+}
+
+float  errorCalc(float angleReference,float angleCurrent)     // Calculates the error of the system, based on the current angle and the reference value
+{
+    float   error = angleReference - angleCurrent;
+    return  error;
+}
+
+float angleDesired()        // Sets the desired angle for the controller dependent on the scaled angle of potmeter 1
+{
+    float   angle = (pot2*2.0f*PI)-PI;
+    return  angle;
+}
+
+// ------------------------- MOTOR FUNCTIONS FOR MOTOR LEFT --------------------
+float PIDControllerL(float angleReference,float angleCurrent)  // PID controller for the motors, based on the reference value and the current angle of the motor
+{
+    float   Kp = 19.24f;
+    float   Ki = 1.02f;
+    float   Kd = 0.827f;
+    float   error = errorCalc(angleReference,angleCurrent);
+    static float    errorIntegralL = 0.0;
+    static float    errorPreviousL = error; // initialization with this value only done once!
+    static  BiQuad  PIDFilterL(0.0640, 0.1279, 0.0640, -1.1683, 0.4241);
+    // Proportional part:
+    float   u_k = Kp * error;
+    // Integral part
+    errorIntegralL = errorIntegralL + error * dt;
+    float   u_i = Ki * errorIntegralL;
+    // Derivative part
+    float   errorDerivative = (error - errorPreviousL)/dt;
+    float   errorDerivativeFiltered = PIDFilterL.step(errorDerivative);
+    float   u_d = Kd * errorDerivativeFiltered;
+    errorPreviousL = error;
+    // Sum all parts and return it
+    return  u_k + u_i + u_d;
+}
+
+int countsInputCalibratedL()      // Gets the counts from encoder 1
+{
+    countsL = encoderL.getPulses()- countsOffsetL + countsCalibration;
+    return  countsL;
+}
+
+void    motorTurnL()    // main function for movement of motor 1, all above functions with an extra tab are called
+{
+    float   angleReferenceL = theta1;                              // insert kinematics output here instead of angleDesired()
+    angleReferenceL = -angleReferenceL;                                     // different minus sign per motor
+    countsL = countsInputCalibratedL();                                       // different encoder pins per motor
+    angleCurrentL = angleCurrent(countsL);                                  // different minus sign per motor
+    errorL = errorCalc(angleReferenceL,angleCurrentL);                      // same for every motor
+
+    float   PIDControlL = PIDControllerL(angleReferenceL,angleCurrentL);    // same for every motor
+    pin6 = fabs(PIDControlL);                                               // different pins for every motor
+    pin7 = PIDControlL > 0.0f;                                              // different pins for every motor
+}
+
+void calibrationL()        // Partially the same as motorTurnL, only with potmeter input
+// How it works: manually turn motor using potmeters until the robot arm touches the bookholder.
+// This program sets the counts from the motor to the reference counts (an angle of PI/4.0)
+// Do this for every motor and after calibrated all motors, press a button
+{
+    float   angleReferenceL = angleDesired();                              // insert kinematics output here instead of angleDesired()
+    angleReferenceL = -angleReferenceL;                                     // different minus sign per motor
+    countsL = countsInputL();                                       // different encoder pins per motor
+    angleCurrentL = angleCurrent(countsL);                                  // different minus sign per motor
+    errorL = errorCalc(angleReferenceL,angleCurrentL);                      // same for every motor
+
+    if (fabs(errorL) >= 0.01f) {
+        float   PIDControlL = PIDControllerL(angleReferenceL,angleCurrentL);    // same for every motor
+        pin6 = fabs(PIDControlL);                                               // different pins for every motor
+        pin7 = PIDControlL > 0.0f;                                              // different pins for every motor
+    } else if (fabs(errorL) < 0.01f) {
+        countsOffsetL = countsL;
+        countsL = countsL - countsOffsetL + countsCalibration;
+        //countsL = 0;
+        pin6 = 0.0f;
+        // BUTTON PRESS: TO NEXT STATE
+    }
+}
+
+// ------------------------ MOTOR FUNCTIONS FOR MOTOR RIGHT --------------------
+float PIDControllerR(float angleReference,float angleCurrent)  // PID controller for the motors, based on the reference value and the current angle of the motor
+{
+    float   Kp = 19.24f;
+    float   Ki = 1.02f;
+    float   Kd = 0.827f;
+    float   error = errorCalc(angleReference,angleCurrent);
+    static float    errorIntegralR = 0.0;
+    static float    errorPreviousR = error; // initialization with this value only done once!
+    static  BiQuad  PIDFilterR(0.0640, 0.1279, 0.0640, -1.1683, 0.4241);
+    // Proportional part:
+    float   u_k = Kp * error;
+    // Integral part
+    errorIntegralR = errorIntegralR + error * dt;
+    float   u_i = Ki * errorIntegralR;
+    // Derivative part
+    float   errorDerivative = (error - errorPreviousR)/dt;
+    float   errorDerivativeFiltered = PIDFilterR.step(errorDerivative);
+    float   u_d = Kd * errorDerivativeFiltered;
+    errorPreviousR = error;
+    // Sum all parts and return it
+    return  u_k + u_i + u_d;
+}
+
+int countsInputCalibratedR()      // Gets the counts from encoder 1
+{
+    countsR = encoderR.getPulses()- countsOffsetR + countsCalibration;
+    return  countsR;
+}
+
+void    motorTurnR()    // main function for movement of motor 1, all above functions with an extra tab are called
+{
+    float   angleReferenceR = theta4;                              // insert kinematics output here instead of angleDesired()
+    angleReferenceR = -angleReferenceR;                                     // different minus sign per motor
+    countsR = countsInputCalibratedR();                                       // different encoder pins per motor
+    angleCurrentR = angleCurrent(countsR);                                  // different minus sign per motor
+    errorR = errorCalc(angleReferenceR,angleCurrentR);                      // same for every motor
+
+    float   PIDControlR = PIDControllerR(angleReferenceR,angleCurrentR);    // same for every motor
+    pin5 = fabs(PIDControlR);                                               // different pins for every motor
+    pin4 = PIDControlR > 0.0f;                                              // different pins for every motor
+}
+
+void calibrationR()        // Partially the same as motorTurnR, only with potmeter input
+// How it works: manually turn motor using potmeters until the robot arm touches the bookholder.
+// This program sets the counts from the motor to the reference counts (an angle of PI/4.0)
+// Do this for every motor and after calibrated all motors, press a button
+{
+    float   angleReferenceR = angleDesired();                              // insert kinematics output here instead of angleDesired()
+    angleReferenceR = -angleReferenceR;                                     // different minus sign per motor
+    countsR = countsInputR();                                       // different encoder pins per motor
+    angleCurrentR = angleCurrent(countsR);                                  // different minus sign per motor
+    errorR = errorCalc(angleReferenceR,angleCurrentR);                      // same for every motor
+
+    if (fabs(errorR) >= 0.01f) {
+        float   PIDControlR = PIDControllerR(angleReferenceR,angleCurrentR);    // same for every motor
+        pin5 = fabs(PIDControlR);                                               // different pins for every motor
+        pin4 = PIDControlR > 0.0f;                                              // different pins for every motor
+    } else if (fabs(errorR) < 0.01f) {
+        countsOffsetR = countsR;
+        countsR = countsR - countsOffsetR + countsCalibration;
+        //countsR = 0;
+        pin5 = 0.0f;
+        // BUTTON PRESS: TO NEXT STATE
+    }
+}
+
+// ------------------------- MOTOR FUNCTIONS FOR MOTOR FLIP --------------------
+
+float PIDControllerF(float angleReference,float angleCurrent)  // PID controller for the motors, based on the reference value and the current angle of the motor
+    {   float   Kp = 19.24f; 
+        float   Ki = 1.02f;
+        float   Kd = 0.827f;
+        float   error = errorCalc(angleReference,angleCurrent);
+        static float    errorIntegralF = 0.0;
+        static float    errorPreviousF = error; // initialization with this value only done once!
+        static  BiQuad  PIDFilterF(0.0640, 0.1279, 0.0640, -1.1683, 0.4241);
+        // Proportional part:
+        float   u_k = Kp * error;
+        // Integral part
+        errorIntegralF = errorIntegralF + error * dt;
+        float   u_i = Ki * errorIntegralF;
+        // Derivative part
+        float   errorDerivative = (error - errorPreviousF)/dt;
+        float   errorDerivativeFiltered = PIDFilterF.step(errorDerivative);
+        float   u_d = Kd * errorDerivativeFiltered;
+        errorPreviousF = error;
+        // Sum all parts and return it
+        return  u_k + u_i + u_d;
+    }
+    
+    int countsInputCalibratedF()      // Gets the counts from encoder 1
+    {   countsF = encoderF.getPulses()- countsOffsetF + countsCalibration;
+        return  countsF;
+    }    
+        
+void    motorTurnF()    // main function for movement of motor 1, all above functions with an extra tab are called
+{   
+    float   angleReferenceF = 0.0f;                              // insert kinematics output here instead of angleDesired()
+    angleReferenceF = -angleReferenceF;                                     // different minus sign per motor
+    countsF = countsInputCalibratedF();                                       // different encoder pins per motor
+    angleCurrentF = angleCurrent(countsF);                                  // different minus sign per motor
+    errorF = errorCalc(angleReferenceF,angleCurrentF);                      // same for every motor
+    
+    float   PIDControlF = PIDControllerF(angleReferenceF,angleCurrentF);    // same for every motor
+    pin5 = fabs(PIDControlF);                                               // different pins for every motor
+    pin4 = PIDControlF > 0.0f;                                              // different pins for every motor
+}
+
+void calibrationF()        // Partially the same as motorTurnF, only with potmeter input
+// How it works: manually turn motor using potmeters until the robot arm touches the bookholder.
+// This program sets the counts from the motor to the reference counts (an angle of PI/4.0)
+// Do this for every motor and after calibrated all motors, press a button
+{   float   angleReferenceF = 0.0f;                          // insert kinematics output here instead of angleDesired()
+    angleReferenceF = -angleReferenceF;                                     // different minus sign per motor
+    countsF = countsInputF();                                       // different encoder pins per motor
+    angleCurrentF = angleCurrent(countsF);                                  // different minus sign per motor
+    errorF = errorCalc(angleReferenceF,angleCurrentF);                      // same for every motor
+    
+    if (fabs(errorF) >= 0.01f)
+    {   float   PIDControlF = PIDControllerF(angleReferenceF,angleCurrentF);    // same for every motor
+        pin5 = fabs(PIDControlF);                                               // different pins for every motor
+        pin4 = PIDControlF > 0.0f;                                              // different pins for every motor
+    }
+    else if (fabs(errorF) < 0.01f)
+    {   countsOffsetF = countsF;
+        countsF = countsF - countsOffsetF + countsCalibration; 
+        //countsF = 0; 
+        pin5 = 0.0f; 
+        // BUTTON PRESS: TO NEXT STATE 
+    }         
+}
+
+// ≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡ STATE MACHINE ≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡
+void stateMachine(void)
+{
+    switch (currentState) {         // determine which state Odin is in
+
+//  ======================== MOTOR L CALIBRATION MODE =========================
+        case calibratingMotorL:
+//      ------------------------- initialisation --------------------------
+            if (changeState) {                  // when entering the state
+                pc.printf("[MODE] calibrating motor L...\r\n");
+                // print current state
+                changeState = false;            // stay in this state
+
+                // Actions when entering state
+                ledRed = 1;                     // green blinking LED
+                ledGreen = 1;
+                ledBlue = 1;
+                blinkTimer.attach(&blinkLedGreen,0.5);
+
+            }
+//      ----------------------------- action ------------------------------
+            // Actions for each loop iteration
+            calibrationL();
+
+//      --------------------------- transition ----------------------------
+            // Transition condition: when all motor errors smaller than 0.01,
+            //      start calibrating EMG
+            if (errorL < 0.01f && buttonBio1 == false) {
+
+                // Actions when leaving state
+                blinkTimer.detach();
+
+                currentState = calibratingMotorR;    // change to state
+                changeState = true;               // next loop, switch states
+            }
+
+            break; // end case
+
+//  ======================== MOTOR R CALIBRATION MODE =========================
+            case calibratingMotorR:
+//      ------------------------- initialisation --------------------------
+            if (changeState) {                  // when entering the state
+                pc.printf("[MODE] calibrating motor R...\r\n");
+                // print current state
+                changeState = false;            // stay in this state
+
+                // Actions when entering state
+                ledRed = 1;                     // cyan-green blinking LED
+                ledGreen = 0;
+                ledBlue = 1;
+                blinkTimer.attach(&blinkLedBlue,0.5);
+
+            }
+//      ----------------------------- action ------------------------------
+            // Actions for each loop iteration
+            calibrationR();
+//      --------------------------- transition ----------------------------
+            // Transition condition: when all motor errors smaller than 0.01,
+            //      start calibrating EMG
+            if (errorR < 0.01f && buttonBio2 == false) {
+
+                // Actions when leaving state
+                blinkTimer.detach();
+
+                currentState = calibratingMotorF;    // change to state
+                changeState = true;               // next loop, switch states
+            }
+
+            break; // end case
+            
+//  ======================== MOTOR F CALIBRATION MODE =========================
+            case calibratingMotorF:
+//      ------------------------- initialisation --------------------------
+            if (changeState) {                  // when entering the state
+                pc.printf("[MODE] calibrating motor F...\r\n");
+                // print current state
+                changeState = false;            // stay in this state
+
+                // Actions when entering state
+                ledRed = 1;                     // green blinking LED
+                ledGreen = 1;
+                ledBlue = 1;
+                blinkTimer.attach(&blinkLedGreen,0.5);
+
+            }
+//      ----------------------------- action ------------------------------
+            // Actions for each loop iteration
+            calibrationF();
+//      --------------------------- transition ----------------------------
+            // Transition condition: when all motor errors smaller than 0.01,
+            //      start calibrating EMG
+            if (errorF < 0.01f && buttonHome == false) {
+
+                // Actions when leaving state
+                blinkTimer.detach();
+
+                currentState = calibratingEMG;    // change to state
+                changeState = true;               // next loop, switch states
+            }
+
+            break; // end case
+            
+//  =========================== EMG CALIBRATION MODE ===========================
+        case calibratingEMG:
+//      ------------------------- initialisation --------------------------
+            if (changeState) {                  // when entering the state
+                pc.printf("[MODE] calibrating EMG...\r\n");
+                // print current state
+                changeState = false;            // stay in this state
+
+                // Actions when entering state
+                ledRed = 1;                     // cyan-blue blinking LED
+                ledGreen = 0;
+                ledBlue = 0;
+                blinkTimer.attach(&blinkLedGreen,0.5);
+
+                FindMax0_timer.attach(&FindMax0,15);            //Find the maximum value after 15 seconds
+                FindMax1_timer.attach(&FindMax1,15);            //Find the maximum value after 15 seconds
+
+                EMGtransition_timer.reset();
+                EMGtransition_timer.start();
+            }
+//      ----------------------------- action ------------------------------
+            // Actions for each loop iteration
+            CalibrateEMG0();   //start emg calibration every 0.005 seconds
+            CalibrateEMG1();   //Start EMG calibration every 0.005 seconds
+            /* */
+
+
+//      --------------------------- transition ----------------------------
+            // Transition condition: after 20 sec in state
+            if (EMGtransition_timer.read() > 20) {
+                // Actions when leaving state
+                blinkTimer.detach();
+
+                currentState = homing;              // change to state
+                changeState = true;                 // next loop, switch states
+            }
+            break; // end case
+
+//  ============================== HOMING MODE ================================
+        case homing:
+//      ------------------------- initialisation --------------------------
+            if (changeState) {                  // when entering the state
+                pc.printf("[MODE] homing...\r\n");
+                // print current state
+                changeState = false;            // stay in this state
+
+
+                // Actions when entering state
+                ledRed = 1;                     // cyan LED on
+                ledGreen = 0;
+                ledBlue = 0;
+
+            }
+//      ----------------------------- action ------------------------------
+            // Actions for each loop iteration
+            /* */
+
+//      --------------------------- transition ----------------------------
+            // Transition condition #1: with button press, enter reading mode,
+            //      but only when velocity == 0
+            if (errorCalibratedL < 0.01f && errorCalibratedR < 0.01f
+                    && errorCalibratedF < 0.01f && buttonBio1 == false) {
+                // Actions when leaving state
+                /* */
+
+                currentState = reading;             // change to state
+                changeState = true;                 // next loop, switch states
+            }
+            // Transition condition #2: with button press, enter demo mode
+            //      but only when velocity == 0
+            if (errorCalibratedL < 0.01f && errorCalibratedR < 0.01f
+                    && errorCalibratedF < 0.01f && buttonBio2 == false) {
+                // Actions when leaving state
+                /* */
+
+                currentState = demoing;             // change to state
+                changeState = true;                 // next loop, switch states
+            }
+            break; // end case
+
+//  ============================== READING MODE ===============================
+        case reading:
+//      ------------------------- initialisation --------------------------
+            if (changeState) {                  // when entering the state
+                pc.printf("[MODE] reading...\r\n");
+                // print current state
+                changeState = false;            // stay in this state
+
+                // Actions when entering state
+                ledRed = 1;                     // blue LED on
+                ledGreen = 1;
+                ledBlue = 0;
+
+                // TERUGKLAPPEN
+
+            }
+//      ----------------------------- action ------------------------------
+            // Actions for each loop iteration
+            ReadUseEMG0();                  //Start the use of EMG
+            ReadUseEMG1();                  //Start the use of EMG
+            /* */
+
+//      --------------------------- transition ----------------------------
+            // Transition condition #1: when EMG signal detected, enter operating
+            //      mode
+            if (xMove == true && yMove == true) {
+                // Actions when leaving state
+                /* */
+
+                currentState = operating;             // change to state
+                changeState = true;                 // next loop, switch states
+            }
+            // Transition condition #2: with button press, back to homing mode
+            if (buttonHome == false) {
+                // Actions when leaving state
+                /* */
+
+                currentState = homing;              // change to state
+                changeState = true;                 // next loop, switch states
+            }
+            break; // end case
+
+//  ============================= OPERATING MODE ==============================
+        case operating:
+//      ------------------------- initialisation --------------------------
+            if (changeState) {                  // when entering the state
+                pc.printf("[MODE] operating...\r\n");
+                // print current state
+                changeState = false;            // stay in this state
+
+                // Actions when entering state
+                ledRed = 1;                     // blue fast blinking LED
+                ledGreen = 1;
+                ledBlue = 1;
+                blinkTimer.attach(&blinkLedBlue,0.25);
+
+            }
+//      ----------------------------- action ------------------------------
+            // Actions for each loop iteration
+            /* */
+
+//      --------------------------- transition ----------------------------
+            // Transition condition: when path is over, back to reading mode
+            if (errorCalibratedL < 0.01f && errorCalibratedR < 0.01f) {
+                // Actions when leaving state
+                blinkTimer.detach();
+
+                currentState = reading;              // change to state
+                changeState = true;                 // next loop, switch states
+            }
+            break; // end case
+
+//  ============================== DEMOING MODE ===============================
+        case demoing:
+//      ------------------------- initialisation --------------------------
+            if (changeState) {                  // when entering the state
+                pc.printf("[MODE] demoing...\r\n");
+                // print current state
+                changeState = false;            // stay in this state
+
+                // Actions when entering state
+                ledRed = 0;                     // yellow LED on
+                ledGreen = 0;
+                ledBlue = 1;
+
+            }
+//      ----------------------------- action ------------------------------
+            // Actions for each loop iteration
+            /* */
+            ReadUseEMG0();                  //Start the use of EMG
+            ReadUseEMG1();                  //Start the use of EMG
+            pc.printf("theta1 and 4: %f \t\t %f \n\r",theta1,theta4);
+            motorTurnL();
+            motorTurnR();
+            
+
+//      --------------------------- transition ----------------------------
+            // Transition condition: with button press, back to homing mode
+            if (buttonHome == false) {
+                // Actions when leaving state
+                /* */
+
+                currentState = homing;              // change to state
+                changeState = true;                 // next loop, switch states
+            }
+            break; // end case
+
+// =============================== FAILING MODE ================================
+        case failing:
+            changeState = false;            // stay in this state
+
+            // Actions when entering state
+            ledRed = 0;                     // red LED on
+            ledGreen = 1;
+            ledBlue = 1;
+
+            pin3 = 0;                       // all motor forces to zero
+            pin5 = 0;
+            pin6 = 0;
+            exit (0);                       // stop all current functions
+            break; // end case
+
+// ============================== DEFAULT MODE =================================
+        default:
+// ---------------------------- enter failing mode -----------------------------
+            currentState = failing;                 // change to state
+            changeState = true;                     // next loop, switch states
+            // print current state
+            pc.printf("[ERROR] unknown or unimplemented state reached\r\n");
+
+    }   // end switch
+}       // end stateMachine
+
+
+
+// ≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡ MAIN LOOP ≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡
+
+int main()
+{
+//  ================================ EMERGENCY ================================
+    //If the emergency button is pressed, stop program via failing state
+    buttonEmergency.rise(stopProgram);                                          // Automatische triggers voor failure mode? -> ook error message in andere functies plaatsen!
+
+//  ============================= PC-COMMUNICATION ============================
+    pc.baud(115200); // communication with terminal
+    pc.printf("\n\n[START] starting O.D.I.N\r\n");
+
+//  ============================= PIN DEFINE PERIOD ===========================
+    // If you give a period on one pin, c++ gives all pins this period
+    pin3.period_us(15);
+
+// ==================================== LOOP ==================================
+    // run state machine at 500 Hz
+    stateTimer.attach(&stateMachine,dt);
+
+//  =============================== ADD FILTERS ===============================
+    //Make filter chain for the first EMG
+    filter0.add(&Notch50_0).add(&Notch100_0).add(&Notch150_0).add(&Notch200_0).add(&Low_0).add(&High_0);
+    //Make filter chain for the second EMG
+    filter1.add(&Notch50_1).add(&Notch100_1).add(&Notch150_1).add(&Notch200_1).add(&Low_1).add(&High_1);
+}

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/mbed.bld	Fri Nov 02 08:53:55 2018 +0000
@@ -0,0 +1,1 @@
+https://os.mbed.com/users/mbed_official/code/mbed/builds/675da3299148
\ No newline at end of file