File content as of revision 25:1123d100d964:

//Libraries
#include "mbed.h"
#include <math.h>
#include <cmath>
#include "MODSERIAL.h"
#include "BiQuad.h"
#include <algorithm>
//#include    "FastPWM.h"
#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 float ll = 230;                                          //Length of the lower arm
const float lu = 198;                                          //Length of the upper arm
const float lb = 50;                                           //Length of the part between the upper arms
const float le = 79;                                           //Length of the end-effector beam
const float 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
float theta1 = PI*0.49;                                        //Angle of the left motor
float theta4 = PI*0.49;                                        //Angle of the right motor
float thetaflip = 0;                                           //Angle of the flipping motor
float prefx;                                                   //Desired x velocity
float prefy;                                                   //Desired y velocity                                                        "
float deltat = 0.01;
float iJ[2][2];
//Time step (dependent on sample frequency)

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

//Kinematics parameters for y
float yendsum;
float yendsqrt1;
float yendsqrt2;
float yend;
float jacobianb;
float 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
Timer local_timer;

//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
}*/


void FK(float &xend_, float &yend_, float theta1_, float theta4_)
{
    float xendsum_ = lb + xbase +ll*(cos(theta1_) - cos(theta4_));
    float xendsqrt1_ = 2*sqrt(-xbase*xbase/4 + lu*lu + ll*(xbase*(cos(theta1_)+cos(theta4_))/2) -ll*(1+ cos(theta1_+theta4_)))*(-sin(theta1_)+sin(theta4_));
    float 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;
    float yendsum_ = -le + ll/2*(sin(theta1_)+sin(theta4_));
    float 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_))));
    float yendsqrt2_ = sqrt(pow((-xbase/ll + cos(theta1_)+ cos(theta4_)),2)+ pow((sin(theta1_)-sin(theta4_)),2));
    yend_ = (yendsum_ + yendsqrt1_/yendsqrt2_);
}

//dit wordt aangeroepen in de tickerfunctie
void inverse(float prex, float 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*(iJ[0][0])+iJ[0][1]*prey)*deltat; //theta 1 is zichzelf plus wat hier staat (is kinematics)
    theta4 += (prefx*iJ[1][0]+iJ[1][1]*prey)*deltat;//"                                                       "
    //Hier worden xend en yend doorgerekend, die formules kan je overslaan

    FK(xend,yend,theta1,theta4);

}



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

//Hieronder rekenen we eerst de aparte dingen van de jacobiaan uit. (sla maar over)
    float xend1,xend2,xend3,yend1,yend2,yend3;
    const float dq = 0.0001;
    FK(xend1,yend1,theta1,theta4);
    FK(xend2,yend2,theta1+dq,theta4);
    FK(xend3,yend3,theta1,theta4+dq);

    float a,b,c,d;
    a = xend2-xend1;
    b = xend3-xend1;
    c = yend2-yend1;
    d = yend3-yend1;

    float Q = 1/(a*d-b*c)/dq;


    iJ[0][0] = d*Q;
    iJ[0][1]= -c*Q;
    iJ[1][0] = -b*Q;
    iJ[1][1] = a*Q;
    /*

    jacobiana = (500*(-(((-(xbase/ll) + cos(theta1) + cos(theta4))*sqrt(-pow(xbase,2)/4. + pow(lu,2) + (ll*(xbase*(cos(theta1) + cos(theta4)) - ll*(1 + cos(theta1 + theta4))))/2.))/
          sqrt(pow(-(xbase/ll) + cos(theta1) + cos(theta4),2) + pow(sin(theta1) - sin(theta4),2))) - (ll*(sin(theta1) + sin(theta4)))/2. +
       ((-xbase + ll*(cos(theta1) + cos(0.002 + theta4)))*sqrt(-pow(xbase,2)/4. + pow(lu,2) + (ll*(xbase*(cos(theta1) + cos(0.002 + theta4)) - ll*(1 + cos(0.002 + theta1 + theta4))))/2.))/
        (ll*sqrt(pow(-(xbase/ll) + cos(theta1) + cos(0.002 + theta4),2) + pow(sin(theta1) - sin(0.002 + theta4),2))) + (ll*(sin(theta1) + sin(0.002 + theta4)))/2.))/
    (-125000*(-(((-(xbase/ll) + cos(theta1) + cos(theta4))*sqrt(-pow(xbase,2)/4. + pow(lu,2) + (ll*(xbase*(cos(theta1) + cos(theta4)) - ll*(1 + cos(theta1 + theta4))))/2.))/
           sqrt(pow(-(xbase/ll) + cos(theta1) + cos(theta4),2) + pow(sin(theta1) - sin(theta4),2))) +
        ((-xbase + ll*(cos(0.002 + theta1) + cos(theta4)))*sqrt(-pow(xbase,2)/4. + pow(lu,2) + (ll*(xbase*(cos(0.002 + theta1) + cos(theta4)) - ll*(1 + cos(0.002 + theta1 + theta4))))/2.))/
         (ll*sqrt(pow(-(xbase/ll) + cos(0.002 + theta1) + cos(theta4),2) + pow(sin(0.002 + theta1) - sin(theta4),2))) - (ll*(sin(theta1) + sin(theta4)))/2. + (ll*(sin(0.002 + theta1) + sin(theta4)))/2.)*
      (ll*(-cos(theta1) + cos(theta4)) + ll*(cos(theta1) - cos(0.002 + theta4)) + (sqrt(-pow(xbase,2) - 2*pow(ll,2) + 4*pow(lu,2) + 2*xbase*ll*cos(theta1) + 2*xbase*ll*cos(theta4) - 2*pow(ll,2)*cos(theta1 + theta4))*
           (sin(theta1) - sin(theta4)))/sqrt(pow(-(xbase/ll) + cos(theta1) + cos(theta4),2) + pow(sin(theta1) - sin(theta4),2)) +
        (sqrt(-pow(xbase,2) - 2*pow(ll,2) + 4*pow(lu,2) + 2*xbase*ll*cos(theta1) + 2*xbase*ll*cos(0.002 + theta4) - 2*pow(ll,2)*cos(0.002 + theta1 + theta4))*(-sin(theta1) + sin(0.002 + theta4)))/
         sqrt(pow(-(xbase/ll) + cos(theta1) + cos(0.002 + theta4),2) + pow(sin(theta1) - sin(0.002 + theta4),2))) +
     125000*(ll*(cos(0.002 + theta1) - cos(theta4)) + ll*(-cos(theta1) + cos(theta4)) + (sqrt(-pow(xbase,2) - 2*pow(ll,2) + 4*pow(lu,2) + 2*xbase*ll*cos(theta1) + 2*xbase*ll*cos(theta4) - 2*pow(ll,2)*cos(theta1 + theta4))*
           (sin(theta1) - sin(theta4)))/sqrt(pow(-(xbase/ll) + cos(theta1) + cos(theta4),2) + pow(sin(theta1) - sin(theta4),2)) +
        (sqrt(-pow(xbase,2) - 2*pow(ll,2) + 4*pow(lu,2) + 2*xbase*ll*cos(0.002 + theta1) + 2*xbase*ll*cos(theta4) - 2*pow(ll,2)*cos(0.002 + theta1 + theta4))*(-sin(0.002 + theta1) + sin(theta4)))/
         sqrt(pow(-(xbase/ll) + cos(0.002 + theta1) + cos(theta4),2) + pow(sin(0.002 + theta1) - sin(theta4),2)))*
      (-(((-(xbase/ll) + cos(theta1) + cos(theta4))*sqrt(-pow(xbase,2)/4. + pow(lu,2) + (ll*(xbase*(cos(theta1) + cos(theta4)) - ll*(1 + cos(theta1 + theta4))))/2.))/
           sqrt(pow(-(xbase/ll) + cos(theta1) + cos(theta4),2) + pow(sin(theta1) - sin(theta4),2))) - (ll*(sin(theta1) + sin(theta4)))/2. +
        ((-xbase + ll*(cos(theta1) + cos(0.002 + theta4)))*sqrt(-pow(xbase,2)/4. + pow(lu,2) + (ll*(xbase*(cos(theta1) + cos(0.002 + theta4)) - ll*(1 + cos(0.002 + theta1 + theta4))))/2.))/
         (ll*sqrt(pow(-(xbase/ll) + cos(theta1) + cos(0.002 + theta4),2) + pow(sin(theta1) - sin(0.002 + theta4),2))) + (ll*(sin(theta1) + sin(0.002 + theta4)))/2.));

    jacobianb = (-250*(ll*(-cos(theta1) + cos(theta4)) + ll*(cos(theta1) - cos(0.002 + theta4)) + (sqrt(-pow(xbase,2) - 2*pow(ll,2) + 4*pow(lu,2) + 2*xbase*ll*cos(theta1) + 2*xbase*ll*cos(theta4) - 2*pow(ll,2)*cos(theta1 + theta4))*
          (sin(theta1) - sin(theta4)))/sqrt(pow(-(xbase/ll) + cos(theta1) + cos(theta4),2) + pow(sin(theta1) - sin(theta4),2)) +
       (sqrt(-pow(xbase,2) - 2*pow(ll,2) + 4*pow(lu,2) + 2*xbase*ll*cos(theta1) + 2*xbase*ll*cos(0.002 + theta4) - 2*pow(ll,2)*cos(0.002 + theta1 + theta4))*(-sin(theta1) + sin(0.002 + theta4)))/
        sqrt(pow(-(xbase/ll) + cos(theta1) + cos(0.002 + theta4),2) + pow(sin(theta1) - sin(0.002 + theta4),2))))/
    (-125000*(-(((-(xbase/ll) + cos(theta1) + cos(theta4))*sqrt(-pow(xbase,2)/4. + pow(lu,2) + (ll*(xbase*(cos(theta1) + cos(theta4)) - ll*(1 + cos(theta1 + theta4))))/2.))/
           sqrt(pow(-(xbase/ll) + cos(theta1) + cos(theta4),2) + pow(sin(theta1) - sin(theta4),2))) +
        ((-xbase + ll*(cos(0.002 + theta1) + cos(theta4)))*sqrt(-pow(xbase,2)/4. + pow(lu,2) + (ll*(xbase*(cos(0.002 + theta1) + cos(theta4)) - ll*(1 + cos(0.002 + theta1 + theta4))))/2.))/
         (ll*sqrt(pow(-(xbase/ll) + cos(0.002 + theta1) + cos(theta4),2) + pow(sin(0.002 + theta1) - sin(theta4),2))) - (ll*(sin(theta1) + sin(theta4)))/2. + (ll*(sin(0.002 + theta1) + sin(theta4)))/2.)*
      (ll*(-cos(theta1) + cos(theta4)) + ll*(cos(theta1) - cos(0.002 + theta4)) + (sqrt(-pow(xbase,2) - 2*pow(ll,2) + 4*pow(lu,2) + 2*xbase*ll*cos(theta1) + 2*xbase*ll*cos(theta4) - 2*pow(ll,2)*cos(theta1 + theta4))*
           (sin(theta1) - sin(theta4)))/sqrt(pow(-(xbase/ll) + cos(theta1) + cos(theta4),2) + pow(sin(theta1) - sin(theta4),2)) +
        (sqrt(-pow(xbase,2) - 2*pow(ll,2) + 4*pow(lu,2) + 2*xbase*ll*cos(theta1) + 2*xbase*ll*cos(0.002 + theta4) - 2*pow(ll,2)*cos(0.002 + theta1 + theta4))*(-sin(theta1) + sin(0.002 + theta4)))/
         sqrt(pow(-(xbase/ll) + cos(theta1) + cos(0.002 + theta4),2) + pow(sin(theta1) - sin(0.002 + theta4),2))) +
     125000*(ll*(cos(0.002 + theta1) - cos(theta4)) + ll*(-cos(theta1) + cos(theta4)) + (sqrt(-pow(xbase,2) - 2*pow(ll,2) + 4*pow(lu,2) + 2*xbase*ll*cos(theta1) + 2*xbase*ll*cos(theta4) - 2*pow(ll,2)*cos(theta1 + theta4))*
           (sin(theta1) - sin(theta4)))/sqrt(pow(-(xbase/ll) + cos(theta1) + cos(theta4),2) + pow(sin(theta1) - sin(theta4),2)) +
        (sqrt(-pow(xbase,2) - 2*pow(ll,2) + 4*pow(lu,2) + 2*xbase*ll*cos(0.002 + theta1) + 2*xbase*ll*cos(theta4) - 2*pow(ll,2)*cos(0.002 + theta1 + theta4))*(-sin(0.002 + theta1) + sin(theta4)))/
         sqrt(pow(-(xbase/ll) + cos(0.002 + theta1) + cos(theta4),2) + pow(sin(0.002 + theta1) - sin(theta4),2)))*
      (-(((-(xbase/ll) + cos(theta1) + cos(theta4))*sqrt(-pow(xbase,2)/4. + pow(lu,2) + (ll*(xbase*(cos(theta1) + cos(theta4)) - ll*(1 + cos(theta1 + theta4))))/2.))/
           sqrt(pow(-(xbase/ll) + cos(theta1) + cos(theta4),2) + pow(sin(theta1) - sin(theta4),2))) - (ll*(sin(theta1) + sin(theta4)))/2. +
        ((-xbase + ll*(cos(theta1) + cos(0.002 + theta4)))*sqrt(-pow(xbase,2)/4. + pow(lu,2) + (ll*(xbase*(cos(theta1) + cos(0.002 + theta4)) - ll*(1 + cos(0.002 + theta1 + theta4))))/2.))/
         (ll*sqrt(pow(-(xbase/ll) + cos(theta1) + cos(0.002 + theta4),2) + pow(sin(theta1) - sin(0.002 + theta4),2))) + (ll*(sin(theta1) + sin(0.002 + theta4)))/2.));

    jacobianc = (-500*(-(((-(xbase/ll) + cos(theta1) + cos(theta4))*sqrt(-pow(xbase,2)/4. + pow(lu,2) + (ll*(xbase*(cos(theta1) + cos(theta4)) - ll*(1 + cos(theta1 + theta4))))/2.))/
          sqrt(pow(-(xbase/ll) + cos(theta1) + cos(theta4),2) + pow(sin(theta1) - sin(theta4),2))) +
       ((-xbase + ll*(cos(0.002 + theta1) + cos(theta4)))*sqrt(-pow(xbase,2)/4. + pow(lu,2) + (ll*(xbase*(cos(0.002 + theta1) + cos(theta4)) - ll*(1 + cos(0.002 + theta1 + theta4))))/2.))/
        (ll*sqrt(pow(-(xbase/ll) + cos(0.002 + theta1) + cos(theta4),2) + pow(sin(0.002 + theta1) - sin(theta4),2))) - (ll*(sin(theta1) + sin(theta4)))/2. + (ll*(sin(0.002 + theta1) + sin(theta4)))/2.))/
    (-125000*(-(((-(xbase/ll) + cos(theta1) + cos(theta4))*sqrt(-pow(xbase,2)/4. + pow(lu,2) + (ll*(xbase*(cos(theta1) + cos(theta4)) - ll*(1 + cos(theta1 + theta4))))/2.))/
           sqrt(pow(-(xbase/ll) + cos(theta1) + cos(theta4),2) + pow(sin(theta1) - sin(theta4),2))) +
        ((-xbase + ll*(cos(0.002 + theta1) + cos(theta4)))*sqrt(-pow(xbase,2)/4. + pow(lu,2) + (ll*(xbase*(cos(0.002 + theta1) + cos(theta4)) - ll*(1 + cos(0.002 + theta1 + theta4))))/2.))/
         (ll*sqrt(pow(-(xbase/ll) + cos(0.002 + theta1) + cos(theta4),2) + pow(sin(0.002 + theta1) - sin(theta4),2))) - (ll*(sin(theta1) + sin(theta4)))/2. + (ll*(sin(0.002 + theta1) + sin(theta4)))/2.)*
      (ll*(-cos(theta1) + cos(theta4)) + ll*(cos(theta1) - cos(0.002 + theta4)) + (sqrt(-pow(xbase,2) - 2*pow(ll,2) + 4*pow(lu,2) + 2*xbase*ll*cos(theta1) + 2*xbase*ll*cos(theta4) - 2*pow(ll,2)*cos(theta1 + theta4))*
           (sin(theta1) - sin(theta4)))/sqrt(pow(-(xbase/ll) + cos(theta1) + cos(theta4),2) + pow(sin(theta1) - sin(theta4),2)) +
        (sqrt(-pow(xbase,2) - 2*pow(ll,2) + 4*pow(lu,2) + 2*xbase*ll*cos(theta1) + 2*xbase*ll*cos(0.002 + theta4) - 2*pow(ll,2)*cos(0.002 + theta1 + theta4))*(-sin(theta1) + sin(0.002 + theta4)))/
         sqrt(pow(-(xbase/ll) + cos(theta1) + cos(0.002 + theta4),2) + pow(sin(theta1) - sin(0.002 + theta4),2))) +
     125000*(ll*(cos(0.002 + theta1) - cos(theta4)) + ll*(-cos(theta1) + cos(theta4)) + (sqrt(-pow(xbase,2) - 2*pow(ll,2) + 4*pow(lu,2) + 2*xbase*ll*cos(theta1) + 2*xbase*ll*cos(theta4) - 2*pow(ll,2)*cos(theta1 + theta4))*
           (sin(theta1) - sin(theta4)))/sqrt(pow(-(xbase/ll) + cos(theta1) + cos(theta4),2) + pow(sin(theta1) - sin(theta4),2)) +
        (sqrt(-pow(xbase,2) - 2*pow(ll,2) + 4*pow(lu,2) + 2*xbase*ll*cos(0.002 + theta1) + 2*xbase*ll*cos(theta4) - 2*pow(ll,2)*cos(0.002 + theta1 + theta4))*(-sin(0.002 + theta1) + sin(theta4)))/
         sqrt(pow(-(xbase/ll) + cos(0.002 + theta1) + cos(theta4),2) + pow(sin(0.002 + theta1) - sin(theta4),2)))*
      (-(((-(xbase/ll) + cos(theta1) + cos(theta4))*sqrt(-pow(xbase,2)/4. + pow(lu,2) + (ll*(xbase*(cos(theta1) + cos(theta4)) - ll*(1 + cos(theta1 + theta4))))/2.))/
           sqrt(pow(-(xbase/ll) + cos(theta1) + cos(theta4),2) + pow(sin(theta1) - sin(theta4),2))) - (ll*(sin(theta1) + sin(theta4)))/2. +
        ((-xbase + ll*(cos(theta1) + cos(0.002 + theta4)))*sqrt(-pow(xbase,2)/4. + pow(lu,2) + (ll*(xbase*(cos(theta1) + cos(0.002 + theta4)) - ll*(1 + cos(0.002 + theta1 + theta4))))/2.))/
         (ll*sqrt(pow(-(xbase/ll) + cos(theta1) + cos(0.002 + theta4),2) + pow(sin(theta1) - sin(0.002 + theta4),2))) + (ll*(sin(theta1) + sin(0.002 + theta4)))/2.));

    jacobiand = (250*(ll*(cos(0.002 + theta1) - cos(theta4)) + ll*(-cos(theta1) + cos(theta4)) + (sqrt(-pow(xbase,2) - 2*pow(ll,2) + 4*pow(lu,2) + 2*xbase*ll*cos(theta1) + 2*xbase*ll*cos(theta4) - 2*pow(ll,2)*cos(theta1 + theta4))*
          (sin(theta1) - sin(theta4)))/sqrt(pow(-(xbase/ll) + cos(theta1) + cos(theta4),2) + pow(sin(theta1) - sin(theta4),2)) +
       (sqrt(-pow(xbase,2) - 2*pow(ll,2) + 4*pow(lu,2) + 2*xbase*ll*cos(0.002 + theta1) + 2*xbase*ll*cos(theta4) - 2*pow(ll,2)*cos(0.002 + theta1 + theta4))*(-sin(0.002 + theta1) + sin(theta4)))/
        sqrt(pow(-(xbase/ll) + cos(0.002 + theta1) + cos(theta4),2) + pow(sin(0.002 + theta1) - sin(theta4),2))))/
    (-125000*(-(((-(xbase/ll) + cos(theta1) + cos(theta4))*sqrt(-pow(xbase,2)/4. + pow(lu,2) + (ll*(xbase*(cos(theta1) + cos(theta4)) - ll*(1 + cos(theta1 + theta4))))/2.))/
           sqrt(pow(-(xbase/ll) + cos(theta1) + cos(theta4),2) + pow(sin(theta1) - sin(theta4),2))) +
        ((-xbase + ll*(cos(0.002 + theta1) + cos(theta4)))*sqrt(-pow(xbase,2)/4. + pow(lu,2) + (ll*(xbase*(cos(0.002 + theta1) + cos(theta4)) - ll*(1 + cos(0.002 + theta1 + theta4))))/2.))/
         (ll*sqrt(pow(-(xbase/ll) + cos(0.002 + theta1) + cos(theta4),2) + pow(sin(0.002 + theta1) - sin(theta4),2))) - (ll*(sin(theta1) + sin(theta4)))/2. + (ll*(sin(0.002 + theta1) + sin(theta4)))/2.)*
      (ll*(-cos(theta1) + cos(theta4)) + ll*(cos(theta1) - cos(0.002 + theta4)) + (sqrt(-pow(xbase,2) - 2*pow(ll,2) + 4*pow(lu,2) + 2*xbase*ll*cos(theta1) + 2*xbase*ll*cos(theta4) - 2*pow(ll,2)*cos(theta1 + theta4))*
           (sin(theta1) - sin(theta4)))/sqrt(pow(-(xbase/ll) + cos(theta1) + cos(theta4),2) + pow(sin(theta1) - sin(theta4),2)) +
        (sqrt(-pow(xbase,2) - 2*pow(ll,2) + 4*pow(lu,2) + 2*xbase*ll*cos(theta1) + 2*xbase*ll*cos(0.002 + theta4) - 2*pow(ll,2)*cos(0.002 + theta1 + theta4))*(-sin(theta1) + sin(0.002 + theta4)))/
         sqrt(pow(-(xbase/ll) + cos(theta1) + cos(0.002 + theta4),2) + pow(sin(theta1) - sin(0.002 + theta4),2))) +
     125000*(ll*(cos(0.002 + theta1) - cos(theta4)) + ll*(-cos(theta1) + cos(theta4)) + (sqrt(-pow(xbase,2) - 2*pow(ll,2) + 4*pow(lu,2) + 2*xbase*ll*cos(theta1) + 2*xbase*ll*cos(theta4) - 2*pow(ll,2)*cos(theta1 + theta4))*
           (sin(theta1) - sin(theta4)))/sqrt(pow(-(xbase/ll) + cos(theta1) + cos(theta4),2) + pow(sin(theta1) - sin(theta4),2)) +
        (sqrt(-pow(xbase,2) - 2*pow(ll,2) + 4*pow(lu,2) + 2*xbase*ll*cos(0.002 + theta1) + 2*xbase*ll*cos(theta4) - 2*pow(ll,2)*cos(0.002 + theta1 + theta4))*(-sin(0.002 + theta1) + sin(theta4)))/
         sqrt(pow(-(xbase/ll) + cos(0.002 + theta1) + cos(theta4),2) + pow(sin(0.002 + theta1) - sin(theta4),2)))*
      (-(((-(xbase/ll) + cos(theta1) + cos(theta4))*sqrt(-pow(xbase,2)/4. + pow(lu,2) + (ll*(xbase*(cos(theta1) + cos(theta4)) - ll*(1 + cos(theta1 + theta4))))/2.))/
           sqrt(pow(-(xbase/ll) + cos(theta1) + cos(theta4),2) + pow(sin(theta1) - sin(theta4),2))) - (ll*(sin(theta1) + sin(theta4)))/2. +
        ((-xbase + ll*(cos(theta1) + cos(0.002 + theta4)))*sqrt(-pow(xbase,2)/4. + pow(lu,2) + (ll*(xbase*(cos(theta1) + cos(0.002 + theta4)) - ll*(1 + cos(0.002 + theta1 + theta4))))/2.))/
         (ll*sqrt(pow(-(xbase/ll) + cos(theta1) + cos(0.002 + theta4),2) + pow(sin(theta1) - sin(0.002 + theta4),2))) + (ll*(sin(theta1) + sin(0.002 + theta4)))/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 \t theta 1 = %f \t theta4 =  %f",xend, yend,theta1,theta4);           // in teraterm zijn de bovenste twee waardes hoeken, de onderste twee zijn de x en y coordinaat

}

//State Machine
void StateMachine()
{
    redled  = 1;
    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
                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
                local_timer.reset();
                local_timer.start();
                blueled = 0;
            }

            CalibrateEMG0();
            CalibrateEMG1();   //Start EMG calibration every 0.005 seconds

            if (local_timer.read() > 20) {                              //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.005); // roep de ticker aan (
                //simulateval.attach(printvalue, 0.1);
            }
            blueled = 1;

            ReadUseEMG0();
            ReadUseEMG1();
            kinematics();

            //motorcontroller
            //Set the blue led off
            //pc.printf("First EMG: %f, Second EMG: %f \n\r",EMG0Average,EMG1Average);
            break;
    }
    redled = 0;
}

int main()
{
    Ticker hoofdticker;
    //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
    hoofdticker.attach(&StateMachine,0.002);
    while(true) {
        printvalue();
        wait(0.75);
        //Start the state machine
    }
}