Diff: main.cpp

Revision:

58:6660127e5d34

Parent:

57:099ebbb041b0

--- a/main.cpp	Fri Nov 02 10:13:33 2018 +0000
+++ b/main.cpp	Fri Nov 02 10:20:02 2018 +0000
@@ -332,111 +332,15 @@
 
 //  ========================= KINEMATICS FUNCTIONS ============================
 
-
-
-//forward kinematics function , &xend and&yend are output.
-void kinematicsForward1(float theta1_, float theta4_)
-{
-
-    //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));
-    xend1 = (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));
-    yend1 = (yendsum_ + yendsqrt1_/yendsqrt2_);
-    
-}
-
-//forward kinematics function , &xend and&yend are output.
-void kinematicsForward2(float theta1_, float theta4_)
-{
-
-    //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));
-    xend2 = (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));
-    yend2 = (yendsum_ + yendsqrt1_/yendsqrt2_);
-    
-}
-
-//forward kinematics function , &xend and&yend are output.
-void kinematicsForward3(float theta1_, float theta4_)
-{
-
-    //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));
-    xend3 = (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));
-    yend3 = (yendsum_ + yendsqrt1_/yendsqrt2_);
-    
-}
-
-
-//Below we have the inverse kinematics function.
-void kinematicsInverse(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
-    //the iJ values are defined in the "kinematics" function
-
-    //Calling the forward kinematics, to calculate xend and yend
-    kinematicsForward1(/*xend,yend,*/angleCurrent(countsCalibratedL),angleCurrent(countsCalibratedR));
-
-}
-
-void kinematics()
-{
-    //float xend1,xend2,xend3,yend1,yend2,yend3;
-
-    const float dq = 0.001;      //benadering van 'delta' hoek
-    
-    kinematicsForward1(/*xend1,yend1,*/angleCurrent(countsCalibratedL),angleCurrent(countsCalibratedR));
-    kinematicsForward2(/*xend2,yend2,*/angleCurrent(countsCalibratedL)+dq,angleCurrent(countsCalibratedR));
-    kinematicsForward3(/*xend3,yend3,*/angleCurrent(countsCalibratedL),angleCurrent(countsCalibratedR)+dq);
-    
-    //float a,b,c,d;
-    a = xend2-xend1;
-    b = xend3-xend1;
-    c = yend2-yend1;
-    d = yend3-yend1;
-
-    float Q = 1/(a*d-b*c)/dq;
-
-    iJ[0][0] = d*Q;
-    iJ[0][1]= -c*Q;
-    iJ[1][0] = -b*Q;
-    iJ[1][1] = a*Q;
-
-    prefx = 0.00001*xMove;                    //sw3, Prefx has multiplier one,    // Gain aanpassen eventueel??
-    // but that has to become a value
-    // dependant on the motor
-    prefy = 0.00001*yMove;                    //sw2,
-    kinematicsInverse(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\n\r",xend, yend,theta1,theta4);
-    //pc.printf("%f\n\r",xend1);
+void EMGMotorLink(){
+    if (xMove == true){
+        theta1 = theta1*0.1;
+        theta4 = -theta4*0.1;
+    }
+    if(yMove == true){
+        theta1 = -theta1*0.1;
+        theta4 = theta4*0.1;
+    }
 }
 
 //  ============================ MOTOR FUNCTIONS ===============================
@@ -504,7 +408,7 @@
 
 void    motorTurnL()    // main function for movement of motor 1, all above functions with an extra tab are called
 {
-    float   angleReferenceL = angleDesired();                              // insert kinematics output here instead of angleDesired()
+    float   angleReferenceL = theta4;                              // insert kinematics output here instead of angleDesired()
     angleReferenceL = -angleReferenceL;                                     // different minus sign per motor
     countsL = countsInputL();                                       // different encoder pins per motor
     angleCurrentL = angleCurrent(countsL);                                  // different minus sign per motor
@@ -948,11 +852,11 @@
             
             ReadUseEMG0();                  //Start the use of EMG
             ReadUseEMG1();                  //Start the use of EMG
-            pc.printf("theta1 and 4: %f \t\t %f \n\r",theta1,theta4);
-            kinematics();
-            //motorTurnR();
-            //motorTurnL();
-            
+            //pc.printf("theta1 and 4: %f \t\t %f \n\r",theta1,theta4);
+            //kinematics();
+            motorTurnR();
+            motorTurnL();
+            EMGMotorLink();         
             
 
 //      --------------------------- transition ----------------------------