Revision 28:8bf22ccd11cc, committed 2018-11-01

Comitter:

Ramonwaninge

Date:

Thu Nov 01 14:26:39 2018 +0000

Parent:

27:22bfc75f8d1a

Commit message:

Kinematics WORKS!!

Changed in this revision

--- a/main.cpp	Thu Nov 01 11:17:44 2018 +0000
+++ b/main.cpp	Thu Nov 01 14:26:39 2018 +0000
@@ -8,6 +8,8 @@
 //#include    "FastPWM.h"
 #define PI 3.14159265
 
+//DIKKE PANIEK, MBED MOET VERSIE 151 HEBBEN!!
+
 //Inputs and outputs
 MODSERIAL pc(USBTX, USBRX);                                     //Connecting to PC
 AnalogIn EMG0In(A0);                                            //EMG input 0
@@ -16,7 +18,6 @@
 DigitalOut greenled(LED_GREEN);                                 //Green led
 DigitalOut blueled(LED_BLUE);                                   //Blue led
 DigitalOut redled(LED_RED);                                     //Red led
-
 //Constants
 const float ll = 230;                                          //Length of the lower arm
 const float lu = 198;                                          //Length of the upper arm
@@ -241,22 +242,25 @@
 {
     
     //Below we have the forward kinematics formula. Input should be the measured angles theta1 &theta4. Output 
+    
     float xendsum_ = lb + xbase +ll*(cos(theta1_) - cos(theta4_));
     float xendsqrt1_ = 2*sqrt(-xbase*xbase/4 + lu*lu + ll*(xbase*(cos(theta1_)+cos(theta4_))/2) -ll*(1+ cos(theta1_+theta4_)))*(-sin(theta1_)+sin(theta4_));
     float xendsqrt2_ = sqrt(pow((-xbase/ll+cos(theta1_)+cos(theta4_)),2)+ pow(sin(theta1_) - sin(theta4_),2));
-    xend_ = (xendsum_ + xendsqrt1_/xendsqrt2_)/2;
+     xend_ = (xendsum_ + xendsqrt1_/xendsqrt2_)/2;
 
     float yendsum_ = -le + ll/2*(sin(theta1_)+sin(theta4_));
     float 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_))));
     float yendsqrt2_ = sqrt(pow((-xbase/ll + cos(theta1_)+ cos(theta4_)),2)+ pow((sin(theta1_)-sin(theta4_)),2));
-    yend_ = (yendsum_ + yendsqrt1_/yendsqrt2_);
+     yend_ = (yendsum_ + yendsqrt1_/yendsqrt2_);
 }
 
 //Below we have the inverse kinematics function.
 void inverse(float prex, float prey)
 {
-    theta1 += (prefx*(iJ[0][0])+iJ[0][1]*prey)*dt;                          //theta 1 is itself + the desired speeds in x and y direction, both
-    theta4 += (prefx*iJ[1][0]+iJ[1][1]*prey)*dt;                            //multiplied with a prefactor which comes out of the motor   
+    
+//    pc.printf(
+    theta1 += (prefx*(iJ[0][0])-iJ[0][1]*prey)*dt;                          //theta 1 is itself + the desired speeds in x and y direction, both
+    theta4 += (prefx*iJ[1][0]-iJ[1][1]*prey)*dt;                            //multiplied with a prefactor which comes out of the motor   
                                                                             //the iJ values are defined in the "kinematics" function
     
     //Calling the forward kinematics, to calculate xend and yend
@@ -268,16 +272,27 @@
 {
 
     float xend1,xend2,xend3,yend1,yend2,yend3;
-    const float dq = 0.0001;
+    
+    const float dq = 0.001;      //benadering van 'delta' hoek
+    //pc.printf("%f, \t %f\t \t%f \n\r",xend1, theta1, theta4);
+    
     FK(xend1,yend1,theta1,theta4);
+        //pc.printf("%f \t %f\t %f \n\r",xend1,yend1, theta1, theta4);
+
     FK(xend2,yend2,theta1+dq,theta4);
+    
+            //pc.printf("%f \t %f\t %f \n\r",xend,yend1, theta1, theta4);
+
     FK(xend3,yend3,theta1,theta4+dq);
+        //pc.printf("%f \t %f\t %f \n\r",xend1,yend1, theta1, theta4);
 
     float a,b,c,d;
     a = xend2-xend1;
     b = xend3-xend1;
     c = yend2-yend1;
     d = yend3-yend1;
+    
+    //pc.printf("%f\t%f\t%f\t%f\t\n\r",a,b,c,d);
 
     float Q = 1/(a*d-b*c)/dq;
 
@@ -288,15 +303,16 @@
     iJ[1][1] = a*Q;
     
     
-    prefx = 1*xMove;                    //sw3, Prefx has multiplier one, but that has to become a value dependant on the motor
-    prefy = 1*yMove;                    //sw2,
+    prefx = 0.001*xMove;                    //sw3, Prefx has multiplier one, but that has to become a value dependant on the motor
+    prefy = 0.001*yMove;                    //sw2,
     inverse(prefx, prefy);
 }
 
 // these values are printed for controlling purposes (can later be removed)
 void printvalue()
 {
-    pc.printf("X-value: %f \t Y-value: %f \n\r \t theta 1 = %f \t theta4 =  %f",xend, yend,theta1,theta4);
+    pc.printf("X-value: %f \t Y-value: %f \n\r \t theta 1 = %f \t theta4 =  %f\n\r",xend, yend,theta1,theta4);
+    //pc.printf("%f\n\r",xend1);
     }
 
 //State Machine
@@ -311,7 +327,7 @@
                 StateBool = false;                              //Set the bool for the start of a state to false
                 FindMax0_timer.attach(&FindMax0,15);            //Find the maximum value after 15 seconds
                 FindMax1_timer.attach(&FindMax1,15);            //Find the maximum value after 15 seconds
-                //local_timer.reset();
+                local_timer.reset();
                 local_timer.start();
                 blueled = 0;
             }
@@ -336,6 +352,8 @@
             ReadUseEMG1();//Start the use of EMG
             kinematics(); //Starts calculating the x and y value of the endeffector, as well as the desired values and their BIJBEHORENDE angles
 
+
+
             //motorcontroller
             //Set the blue led off
             //pc.printf("First EMG: %f, Second EMG: %f \n\r",EMG0Average,EMG1Average);
@@ -353,7 +371,7 @@
     filter0.add(&Notch50_0).add(&Notch100_0).add(&Notch150_0).add(&Notch200_0).add(&Low_0).add(&High_0);                        //Make filter chain for the first EMG
     filter1.add(&Notch50_1).add(&Notch100_1).add(&Notch150_1).add(&Notch200_1).add(&Low_1).add(&High_1);                        //Make filter chain for the second EMG
     button.rise(StopProgram);                                   //If the button is pressed, stop program
-    hoofdticker.attach(&StateMachine,0.002);
+    hoofdticker.attach(&StateMachine,0.1);
     while(true) {
         printvalue();
         wait(0.75);

--- a/mbed.bld	Thu Nov 01 11:17:44 2018 +0000
+++ b/mbed.bld	Thu Nov 01 14:26:39 2018 +0000
@@ -1,1 +1,1 @@
-https://os.mbed.com/users/mbed_official/code/mbed/builds/e95d10626187
\ No newline at end of file
+https://os.mbed.com/users/mbed_official/code/mbed/builds/675da3299148
\ No newline at end of file