Diff: main.cpp

Revision:

10:2b965defcde5

Parent:

9:930bd825689f

Child:

11:256e84fea925

--- a/main.cpp	Tue Oct 30 13:35:51 2018 +0000
+++ b/main.cpp	Wed Oct 31 09:09:43 2018 +0000
@@ -5,25 +5,27 @@
 #define PI 3.14159265
 
 MODSERIAL pc(USBTX, USBRX);  // connecting to pc
-DigitalIn button1(SW3);      
-DigitalOut ledr(LED1);
-DigitalOut led2(LED2);
-InterruptIn button2(SW2);
+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
-double theta1;
+// nog te verwijderen/ aan te passen, zijn dubbel gedefinieerd
+double theta11;
 
-//Joe dit zijn de inputsignalen
-double theta10 ; 
-double theta1i;
-double theta11; 
-double theta4 = PI*0.5; 
-double emg1; 
-double emg2; 
-double emg3; 
+//Joe dit zijn de inputsignalen (en tussenvariabelen)
+double theta10;                //vorige theta
+double theta1;                  // huidige/nieuwe theta
+double theta40;
+double theta4; 
+bool emg1; 
+bool emg2; 
+bool emg3; 
 double thetaflip = 0;
 double omega1; 
 double omega4; 
+double prefx;
+double prefy;
 double deltat = 0.01;
 //Joe dit zijn de constantes 
 double ll = 200.0; 
@@ -32,7 +34,9 @@
 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
+//check void forward voor de berekeningen
 //First define the position equation of x
 double xendsum;
 double xendsqrt1;
@@ -45,7 +49,7 @@
 double yend;
 
 
-//Hier definieren we de functies
+//Hier definieren we de functies en tickers
 Ticker emgcheck;
 Ticker emgcheck2;
 Ticker emgcheck3; 
@@ -55,12 +59,13 @@
 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, 
-        theta1 = PI*(theta1/PI + 0.1);          //double theta1-> plus een paar counts (emg*richting)
-                                                //double theta4-> plus een paar counts (emg*richting)
-    //default = als x = xbase/2... break, okee dit moet hier niet
+    /*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;}
+    else {theta1 = theta1;}*/
+    //hieronder komen de doorrekeningen van de hoeken naar de coordinaten, check de mathematicafile voor de afleiding
     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));
@@ -70,59 +75,85 @@
     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);
+    
+    if (xend !=xend){
+        
+        
+        
+    }//end van if xend !=xend
 }
-void demomode(){}   //Alleen nodig in de DEMOMODE
+void demomode(){}       //Komt nog... 
+                    //Alleen nodig in de 
                    //als emg2 == voorbij treshold,
-            //double theta1 -> plus counts (emg*richting)
+                    //double theta1 -> plus counts (emg*richting)
                     //double theta4 -> plus counts (emg*richting)
                     //reken y door
                     //default = als y = default... break
                     //end
-void flip(){
-    if(button2==0){thetaflip = PI*(thetaflip/PI+0.5);} // button2==0 -> emg3>= treshold
-}
+/*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!
 void inverse(){
     /*
-    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
-    theta10 = theta1 versie i-1
-    theta1i = theta1 versie i
-    theta11 = theta1 versie i+1
+    qn = qn-1 + (jacobian^-1)*dPref/dt *deltaT
+    ofwel 
+    thetai+1 = thetai +(jacobian)^-1*vector(deltaX, DeltaY)
+    waar Pref = emg signaal
     */
-    
-    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;
+    emg1 = false;                               
+    if(theta10 == 0){             //als de hoek voor de beweging niet bekend is
+        theta10 = PI*0.5;        //Definieer de hoek als half pi !!DIT KAN OOK ERGENS IN DEFAULT STAAN.
+        theta1 = theta10;
     }
-    else{
-    omega1 = (theta1i - theta10)/deltat;
+    if(button2 == 0){emg1 = true;
+    
+        if(emg1 == true){
+            prefx = 1;                                                          //derivative of Preference = 1 (needs to be adjusted)
+                theta1 = theta10 +/*inverse jacobian keer*/prefx*deltat;        //theta nieuw = oud +jacob*deltax
+                theta10 = theta1;
+        } 
+    }//sluit van if emg1=true
+    
     
-    theta1i = theta10+omega1*deltat;
+    
+}//sluit van void inverse
+
+//dit is voor de ycoordinaat
+void inverse2(){
+    emg2 = false;                            //emg is het inputsignaal van de biceps
+    if(theta40 == 0){
+        theta40 = PI*0.5; 
+        theta4 = theta40;
     }
-    theta11 = theta1i+ (theta1i - theta10)/deltat*deltat;
-    
-    theta10 = theta1i;
-    theta1i = theta11;
-    
+    if(button1 == 0){ 
+        emg2 = true;
+        if(emg2 == true){
+            prefy = 1;
+                theta4 = theta40 + /*inverse jacobian keer*/prefy*deltat;        //theta nieuw = oud +jacob*deltax
+                theta40 = theta4;
+        }  //sluit van if emg2==true    
+    }
+
 }
 
+
 int main()
 {
     
     pc.baud(115200);
     //default = theta1 = theta4 = pi/2 
-    emgcheck.attach(forward, 0.1);
-    emgcheck2.attach(flip, 0.1);
+    emgcheck3.attach(forward, 0.1);
+    emgcheck2.attach(inverse2, 0.1);
     emgcheck.attach(inverse, 0.1);
     ledr=1;
     pc.printf("%f", theta10);
     while(true){
         
-        if (button1 == 0){
-           pc.printf("\n\r %f %f \n\r", theta10,theta1i);
-           wait(1.5);
-        }
+        //if (button1 == 0){
+           pc.printf("\n\r %f %f \n\r %f %f", theta4,theta1, xend, yend);
+           wait(0.5);
+        //}
     }
 }
\ No newline at end of file