Gerhard Berman / Mbed 2 deprecated prog_inversekin_directjacobian

Important changes to repositories hosted on mbed.com

Mbed hosted mercurial repositories are deprecated and are due to be permanently deleted in July 2026.

To keep a copy of this software download the repository Zip archive or clone locally using Mercurial.

It is also possible to export all your personal repositories from the account settings page.

Dependencies:   HIDScope MODSERIAL QEI biquadFilter mbed

Fork of prog_inversekin by Gerhard Berman

Files at this revision

API Documentation at this revision

Revision 18:b4b6181cc943, committed 2016-10-24

Comitter:
GerhardBerman
Date:
Mon Oct 24 13:51:34 2016 +0000
Parent:
17:91d20d362e72
Commit message:
New trial for inversekin with direct inverse Jacobian

Changed in this revision

main.cpp Show annotated file Show diff for this revision Revisions of this file 
--- a/main.cpp	Fri Oct 21 10:39:46 2016 +0000
+++ b/main.cpp	Mon Oct 24 13:51:34 2016 +0000
@@ -36,7 +36,7 @@
 //library settings
 Serial pc(USBTX,USBRX);
 Ticker MeasureTicker, BiQuadTicker; //, TimeTracker; // sampleT;
-//HIDScope    scope(4);
+HIDScope    scope(6);
 
 //set initial conditions
 float error1_prev = 0;
@@ -60,6 +60,7 @@
 float x0 = 1.0;
 float L0 = 1.0;
 float L1 = 1.0;
+float L2 = 1.0;
 float dx;
 float dy;
 float dy_stampdown = 0.05; //5 cm movement downward to stamp
@@ -111,8 +112,8 @@
         //led2 = 1;
         dx = 0;
         dy = 1; //dy_stampdown; //into stamping vertical position?? ~the stamp down action
-        q1_dotOut = dy*(((x0 + L1*cos(q1))*(L0*L0 + L1*sin(q1)*L0 + x0*x0 + L1*cos(q1)*x0 + 1))/(pow(L1*cos(q1),2) + pow(L1*sin(q1),2) + pow(L1*x0*sin(q1),2) + pow(L0*L1*cos(q1),2) - 2*L0*L1*L1*x0*cos(q1)*sin(q1)) - (x0*(pow(L1*cos(q1),2) + pow(L1*sin(q1),2) + L0*L0 + x0*x0 + 2*L0*L1*sin(q1) + 2*L1*x0*cos(q1) + 1))/(pow(L1*cos(q1),2) + pow(L1*sin(q1),2) + pow(L1*x0*sin(q1),2) + pow(L0*L1*cos(q1),2) - 2*L0*L1*L1*x0*cos(q1)*sin(q1))) - dx*(((L0 + L1*sin(q1))*(L0*L0 + L1*sin(q1)*L0 + x0*x0 + L1*cos(q1)*x0 + 1))/(pow(L1*cos(q1),2) + pow(L1*sin(q1),2) + pow(L1*x0*sin(q1),2) + pow(L0*L1*cos(q1),2) - 2*L0*L1*L1*x0*cos(q1)*sin(q1)) - (L0*(pow(L1*cos(q1),2) + pow(L1*sin(q1),2) + L0*L0 + x0*x0 + 2*L0*L1*sin(q1) + 2*L1*x0*cos(q1) + 1))/(pow(L1*cos(q1),2) + pow(L1*sin(q1),2) + pow(L1*x0*sin(q1),2) + pow(L0*L1*cos(q1),2) - 2*L0*L1*L1*x0*cos(q1)*sin(q1)));
-        q2_dotOut = dy*((x0*(L0*L0 + L1*sin(q1)*L0 + x0*x0 + L1*cos(q1)*x0 + 1))/(L1*L1*pow(cos(q1),2) + pow(L1*sin(q1),2) + pow(L1*x0*sin(q1),2) + pow(L0*L1*cos(q1),2) - 2*L0*pow(L1,2)*x0*cos(q1)*sin(q1)) - ((x0 + L1*cos(q1))*(pow(L0,2) + pow(x0,2) + 1))/(pow(L1*cos(q1),2)) + pow(L1*sin(q1),2) + pow(L1*x0*sin(q1),2) + pow(L0*L1*cos(q1),2) - 2*L0*pow(L1,2)*x0*cos(q1)*sin(q1)) - dx*((L0*(L0*L0+L1*sin(q1)*L0+x0*x0+L1*cos(q1)*x0+1))/(pow(L1*cos(q1),2)+pow(L1*sin(q1),2)+pow(L1*x0*sin(q1),2)+pow(L0*L1*cos(q1),2)-2*L0*L1*L1*x0*cos(q1)*sin(q1))-((L0 + L1*sin(q1))*(L0*L0 + x0*x0 + 1))/(pow(L1*cos(q1),2)+pow(L1*sin(q1),2)+pow(L1*x0*sin(q1),2)+pow(L0*L1*cos(q1),2)-2*L0*L1*L1*x0*cos(q1)*sin(q1)));
+        q1_dotOut = (-L1*dx*sin(q1+q2) - L2*dy*cos(q1+q2)) / ( ( (cos(q1) * L1) + (cos(q1 + q2) * L2) ) * ( -sin(q1 + q2) * L2 ) ) -(( cos(q1 + q2) * L2) * ( ( -sin(q1) * L1)-(sin(q1 + q2) * L2) ) );
+        q2_dotOut = ((dx*(L1*sin(q1)+L2*sin(q1+q2)))+dy*(L1*cos(q1)+L2*cos(q1+q2))) / ( ( (cos(q1) * L1) + (cos(q1 + q2) * L2) ) * ( -sin(q1 + q2) * L2 ))-(( cos(q1 + q2) * L2)*( ( -sin(q1) * L1) -(sin(q1 + q2) * L2) ) );
 
         /*
          wait(1);
@@ -127,8 +128,8 @@
         //led2 = 0;
         dx = 1; //-biceps1;
         dy = 0;
-        q1_dotOut = dy*(((x0 + L1*cos(q1))*(L0*L0 + L1*sin(q1)*L0 + x0*x0 + L1*cos(q1)*x0 + 1))/(pow(L1*cos(q1),2) + pow(L1*sin(q1),2) + pow(L1*x0*sin(q1),2) + pow(L0*L1*cos(q1),2) - 2*L0*L1*L1*x0*cos(q1)*sin(q1)) - (x0*(pow(L1*cos(q1),2) + pow(L1*sin(q1),2) + L0*L0 + x0*x0 + 2*L0*L1*sin(q1) + 2*L1*x0*cos(q1) + 1))/(pow(L1*cos(q1),2) + pow(L1*sin(q1),2) + pow(L1*x0*sin(q1),2) + pow(L0*L1*cos(q1),2) - 2*L0*L1*L1*x0*cos(q1)*sin(q1))) - dx*(((L0 + L1*sin(q1))*(L0*L0 + L1*sin(q1)*L0 + x0*x0 + L1*cos(q1)*x0 + 1))/(pow(L1*cos(q1),2) + pow(L1*sin(q1),2) + pow(L1*x0*sin(q1),2) + pow(L0*L1*cos(q1),2) - 2*L0*L1*L1*x0*cos(q1)*sin(q1)) - (L0*(pow(L1*cos(q1),2) + pow(L1*sin(q1),2) + L0*L0 + x0*x0 + 2*L0*L1*sin(q1) + 2*L1*x0*cos(q1) + 1))/(pow(L1*cos(q1),2) + pow(L1*sin(q1),2) + pow(L1*x0*sin(q1),2) + pow(L0*L1*cos(q1),2) - 2*L0*L1*L1*x0*cos(q1)*sin(q1)));
-        q2_dotOut = dy*((x0*(L0*L0 + L1*sin(q1)*L0 + x0*x0 + L1*cos(q1)*x0 + 1))/(L1*L1*pow(cos(q1),2) + pow(L1*sin(q1),2) + pow(L1*x0*sin(q1),2) + pow(L0*L1*cos(q1),2) - 2*L0*pow(L1,2)*x0*cos(q1)*sin(q1)) - ((x0 + L1*cos(q1))*(pow(L0,2) + pow(x0,2) + 1))/(pow(L1*cos(q1),2)) + pow(L1*sin(q1),2) + pow(L1*x0*sin(q1),2) + pow(L0*L1*cos(q1),2) - 2*L0*pow(L1,2)*x0*cos(q1)*sin(q1)) - dx*((L0*(L0*L0+L1*sin(q1)*L0+x0*x0+L1*cos(q1)*x0+1))/(pow(L1*cos(q1),2)+pow(L1*sin(q1),2)+pow(L1*x0*sin(q1),2)+pow(L0*L1*cos(q1),2)-2*L0*L1*L1*x0*cos(q1)*sin(q1))-((L0 + L1*sin(q1))*(L0*L0 + x0*x0 + 1))/(pow(L1*cos(q1),2)+pow(L1*sin(q1),2)+pow(L1*x0*sin(q1),2)+pow(L0*L1*cos(q1),2)-2*L0*L1*L1*x0*cos(q1)*sin(q1)));
+        q1_dotOut = (-L1*dx*sin(q1+q2) - L2*dy*cos(q1+q2)) / ( ( (cos(q1) * L1) + (cos(q1 + q2) * L2) ) * ( -sin(q1 + q2) * L2 ) ) -(( cos(q1 + q2) * L2) * ( ( -sin(q1) * L1)-(sin(q1 + q2) * L2) ) );
+        q2_dotOut = ((dx*(L1*sin(q1)+L2*sin(q1+q2)))+dy*(L1*cos(q1)+L2*cos(q1+q2))) / ( ( (cos(q1) * L1) + (cos(q1 + q2) * L2) ) * ( -sin(q1 + q2) * L2 ))-(( cos(q1 + q2) * L2)*( ( -sin(q1) * L1) -(sin(q1 + q2) * L2) ) );
         }
     else if (biceps1 <= 0 && biceps2 > 0){
         //arm 1 activated, move left
@@ -136,9 +137,8 @@
         //led2 = 1;
         dx = 1; //biceps2;
         dy = 0;
-        q1_dotOut = dy*(((x0 + L1*cos(q1))*(L0*L0 + L1*sin(q1)*L0 + x0*x0 + L1*cos(q1)*x0 + 1))/(pow(L1*cos(q1),2) + pow(L1*sin(q1),2) + pow(L1*x0*sin(q1),2) + pow(L0*L1*cos(q1),2) - 2*L0*L1*L1*x0*cos(q1)*sin(q1)) - (x0*(pow(L1*cos(q1),2) + pow(L1*sin(q1),2) + L0*L0 + x0*x0 + 2*L0*L1*sin(q1) + 2*L1*x0*cos(q1) + 1))/(pow(L1*cos(q1),2) + pow(L1*sin(q1),2) + pow(L1*x0*sin(q1),2) + pow(L0*L1*cos(q1),2) - 2*L0*L1*L1*x0*cos(q1)*sin(q1))) - dx*(((L0 + L1*sin(q1))*(L0*L0 + L1*sin(q1)*L0 + x0*x0 + L1*cos(q1)*x0 + 1))/(pow(L1*cos(q1),2) + pow(L1*sin(q1),2) + pow(L1*x0*sin(q1),2) + pow(L0*L1*cos(q1),2) - 2*L0*L1*L1*x0*cos(q1)*sin(q1)) - (L0*(pow(L1*cos(q1),2) + pow(L1*sin(q1),2) + L0*L0 + x0*x0 + 2*L0*L1*sin(q1) + 2*L1*x0*cos(q1) + 1))/(pow(L1*cos(q1),2) + pow(L1*sin(q1),2) + pow(L1*x0*sin(q1),2) + pow(L0*L1*cos(q1),2) - 2*L0*L1*L1*x0*cos(q1)*sin(q1)));
-        q2_dotOut = dy*((x0*(L0*L0 + L1*sin(q1)*L0 + x0*x0 + L1*cos(q1)*x0 + 1))/(L1*L1*pow(cos(q1),2) + pow(L1*sin(q1),2) + pow(L1*x0*sin(q1),2) + pow(L0*L1*cos(q1),2) - 2*L0*pow(L1,2)*x0*cos(q1)*sin(q1)) - ((x0 + L1*cos(q1))*(pow(L0,2) + pow(x0,2) + 1))/(pow(L1*cos(q1),2)) + pow(L1*sin(q1),2) + pow(L1*x0*sin(q1),2) + pow(L0*L1*cos(q1),2) - 2*L0*pow(L1,2)*x0*cos(q1)*sin(q1)) - dx*((L0*(L0*L0+L1*sin(q1)*L0+x0*x0+L1*cos(q1)*x0+1))/(pow(L1*cos(q1),2)+pow(L1*sin(q1),2)+pow(L1*x0*sin(q1),2)+pow(L0*L1*cos(q1),2)-2*L0*L1*L1*x0*cos(q1)*sin(q1))-((L0 + L1*sin(q1))*(L0*L0 + x0*x0 + 1))/(pow(L1*cos(q1),2)+pow(L1*sin(q1),2)+pow(L1*x0*sin(q1),2)+pow(L0*L1*cos(q1),2)-2*L0*L1*L1*x0*cos(q1)*sin(q1)));
-
+        q1_dotOut = (-L1*dx*sin(q1+q2) - L2*dy*cos(q1+q2)) / ( ( (cos(q1) * L1) + (cos(q1 + q2) * L2) ) * ( -sin(q1 + q2) * L2 ) ) -(( cos(q1 + q2) * L2) * ( ( -sin(q1) * L1)-(sin(q1 + q2) * L2) ) );
+        q2_dotOut = ((dx*(L1*sin(q1)+L2*sin(q1+q2)))+dy*(L1*cos(q1)+L2*cos(q1+q2))) / ( ( (cos(q1) * L1) + (cos(q1 + q2) * L2) ) * ( -sin(q1 + q2) * L2 ))-(( cos(q1 + q2) * L2)*( ( -sin(q1) * L1) -(sin(q1 + q2) * L2) ) );
         }
     else{
         //led1 = 0;
@@ -154,7 +154,7 @@
     //update joint angles
     q1Out = q1Out + q1_dotOut;  //in radians
     q2Out = q2Out + q2_dotOut;
-    
+    /*
     pc.baud(115200);
     pc.printf("dx:          %f \r\n", dx);
     pc.printf("dy:          %f \r\n", dy);
@@ -163,12 +163,20 @@
     pc.printf("q2:          %f \r\n", q2Out);
     pc.printf("q2_dot:      %f \r\n", q2_dotOut);
     
-    pc.printf("Counts1:     %f \r\n", counts1);
+    pc.printf("Counts1:     %i \r\n", counts1);
     pc.printf("Encoder1:    %f \r\n", Encoder1Position);
     pc.printf("Motor1:      %f \r\n", Motor1Position);
-    pc.printf("Counts2:    %f \r\n", counts2);
+    pc.printf("Counts2:     %i \r\n", counts2);
     pc.printf("Encoder2:    %f \r\n", Encoder2Position);
     pc.printf("Motor2:      %f \r\n", Motor2Position);
+    */
+    scope.set(0,Motor1Position);
+    scope.set(1,Motor2Position);
+    scope.set(2,q1_dotOut);
+    scope.set(3,q2_dotOut);
+    scope.set(4,q1Out);
+    scope.set(5,q2Out);
+    scope.send();
     }
 
 void FeedForwardControl1(float q1_dot, float q2_dot, float &motorValue1Out, float &motorValue2Out){
@@ -292,16 +300,18 @@
  //int led2val = led2.read();
   pc.baud(115200);
  pc.printf("Test putty IK");
- MeasureTicker.attach(&MeasureTicker_act, 1.0f); 
+ MeasureTicker.attach(&MeasureTicker_act, 0.1f); 
  bqc.add(&bq1).add(&bq2);
  
  while(1)
     {
         if (MeasureTicker_go){
             MeasureTicker_go=false;
+            float RunnerVariable = 0;
             MeasureAndControl();
             counts1 = Encoder1.getPulses();           // gives position of encoder 
             counts2 = Encoder2.getPulses();           // gives position of encoder 
+            RunnerVariable = 1;
             }
 /*
         if (BiQuadTicker_go){