File content as of revision 14:e3fe54f0a4b4:

//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
MODSERIAL pc(USBTX, USBRX);                                     //Use computer
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




// nog te verwijderen/ aan te passen, zijn dubbel gedefinieerd
double omega1;
double omega4;


//parameters for kinematics
//vorige theta
double theta1 = PI*0.49;                  // huidige/nieuwe theta
double theta4 = PI*0.49;
double thetaflip = 0;                     //angle of the flipping motor
double prefx;                             //Preference for x, will later be defined due to the motor
double prefy;                             //"                                                        "
double deltat = 0.01;                     //tijdstap(moet nog aangepast worden)

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

//Constants
const double ll = 200.0;
const double lu = 170.0;
const double lb = 10.0;
const double le = 79.0;
const double xbase = 340;
const int Length = 10000;                                       //Length of the array for the calibration
const int Parts = 50;                                           //Mean average filter over 50 values

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



//forward kinematics, Check mathematica! Omdat mbed in paniek raakt met meerdere wortels, hebben we de vergelijking opgedeeld in 3 stukken
//check void forward voor de berekeningen
//First define the position equation of x
double xendsum;
double xendsqrt1;
double xendsqrt2;
double xend;
double jacobiana;
double jacobianc;
//Now define the position equation of y
double yendsum;
double yendsqrt1;
double yendsqrt2;
double yend;
double jacobianb;
double jacobiand;


//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 rekenen;                                                 //Not used right now
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
        yMove = false;                                          //Otherwise set movement to false
    }                                 
}




//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*(-(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*((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.));

    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*(((-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)))))/
                (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.));

    jacobiand = (500*((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.))/
                (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.));

            //vanaf hier weer door met lezen!
    prefx = 1*(!button1); //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*(!button2); //sw2, 
    inverse(prefx, prefy);
}






/*In de nieuwe versie hieronder is forward overbodig geworden, sla maar over

Joe, hieronder staan de functies die door de tickers aangeroepen worden
void forward(){        dit is de ticker die zegt, als button=0, theta 1 wordt groter. dan worden x en y doorgerekend
                         hieronder moet veranderd worden naar if button1 == 0, x = x+eenbeetje
                         maar daar moet eerst inverse kinematics voor gebeuren.
    if (button1 == 0){                          als emg1==voorbij treshold, komt waarschijnlijk in de inverse functie
        theta1 = PI*(theta1/PI + 0.1);
     hij is geblokt omdat ik de knop nodig heb
    default = als x = xbase/2... break, okee dit moet hier niet, maar weet niet waar wel...
    }
    else {theta1 = theta1;}
    hieronder komen de doorrekeningen van de hoeken naar de coordinaten, check de mathematicafile voor de afleiding*/

/*void demomode(){}       //Komt nog...

                   als emg2 == voorbij treshold,
                    double theta1 -> plus counts (emg*richting)
                    double theta4 -> plus counts (emg*richting)
                    reken y door

void flip(){
    if(button2==0){thetaflip = PI*(thetaflip/PI+0.5);} // button2==0 moet veranderd naar emg3>= treshold
}suppressed omdat ik button 2 nodig heb...*/


// de beweging voor de xcoordinaat!


//tot aan hier overslaan
void printvalue(){
    pc.printf("\n\r %f %f \n\r %f %f", theta4,theta1, xend, yend);           // in teraterm zijn de bovenste twee waardes hoeken, de onderste twee zijn de x en y coordinaat

}


int main()
{
    //Initial conditions

    theta1 = PI*0.49;
    theta4 = PI*0.49;
    pc.baud(115200);
    //default = theta1 = theta4 = pi/2, 
    kin.attach(kinematics, 0.01); // roep de ticker aan (
    simulateval.attach(printvalue, 1);
    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

    pc.printf("%f", theta1);
    while(true) {

        StateMachine();                                                          //Start the state machine


    }
}