Revision 20:11fe0aa7f111, committed 2018-10-31

Comitter:

EvaKrolis

Date:

Wed Oct 31 21:00:51 2018 +0000

Parent:

16:deb42ce3c3a1

Child:

21:73e1cc927968

Commit message:

Working version, with possible good frequency; But not the right Jacobian

Changed in this revision

--- a/main.cpp	Wed Oct 31 20:09:58 2018 +0000
+++ b/main.cpp	Wed Oct 31 21:00:51 2018 +0000
@@ -8,7 +8,7 @@
 #define PI 3.14159265
 
 //Inputs and outputs
-MODSERIAL pc(USBTX, USBRX);                                     // connecting to pc
+MODSERIAL pc(USBTX, USBRX);                                     //Connecting to PC
 AnalogIn EMG0In(A0);                                            //EMG input 0
 AnalogIn EMG1In(A1);                                            //EMG input 1
 InterruptIn button(SW3);                                        //Define button
@@ -16,31 +16,39 @@
 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;
-
 //Constants
-const double ll = 198;
-const double lu = 230;
-const double lb = 50;
-const double le = 79;
-const double xbase = 450-lb;
+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
-//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)
+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
@@ -64,7 +72,6 @@
 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
@@ -73,30 +80,9 @@
 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
@@ -105,14 +91,13 @@
 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 = WorkingMode;                              //Start in the calibration mode
+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
 
@@ -279,109 +264,97 @@
 
 //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.));
+    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.));
+             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.));
+    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*((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 = 10*(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 = 10*(yMove); //sw2,
+    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);
 }
 
@@ -423,8 +396,8 @@
                 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, 1); // roep de ticker aan (
-                simulateval.attach(printvalue, 1);
+                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);
@@ -435,8 +408,7 @@
 int main()
 {
     //Initial conditions
-
-    theta1 = PI*0.49;
+    theta1 = PI*0.49;                                        //Angle of the left motor
     theta4 = PI*0.49;
     pc.baud(115200);
     greenled = 1;                                               //First turn the LEDs off

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/main.cpp.orig	Wed Oct 31 21:00:51 2018 +0000
@@ -0,0 +1,244 @@
+#include "mbed.h"
+#include <math.h>
+#include <cmath>
+#include "MODSERIAL.h"
+#define PI 3.14159265
+
+MODSERIAL pc(USBTX, USBRX);  // connecting to pc
+DigitalIn button1(SW3);      // defining testbutton NEEDS TO BE REMOVED
+DigitalOut ledr(LED1);      // Only for testing
+//DigitalOut led2(LED2);      // Only for testing
+InterruptIn button2(SW2);   //Only for testing
+
+// nog te verwijderen/ aan te passen, zijn dubbel gedefinieerd
+
+
+//Joe dit zijn de inputsignalen (en tussenvariabelen)
+//vorige theta
+float theta1 = PI*0.49;                  // huidige/nieuwe theta
+float theta4 = PI*0.49;
+bool emg1;
+bool emg2;
+bool emg3;
+float thetaflip = 0;
+float omega1;
+float omega4;
+float prefx;
+float prefy;
+float deltat = 0.01;
+//Joe dit zijn de constantes
+float ll = 200.0;
+float lu = 170.0;
+float lb = 10.0;
+float le = 79.0;
+float xbase = 340;
+
+
+//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
+float xendsum;
+float xendsqrt1;
+float xendsqrt2;
+float xend;
+float jacobiana;
+float jacobianc;
+//Now define the pos. eq. of y
+float yendsum;
+float yendsqrt1;
+float yendsqrt2;
+float yend;
+float jacobianb;
+float jacobiand;
+
+
+//Hier definieren we de functies en tickers
+Ticker emgcheck;
+Ticker emgcheck2;
+Ticker rekenen;
+
+
+//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*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*(!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);
+    
+    ledr=!ledr;
+}
+
+
+
+
+
+
+/*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,
+                    float theta1 -> plus counts (emg*richting)
+                    float 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
+
+
+
+int main()
+{
+    //Initial conditions
+
+    theta1 = PI*0.49;
+    theta4 = PI*0.49;
+    pc.baud(115200);
+    //default = theta1 = theta4 = pi/2, 
+    emgcheck.attach(kinematics, 0.008); // roep de ticker aan (
+    ledr = 1;
+    pc.printf("%f", theta1);
+    while(true) {
+
+
+        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
+        wait(1);//anders krijg je DOEZEND waardes...
+
+
+    }
+}
\ No newline at end of file