File content as of revision 20:11fe0aa7f111:

//Libraries
#include "mbed.h"
#include <math.h>
#include <cmath>
#include "MODSERIAL.h"
#include "BiQuad.h"
#include <algorithm>
#define PI 3.14159265

//Inputs and outputs
MODSERIAL pc(USBTX, USBRX);                                     //Connecting to PC
AnalogIn EMG0In(A0);                                            //EMG input 0
AnalogIn EMG1In(A1);                                            //EMG input 1
InterruptIn button(SW3);                                        //Define button
DigitalOut greenled(LED_GREEN);                                 //Green led
DigitalOut blueled(LED_BLUE);                                   //Blue led
DigitalOut redled(LED_RED);                                     //Red led

//Constants
const double ll = 230;                                          //Length of the lower arm
const double lu = 198;                                          //Length of the upper arm
const double lb = 50;                                           //Length of the part between the upper arms
const double le = 79;                                           //Length of the end-effector beam
const double xbase = 450-lb;                                    //Length between the motors
const int Length = 10000;                                       //Length of the array for the calibration
const int Parts = 50;                                           //Mean average filter over 50 values


//parameters for kinematics
double theta1 = PI*0.49;                                        //Angle of the left motor
double theta4 = PI*0.49;                                        //Angle of the right motor
double thetaflip = 0;                                           //Angle of the flipping motor
double prefx;                                                   //Desired x coordinate
double prefy;                                                   //Desired y coordinate                                                        "
double deltat = 0.01;                                           //Time step (dependent on sample frequency)

//Kinematics parameters for x
double xendsum;
double xendsqrt1;
double xendsqrt2;
double xend;
double jacobiana;
double jacobianc;

//Kinematics parameters for y
double yendsum;
double yendsqrt1;
double yendsqrt2;
double yend;
double jacobianb;
double jacobiand;

//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.7887,0,0.7887,0,0.5774);                     //Make Notch filter around 50 Hz
BiQuad Notch50_1(0.7887,0,0.7887,0,0.5774);                     //Make Notch filter around 50 Hz
BiQuad High0(0.8006,-1.6012,0.8006,-1.561,0.6414);              //Make high-pass filter
BiQuad High1(0.8006,-1.6012,0.8006,-1.561,0.6414);              //Make high-pass filter
BiQuadChain filter0;                                            //Make chain of filters for the first EMG signal
BiQuadChain filter1;                                            //Make chain of filters for the second EMG signal

//Timers and Tickers
Ticker kin;                                                     //Timer for calculating x,y,theta1,theta4
Ticker simulateval;                                             //Timer that prints the values for x,y, and angles
Ticker ReadUseEMG0_timer;                                       //Timer to read, filter and use the EMG
Ticker EMGCalibration0_timer;                                   //Timer for the calibration of the EMG
Ticker FindMax0_timer;                                          //Timer for finding the max muscle
Ticker ReadUseEMG1_timer;                                       //Timer to read, filter and use the EMG
Ticker EMGCalibration1_timer;                                   //Timer for the calibration of the EMG
Ticker FindMax1_timer;                                          //Timer for finding the max muscle
Ticker SwitchState_timer;                                       //Timer to switch from the Calibration to the working mode

//Bool for movement
bool xMove = false;                                             //Bool for the x-movement
bool yMove = false;                                             //Bool for the y-movement

//Parameters for the state machine
enum States {Calibration, WorkingMode};                         //Initialize state machine
States CurrentState = Calibration;                              //Start in the calibration mode
bool StateBool = true;                                          //Bool to first go in a state
bool SwitchStateBool = false;                                   //Bool to switch from calibration to working mode

//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
        redled = 0;                                             //Turn the LED on
        xMove = true;                                           //Set movement to true
    } else {
        redled = 1;                                             //Otherwise turn the LED off
        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
        greenled = 0;                                           //Turn the LED on
        yMove = true;                                           //Set y movement to true
    } else {
        greenled = 1;                                           //Otherwise turn the LED off
        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
}

//Function to stop the reading of the EMG
void StopProgram()
{
    greenled = 1;                                               //Turn the LEDs off
    blueled = 1;
    redled = 1;
    exit (0);                                                   //Abort mission!!
}

//Function to switch a state
void SwitchState()
{
    SwitchStateBool = true;                                     //Set the bool for the start of a state to true
    SwitchState_timer.detach();                                 //Use this function once
}



//dit wordt aangeroepen in de tickerfunctie
void inverse(double prex, double prey)
{
    /*
                                    qn = qn-1 + (jacobian^-1)*dPref/dt *deltaT
                                    ofwel
                                    thetai+1 = thetai +(jacobian)^-1*vector(deltaX, DeltaY)
                                    waar Pref = emg signaal
                                    */ //achtergrondinfo hierboven...
    //

    theta1 += (prefx*jacobiana+jacobianb*prey)*deltat; //theta 1 is zichzelf plus wat hier staat (is kinematics)
    theta4 += (prefx*jacobianc+jacobiand*prey)*deltat;//"                                                       "
    //Hier worden xend en yend doorgerekend, die formules kan je overslaan
    xendsum = lb + xbase +ll*(cos(theta1) - cos(theta4));
    xendsqrt1 = 2*sqrt(-xbase*xbase/4 + lu*lu + ll*(xbase*(cos(theta1)+cos(theta4))/2) -ll*(1+ cos(theta1+theta4)))*(-sin(theta1)+sin(theta4));
    xendsqrt2 = sqrt(pow((-xbase/ll+cos(theta1)+cos(theta4)),2)+ pow(sin(theta1) - sin(theta4),2));
    xend = (xendsum + xendsqrt1/xendsqrt2)/2;
    //hieronder rekenen we yendeffector door;
    yendsum = -le + ll/2*(sin(theta1)+sin(theta4));
    yendsqrt1 = (-xbase/ll + cos(theta1)+cos(theta4))*sqrt(-xbase*xbase/4 + lu*lu + ll/2*(xbase*(cos(theta1)+cos(theta4))- ll*(1+cos(theta1+theta4))));
    yendsqrt2 = sqrt(pow((-xbase/ll + cos(theta1)+ cos(theta4)),2)+ pow((sin(theta1)-sin(theta4)),2));
    yend = (yendsum + yendsqrt1/yendsqrt2);

}
//deze onderstaande tickerfunctie wordt aangeroepen
void kinematics()
{

//Hieronder rekenen we eerst de aparte dingen van de jacobiaan uit. (sla maar over)

    jacobiana = (500*(-100*(sin(theta1) - sin(0.001 - theta4)) - ((-1.7 + cos(theta1) + cos(0.001 - theta4))*sqrt(28900 - 10000*(pow(-1.7 + cos(theta1) + cos(0.001 - theta4),2) + pow(sin(theta1) + sin(0.001 - theta4),2))))/
        sqrt(pow(-1.7 + cos(theta1) + cos(0.001 - theta4),2) + pow(sin(theta1) + sin(0.001 - theta4),2)) + 
       ((-340 + 200*(cos(theta1) + cos(0.001 + theta4)))*sqrt(340*(cos(theta1) + cos(0.001 + theta4)) - 200*(1 + cos(0.001 + theta1 + theta4))))/
        (20.*sqrt(pow(-1.7 + cos(theta1) + cos(0.001 + theta4),2) + pow(sin(theta1) - sin(0.001 + theta4),2))) + 100*(sin(theta1) + sin(0.001 + theta4))))/
   (250000*((350 + 200*(cos(0.001 + theta1) - cos(theta4)) + (20*sqrt(340*(cos(0.001 + theta1) + cos(theta4)) - 200*(1 + cos(0.001 + theta1 + theta4)))*(-sin(0.001 + theta1) + sin(theta4)))/
            sqrt(pow(-1.7 + cos(0.001 + theta1) + cos(theta4),2) + pow(sin(0.001 + theta1) - sin(theta4),2)))/2. + 
        (-350 - 200*(cos(0.001 - theta1) - cos(theta4)) - (2*(sin(0.001 - theta1) + sin(theta4))*sqrt(28900 - 10000*(pow(-1.7 + cos(0.001 - theta1) + cos(theta4),2) + pow(sin(0.001 - theta1) + sin(theta4),2))))/
            sqrt(pow(-1.7 + cos(0.001 - theta1) + cos(theta4),2) + pow(sin(0.001 - theta1) + sin(theta4),2)))/2.)*
      (-100*(sin(theta1) - sin(0.001 - theta4)) - ((-1.7 + cos(theta1) + cos(0.001 - theta4))*sqrt(28900 - 10000*(pow(-1.7 + cos(theta1) + cos(0.001 - theta4),2) + pow(sin(theta1) + sin(0.001 - theta4),2))))/
         sqrt(pow(-1.7 + cos(theta1) + cos(0.001 - theta4),2) + pow(sin(theta1) + sin(0.001 - theta4),2)) + 
        ((-340 + 200*(cos(theta1) + cos(0.001 + theta4)))*sqrt(340*(cos(theta1) + cos(0.001 + theta4)) - 200*(1 + cos(0.001 + theta1 + theta4))))/
         (20.*sqrt(pow(-1.7 + cos(theta1) + cos(0.001 + theta4),2) + pow(sin(theta1) - sin(0.001 + theta4),2))) + 100*(sin(theta1) + sin(0.001 + theta4))) - 
     250000*(((-340 + 200*(cos(0.001 + theta1) + cos(theta4)))*sqrt(340*(cos(0.001 + theta1) + cos(theta4)) - 200*(1 + cos(0.001 + theta1 + theta4))))/
         (20.*sqrt(pow(-1.7 + cos(0.001 + theta1) + cos(theta4),2) + pow(sin(0.001 + theta1) - sin(theta4),2))) - 100*(-sin(0.001 - theta1) + sin(theta4)) + 100*(sin(0.001 + theta1) + sin(theta4)) - 
        ((-340 + 200*(cos(0.001 - theta1) + cos(theta4)))*sqrt(28900 - 10000*(pow(-1.7 + cos(0.001 - theta1) + cos(theta4),2) + pow(sin(0.001 - theta1) + sin(theta4),2))))/
         (200.*sqrt(pow(-1.7 + cos(0.001 - theta1) + cos(theta4),2) + pow(sin(0.001 - theta1) + sin(theta4),2))))*
      ((-350 - 200*(cos(theta1) - cos(0.001 - theta4)) + (2*(sin(theta1) + sin(0.001 - theta4))*sqrt(28900 - 10000*(pow(-1.7 + cos(theta1) + cos(0.001 - theta4),2) + pow(sin(theta1) + sin(0.001 - theta4),2))))/
            sqrt(pow(-1.7 + cos(theta1) + cos(0.001 - theta4),2) + pow(sin(theta1) + sin(0.001 - theta4),2)))/2. + 
        (350 + 200*(cos(theta1) - cos(0.001 + theta4)) + (20*sqrt(340*(cos(theta1) + cos(0.001 + theta4)) - 200*(1 + cos(0.001 + theta1 + theta4)))*(-sin(theta1) + sin(0.001 + theta4)))/
            sqrt(pow(-1.7 + cos(theta1) + cos(0.001 + theta4),2) + pow(sin(theta1) - sin(0.001 + theta4),2)))/2.));

    jacobianb = (-500*((-lb - xbase - ll*(cos(theta1) - cos(0.001 - theta4)) + (2*(sin(theta1) + sin(0.001 - theta4))*
             sqrt(pow(lu,2) - (pow(ll,2)*(pow(-(xbase/ll) + cos(theta1) + cos(0.001 - theta4),2) + pow(sin(theta1) + sin(0.001 - theta4),2)))/4.))/
           sqrt(pow(-(xbase/ll) + cos(theta1) + cos(0.001 - theta4),2) + pow(sin(theta1) + sin(0.001 - theta4),2)))/2. + 
       (lb + xbase + ll*(cos(theta1) - cos(0.001 + theta4)) + (2*sqrt(-pow(xbase,2)/4. + pow(lu,2) + (ll*(xbase*(cos(theta1) + cos(0.001 + theta4)) - ll*(1 + cos(0.001 + theta1 + theta4))))/2.)*(-sin(theta1) + sin(0.001 + theta4)))/
           sqrt(pow(-(xbase/ll) + cos(theta1) + cos(0.001 + theta4),2) + pow(sin(theta1) - sin(0.001 + theta4),2)))/2.))/
   (250000*((lb + xbase + ll*(cos(0.001 + theta1) - cos(theta4)) + (2*sqrt(-pow(xbase,2)/4. + pow(lu,2) + (ll*(xbase*(cos(0.001 + theta1) + cos(theta4)) - ll*(1 + cos(0.001 + theta1 + theta4))))/2.)*
              (-sin(0.001 + theta1) + sin(theta4)))/sqrt(pow(-(xbase/ll) + cos(0.001 + theta1) + cos(theta4),2) + pow(sin(0.001 + theta1) - sin(theta4),2)))/2. + 
        (-lb - xbase - ll*(cos(0.001 - theta1) - cos(theta4)) - (2*(sin(0.001 - theta1) + sin(theta4))*
              sqrt(pow(lu,2) - (pow(ll,2)*(pow(-(xbase/ll) + cos(0.001 - theta1) + cos(theta4),2) + pow(sin(0.001 - theta1) + sin(theta4),2)))/4.))/
            sqrt(pow(-(xbase/ll) + cos(0.001 - theta1) + cos(theta4),2) + pow(sin(0.001 - theta1) + sin(theta4),2)))/2.)*
      (-(ll*(sin(theta1) - sin(0.001 - theta4)))/2. - ((-(xbase/ll) + cos(theta1) + cos(0.001 - theta4))*
           sqrt(pow(lu,2) - (pow(ll,2)*(pow(-(xbase/ll) + cos(theta1) + cos(0.001 - theta4),2) + pow(sin(theta1) + sin(0.001 - theta4),2)))/4.))/
         sqrt(pow(-(xbase/ll) + cos(theta1) + cos(0.001 - theta4),2) + pow(sin(theta1) + sin(0.001 - theta4),2)) + 
        ((-xbase + ll*(cos(theta1) + cos(0.001 + theta4)))*sqrt(-pow(xbase,2)/4. + pow(lu,2) + (ll*(xbase*(cos(theta1) + cos(0.001 + theta4)) - ll*(1 + cos(0.001 + theta1 + theta4))))/2.))/
         (ll*sqrt(pow(-(xbase/ll) + cos(theta1) + cos(0.001 + theta4),2) + pow(sin(theta1) - sin(0.001 + theta4),2))) + (ll*(sin(theta1) + sin(0.001 + theta4)))/2.) - 
     250000*(((-xbase + ll*(cos(0.001 + theta1) + cos(theta4)))*sqrt(-pow(xbase,2)/4. + pow(lu,2) + (ll*(xbase*(cos(0.001 + theta1) + cos(theta4)) - ll*(1 + cos(0.001 + theta1 + theta4))))/2.))/
         (ll*sqrt(pow(-(xbase/ll) + cos(0.001 + theta1) + cos(theta4),2) + pow(sin(0.001 + theta1) - sin(theta4),2))) - (ll*(-sin(0.001 - theta1) + sin(theta4)))/2. + (ll*(sin(0.001 + theta1) + sin(theta4)))/2. - 
        ((-xbase + ll*(cos(0.001 - theta1) + cos(theta4)))*sqrt(pow(lu,2) - (pow(ll,2)*(pow(-(xbase/ll) + cos(0.001 - theta1) + cos(theta4),2) + pow(sin(0.001 - theta1) + sin(theta4),2)))/4.))/
         (ll*sqrt(pow(-(xbase/ll) + cos(0.001 - theta1) + cos(theta4),2) + pow(sin(0.001 - theta1) + sin(theta4),2))))*
      ((-lb - xbase - ll*(cos(theta1) - cos(0.001 - theta4)) + (2*(sin(theta1) + sin(0.001 - theta4))*
              sqrt(pow(lu,2) - (pow(ll,2)*(pow(-(xbase/ll) + cos(theta1) + cos(0.001 - theta4),2) + pow(sin(theta1) + sin(0.001 - theta4),2)))/4.))/
            sqrt(pow(-(xbase/ll) + cos(theta1) + cos(0.001 - theta4),2) + pow(sin(theta1) + sin(0.001 - theta4),2)))/2. + 
        (lb + xbase + ll*(cos(theta1) - cos(0.001 + theta4)) + (2*sqrt(-pow(xbase,2)/4. + pow(lu,2) + (ll*(xbase*(cos(theta1) + cos(0.001 + theta4)) - ll*(1 + cos(0.001 + theta1 + theta4))))/2.)*(-sin(theta1) + sin(0.001 + theta4)))/
            sqrt(pow(-(xbase/ll) + cos(theta1) + cos(0.001 + theta4),2) + pow(sin(theta1) - sin(0.001 + theta4),2)))/2.));

    jacobianc = (-500*(((-340 + 200*(cos(0.001 + theta1) + cos(theta4)))*sqrt(340*(cos(0.001 + theta1) + cos(theta4)) - 200*(1 + cos(0.001 + theta1 + theta4))))/
        (20.*sqrt(pow(-1.7 + cos(0.001 + theta1) + cos(theta4),2) + pow(sin(0.001 + theta1) - sin(theta4),2))) - 100*(-sin(0.001 - theta1) + sin(theta4)) + 100*(sin(0.001 + theta1) + sin(theta4)) - 
       ((-340 + 200*(cos(0.001 - theta1) + cos(theta4)))*sqrt(28900 - 10000*(pow(-1.7 + cos(0.001 - theta1) + cos(theta4),2) + pow(sin(0.001 - theta1) + sin(theta4),2))))/
        (200.*sqrt(pow(-1.7 + cos(0.001 - theta1) + cos(theta4),2) + pow(sin(0.001 - theta1) + sin(theta4),2)))))/
   (250000*((350 + 200*(cos(0.001 + theta1) - cos(theta4)) + (20*sqrt(340*(cos(0.001 + theta1) + cos(theta4)) - 200*(1 + cos(0.001 + theta1 + theta4)))*(-sin(0.001 + theta1) + sin(theta4)))/
            sqrt(pow(-1.7 + cos(0.001 + theta1) + cos(theta4),2) + pow(sin(0.001 + theta1) - sin(theta4),2)))/2. + 
        (-350 - 200*(cos(0.001 - theta1) - cos(theta4)) - (2*(sin(0.001 - theta1) + sin(theta4))*sqrt(28900 - 10000*(pow(-1.7 + cos(0.001 - theta1) + cos(theta4),2) + pow(sin(0.001 - theta1) + sin(theta4),2))))/
            sqrt(pow(-1.7 + cos(0.001 - theta1) + cos(theta4),2) + pow(sin(0.001 - theta1) + sin(theta4),2)))/2.)*
      (-100*(sin(theta1) - sin(0.001 - theta4)) - ((-1.7 + cos(theta1) + cos(0.001 - theta4))*sqrt(28900 - 10000*(pow(-1.7 + cos(theta1) + cos(0.001 - theta4),2) + pow(sin(theta1) + sin(0.001 - theta4),2))))/
         sqrt(pow(-1.7 + cos(theta1) + cos(0.001 - theta4),2) + pow(sin(theta1) + sin(0.001 - theta4),2)) + 
        ((-340 + 200*(cos(theta1) + cos(0.001 + theta4)))*sqrt(340*(cos(theta1) + cos(0.001 + theta4)) - 200*(1 + cos(0.001 + theta1 + theta4))))/
         (20.*sqrt(pow(-1.7 + cos(theta1) + cos(0.001 + theta4),2) + pow(sin(theta1) - sin(0.001 + theta4),2))) + 100*(sin(theta1) + sin(0.001 + theta4))) - 
     250000*(((-340 + 200*(cos(0.001 + theta1) + cos(theta4)))*sqrt(340*(cos(0.001 + theta1) + cos(theta4)) - 200*(1 + cos(0.001 + theta1 + theta4))))/
         (20.*sqrt(pow(-1.7 + cos(0.001 + theta1) + cos(theta4),2) + pow(sin(0.001 + theta1) - sin(theta4),2))) - 100*(-sin(0.001 - theta1) + sin(theta4)) + 100*(sin(0.001 + theta1) + sin(theta4)) - 
        ((-340 + 200*(cos(0.001 - theta1) + cos(theta4)))*sqrt(28900 - 10000*(pow(-1.7 + cos(0.001 - theta1) + cos(theta4),2) + pow(sin(0.001 - theta1) + sin(theta4),2))))/
         (200.*sqrt(pow(-1.7 + cos(0.001 - theta1) + cos(theta4),2) + pow(sin(0.001 - theta1) + sin(theta4),2))))*
      ((-350 - 200*(cos(theta1) - cos(0.001 - theta4)) + (2*(sin(theta1) + sin(0.001 - theta4))*sqrt(28900 - 10000*(pow(-1.7 + cos(theta1) + cos(0.001 - theta4),2) + pow(sin(theta1) + sin(0.001 - theta4),2))))/
            sqrt(pow(-1.7 + cos(theta1) + cos(0.001 - theta4),2) + pow(sin(theta1) + sin(0.001 - theta4),2)))/2. + 
        (350 + 200*(cos(theta1) - cos(0.001 + theta4)) + (20*sqrt(340*(cos(theta1) + cos(0.001 + theta4)) - 200*(1 + cos(0.001 + theta1 + theta4)))*(-sin(theta1) + sin(0.001 + theta4)))/
            sqrt(pow(-1.7 + cos(theta1) + cos(0.001 + theta4),2) + pow(sin(theta1) - sin(0.001 + theta4),2)))/2.));

    jacobiand = (500*((350 + 200*(cos(0.001 + theta1) - cos(theta4)) + (20*sqrt(340*(cos(0.001 + theta1) + cos(theta4)) - 200*(1 + cos(0.001 + theta1 + theta4)))*(-sin(0.001 + theta1) + sin(theta4)))/
           sqrt(pow(-1.7 + cos(0.001 + theta1) + cos(theta4),2) + pow(sin(0.001 + theta1) - sin(theta4),2)))/2. + 
       (-350 - 200*(cos(0.001 - theta1) - cos(theta4)) - (2*(sin(0.001 - theta1) + sin(theta4))*sqrt(28900 - 10000*(pow(-1.7 + cos(0.001 - theta1) + cos(theta4),2) + pow(sin(0.001 - theta1) + sin(theta4),2))))/
           sqrt(pow(-1.7 + cos(0.001 - theta1) + cos(theta4),2) + pow(sin(0.001 - theta1) + sin(theta4),2)))/2.))/
   (250000*((350 + 200*(cos(0.001 + theta1) - cos(theta4)) + (20*sqrt(340*(cos(0.001 + theta1) + cos(theta4)) - 200*(1 + cos(0.001 + theta1 + theta4)))*(-sin(0.001 + theta1) + sin(theta4)))/
            sqrt(pow(-1.7 + cos(0.001 + theta1) + cos(theta4),2) + pow(sin(0.001 + theta1) - sin(theta4),2)))/2. + 
        (-350 - 200*(cos(0.001 - theta1) - cos(theta4)) - (2*(sin(0.001 - theta1) + sin(theta4))*sqrt(28900 - 10000*(pow(-1.7 + cos(0.001 - theta1) + cos(theta4),2) + pow(sin(0.001 - theta1) + sin(theta4),2))))/
            sqrt(pow(-1.7 + cos(0.001 - theta1) + cos(theta4),2) + pow(sin(0.001 - theta1) + sin(theta4),2)))/2.)*
      (-100*(sin(theta1) - sin(0.001 - theta4)) - ((-1.7 + cos(theta1) + cos(0.001 - theta4))*sqrt(28900 - 10000*(pow(-1.7 + cos(theta1) + cos(0.001 - theta4),2) + pow(sin(theta1) + sin(0.001 - theta4),2))))/
         sqrt(pow(-1.7 + cos(theta1) + cos(0.001 - theta4),2) + pow(sin(theta1) + sin(0.001 - theta4),2)) + 
        ((-340 + 200*(cos(theta1) + cos(0.001 + theta4)))*sqrt(340*(cos(theta1) + cos(0.001 + theta4)) - 200*(1 + cos(0.001 + theta1 + theta4))))/
         (20.*sqrt(pow(-1.7 + cos(theta1) + cos(0.001 + theta4),2) + pow(sin(theta1) - sin(0.001 + theta4),2))) + 100*(sin(theta1) + sin(0.001 + theta4))) - 
     250000*(((-340 + 200*(cos(0.001 + theta1) + cos(theta4)))*sqrt(340*(cos(0.001 + theta1) + cos(theta4)) - 200*(1 + cos(0.001 + theta1 + theta4))))/
         (20.*sqrt(pow(-1.7 + cos(0.001 + theta1) + cos(theta4),2) + pow(sin(0.001 + theta1) - sin(theta4),2))) - 100*(-sin(0.001 - theta1) + sin(theta4)) + 100*(sin(0.001 + theta1) + sin(theta4)) - 
        ((-340 + 200*(cos(0.001 - theta1) + cos(theta4)))*sqrt(28900 - 10000*(pow(-1.7 + cos(0.001 - theta1) + cos(theta4),2) + pow(sin(0.001 - theta1) + sin(theta4),2))))/
         (200.*sqrt(pow(-1.7 + cos(0.001 - theta1) + cos(theta4),2) + pow(sin(0.001 - theta1) + sin(theta4),2))))*
      ((-350 - 200*(cos(theta1) - cos(0.001 - theta4)) + (2*(sin(theta1) + sin(0.001 - theta4))*sqrt(28900 - 10000*(pow(-1.7 + cos(theta1) + cos(0.001 - theta4),2) + pow(sin(theta1) + sin(0.001 - theta4),2))))/
            sqrt(pow(-1.7 + cos(theta1) + cos(0.001 - theta4),2) + pow(sin(theta1) + sin(0.001 - theta4),2)))/2. + 
        (350 + 200*(cos(theta1) - cos(0.001 + theta4)) + (20*sqrt(340*(cos(theta1) + cos(0.001 + theta4)) - 200*(1 + cos(0.001 + theta1 + theta4)))*(-sin(theta1) + sin(0.001 + theta4)))/
            sqrt(pow(-1.7 + cos(theta1) + cos(0.001 + theta4),2) + pow(sin(theta1) - sin(0.001 + theta4),2)))/2.));
            //vanaf hier weer door met lezen!
    prefx = 1*xMove; //sw3, dit is belangrijk! prefx staat voor P_(reference) en het is de snelheid van de endeffector als 
                        // de button ingedrukt wordt (als emg = boven treshold) is de prefx 1 (da's de rode 1)
    prefy = 1*yMove; //sw2, 
    inverse(prefx, prefy);
}

void printvalue()
{
    pc.printf("X-value: %f \t Y-value: %f \n\r",xend, yend);           // in teraterm zijn de bovenste twee waardes hoeken, de onderste twee zijn de x en y coordinaat

}

//State Machine
void StateMachine()
{
    switch(CurrentState) {                                      //Determine in which state you are

        case Calibration:                                       //Calibration mode
            if(StateBool) {                                     //If you go into this state
                pc.printf("You can start calibrating. \n\r");   //Print that you are in this state
                StateBool = false;                              //Set the bool for the start of a state to false
                EMGCalibration0_timer.attach(&CalibrateEMG0,0.005);     //Start EMG calibration every 0.005 seconds
                EMGCalibration1_timer.attach(&CalibrateEMG1,0.005);     //Start EMG calibration every 0.005 seconds
                FindMax0_timer.attach(&FindMax0,15);            //Find the maximum value after 15 seconds
                FindMax1_timer.attach(&FindMax1,15);            //Find the maximum value after 15 seconds
                SwitchState_timer.attach(&SwitchState,20);      //Switch to the next state after 16 seconds
                blueled = 0;
            }

            if (SwitchStateBool) {                              //If the bool is changed
                CurrentState = WorkingMode;                     //Change the state to the working mode
                StateBool = true;                               //Set the start of a state bool to true
                SwitchStateBool = false;                        //Set the switch bool to false
            }
            break;

        case WorkingMode:                                       //Mode to get the robot working
            if (StateBool) {                                    //If you start to go in this state
                pc.printf("You are know in the working mode. \r\n");    //Print in which mode you are
                StateBool = false;                              //Set the start of state bool to true
                EMGCalibration0_timer.detach();                 //Detach the the calibration
                EMGCalibration1_timer.detach();                 //Detach the calibration
                ReadUseEMG0_timer.attach(&ReadUseEMG0, 0.005);  //Start the use of EMG
                ReadUseEMG1_timer.attach(&ReadUseEMG1,0.005);   //Start the use of EMG
                kin.attach(kinematics, 0.008); // roep de ticker aan (
                simulateval.attach(printvalue, 0.1);
            }
            blueled = 1;                                        //Set the blue led off
            //pc.printf("First EMG: %f, Second EMG: %f \n\r",EMG0Average,EMG1Average);
            break;
    }
}

int main()
{
    //Initial conditions
    theta1 = PI*0.49;                                        //Angle of the left motor
    theta4 = PI*0.49;
    pc.baud(115200);
    greenled = 1;                                               //First turn the LEDs off
    blueled = 1;
    redled = 1;
    filter0.add(&Notch50_0).add(&High0);                        //Make filter chain for the first EMG
    filter1.add(&Notch50_1).add(&High1);                        //Make filter chain for the second EMG
    button.rise(StopProgram);                                   //If the button is pressed, stop program
    
    while(true) {

        StateMachine();                                                          //Start the state machine
    }
}