Diff: main.cpp

Revision:

9:930bd825689f

Parent:

8:697aa3c94209

Child:

10:2b965defcde5

--- a/main.cpp	Tue Oct 30 11:58:36 2018 +0000
+++ b/main.cpp	Tue Oct 30 13:35:51 2018 +0000
@@ -10,12 +10,12 @@
 DigitalOut led2(LED2);
 InterruptIn button2(SW2);
 
-// nog te verwijderen:
+// nog te verwijderen/ aan te passen
 double theta1;
 
 //Joe dit zijn de inputsignalen
-double theta10 = PI*0.5; 
-double theta1i = PI*0.5;
+double theta10 ; 
+double theta1i;
 double theta11; 
 double theta4 = PI*0.5; 
 double emg1; 
@@ -25,11 +25,6 @@
 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; 
@@ -53,12 +48,13 @@
 //Hier definieren we de functies
 Ticker emgcheck;
 Ticker emgcheck2;
+Ticker emgcheck3; 
 
 
 //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.
+                        // 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, 
         theta1 = PI*(theta1/PI + 0.1);          //double theta1-> plus een paar counts (emg*richting)
                                                 //double theta4-> plus een paar counts (emg*richting)
@@ -83,7 +79,7 @@
                     //default = als y = default... break
                     //end
 void flip(){
-    if(button2==0){thetaflip = PI*(thetaflip/PI+0.5);}
+    if(button2==0){thetaflip = PI*(thetaflip/PI+0.5);} // button2==0 -> emg3>= treshold
 }
 
 void inverse(){
@@ -95,17 +91,21 @@
     theta11 = theta1 versie i+1
     */
     
-    if(theta10 == theta1i) {           //initial conditions, oftewel wat moet hij doen, als hij stil staat?
+    if(theta10 == 0) {
+        theta10 = PI*0.5;                       //initial conditions, oftewel wat moet hij doen, als hij stil staat?
         omega1 = 0.01;
         theta1i = theta10+omega1*deltat;
     }
+    else{
     omega1 = (theta1i - theta10)/deltat;
+    
     theta1i = theta10+omega1*deltat;
+    }
     theta11 = theta1i+ (theta1i - theta10)/deltat*deltat;
     
     theta10 = theta1i;
     theta1i = theta11;
-   
+    
 }
 
 int main()
@@ -115,8 +115,9 @@
     //default = theta1 = theta4 = pi/2 
     emgcheck.attach(forward, 0.1);
     emgcheck2.attach(flip, 0.1);
-    button2.rise(inverse);
+    emgcheck.attach(inverse, 0.1);
     ledr=1;
+    pc.printf("%f", theta10);
     while(true){
         
         if (button1 == 0){