Diff: main.cpp

Revision:

8:697aa3c94209

Parent:

7:b59b762c537e

Child:

9:930bd825689f

--- a/main.cpp	Tue Oct 30 10:15:03 2018 +0000
+++ b/main.cpp	Tue Oct 30 11:58:36 2018 +0000
@@ -6,19 +6,36 @@
 
 MODSERIAL pc(USBTX, USBRX);  // connecting to pc
 DigitalIn button1(SW3);      
-DigitalIn button2(SW2);     
 DigitalOut ledr(LED1);
 DigitalOut led2(LED2);
+InterruptIn button2(SW2);
+
+// nog te verwijderen:
+double theta1;
 
 //Joe dit zijn de inputsignalen
-double theta1 = PI*0.5; double theta4 = PI*0.5; double emg1; double emg2; double emg3; double thetaflip = 0;
-double omega1; double omega4;
+double theta10 = PI*0.5; 
+double theta1i = PI*0.5;
+double theta11; 
+double theta4 = PI*0.5; 
+double emg1; 
+double emg2; 
+double emg3; 
+double thetaflip = 0;
+double omega1; 
+double omega4; 
+double deltat = 0.01;
 // Joe dit zijn de outputsignalen
 double x; double y; 
 
+
+
 //Joe dit zijn de constantes 
-double ll = 200.0; double lu = 170.0; double lb = 10.0; double le = 79.0; double xbase = 340;
-//Joe dit zijn de Tickers
+double ll = 200.0; 
+double lu = 170.0; 
+double lb = 10.0; 
+double le = 79.0; 
+double xbase = 340;
 
 //forward kinematics, Check mathematica! Omdat mbed in paniek raakt met meerdere wortels, hebben we de vergelijking opgedeeld in 3 stukken
 //First define the position equation of x
@@ -71,18 +88,23 @@
 
 void inverse(){
     /*
-    We willen ergens beginnen
+    We willen ergens beginnen; toevoeging: als qi=qi-1, EN emg is over treshold, definieerd omega
     Dan de functie qi+1 = qi + (jacobian *(qi - qi-1)/deltaT)deltaT
-    if
+    theta10 = theta1 versie i-1
+    theta1i = theta1 versie i
+    theta11 = theta1 versie i+1
     */
-    theta1-1 = PI*0.5
-    theta4-1 = PI*0.5
-
-    theta11 = 
-    theta14 = 
-
-    theta1+1
-    theta4+1
+    
+    if(theta10 == theta1i) {           //initial conditions, oftewel wat moet hij doen, als hij stil staat?
+        omega1 = 0.01;
+        theta1i = theta10+omega1*deltat;
+    }
+    omega1 = (theta1i - theta10)/deltat;
+    theta1i = theta10+omega1*deltat;
+    theta11 = theta1i+ (theta1i - theta10)/deltat*deltat;
+    
+    theta10 = theta1i;
+    theta1i = theta11;
    
 }
 
@@ -93,19 +115,13 @@
     //default = theta1 = theta4 = pi/2 
     emgcheck.attach(forward, 0.1);
     emgcheck2.attach(flip, 0.1);
+    button2.rise(inverse);
+    ledr=1;
     while(true){
-        if(xend !=xend){ledr = 0;}
-        else{ledr=1;}
-           
-        if(button2==0){pc.printf("Draai! Hij is plat!");}
-    
-       if (button1 == 0){
-     pc.printf("\n\r %f %f \n\r", xend,yend);
-       pc.printf(" %f\n\r", theta1);
-       wait(1.5);
-      }
+        
+        if (button1 == 0){
+           pc.printf("\n\r %f %f \n\r", theta10,theta1i);
+           wait(1.5);
         }
-    
-    
-    
+    }
 }
\ No newline at end of file