Diff: main.cpp.orig

Revision:

20:11fe0aa7f111

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