Diff: main.cpp

Revision:

1:5c2562adca7d

Parent:

0:4a5272e014d8

Child:

2:57237f0a4a34

--- a/main.cpp	Sat Dec 18 10:02:57 2021 +0000
+++ b/main.cpp	Tue Dec 21 06:28:22 2021 +0000
@@ -7,14 +7,14 @@
 //#pragma warning(disable: 4996)
 Serial pc2(USBTX,USBRX);
 Timer tim;
-int times= 200;//実行回数:実行時間は7秒
+int times= 100;//実行回数:実行時間は7秒
 double PI =3.14159265358979323846264338327950288;
 
 using namespace Eigen;
 
 //以下変数定義
 double r=50*PI/180;//斜面の傾き[°]
-double sampling=0.1;//δtの時間[s]
+double sampling=0.01;//δtの時間[s]
 double L[4] = {50.0,50.0,50.0,50.0};//4本のリンク長 後から足したのでL[3]を理論中のL0に対応させる
 double tip[4][3];//足先座標
 double con[4][3] = {  50.0, 50.0,0,
@@ -66,8 +66,8 @@
     L0[0] = 0.0;//ねじ軸原点座標
     L0[1] = 0.0;
     L0[2] = 0.0;
-    vin = 5.0;
-    win = 0.0;
+    vin = 0.0;
+    win = 5.0;
     nol = (double)sqrt(Lin[0] * Lin[0] + Lin[1] * Lin[1] + Lin[2] * Lin[2]);
     for (int i = 0; i < 3; i++)
     {
@@ -87,7 +87,7 @@
         
         count = count + 1;
         double dth;
-        printf("%d  \n", count);
+        //printf("%d  \n", count);
             fwd(0);
             vp(0);
             Jac(0);
@@ -100,7 +100,8 @@
         //pc2.printf("%2.4lf:(%3.3lf, %3.3lf, %3.3lf, %3.3lf)\n\r",t,th[0][0]*180/PI,  th[0][1]*180/PI , th[0][2]*180/PI , th[0][3]*180/PI );
         
         }
-        
+        t=tim.read();
+        pc2.printf("%2.4lf\r\n",t);
     
     return 0;               // ソフトの終了 
 }
@@ -266,7 +267,33 @@
     double c0 = (double)cos(th[leg][0]), s0 = (double)sin(th[leg][0]), c1 = (double)cos(th[leg][1]), s1 = (double)sin(th[leg][1]), s2 = (double)sin(th[leg][2]), s3 = (double)sin(th[leg][2]);
     double c12 = (double)cos(th[leg][1] + th[leg][2]), s12 = (double)sin(th[leg][1] + th[leg][2]), s23 = (double)sin(th[leg][2] + th[leg][3]), c23 = (double)cos(th[leg][2] + th[leg][3]);
     double c123 = (double)cos(th[leg][1] + th[leg][2] + th[leg][3]), s123 = (double)sin(th[leg][1] + th[leg][2] + th[leg][3]);
-    double dadth[4];
+    double cfi=cos(fi),sfi=sin(fi);
+    double x=tip[leg][0],y=tip[leg][1],z=tip[leg][2];
+    
+    double df_da=1/cfi-tan(fi);
+    double df_dfi=a*(-sfi-1)/(cfi*cfi);
+    double da_dx=x*cfi/sqrt(x*x+y*y);
+    double da_dy=y*cfi/sqrt(x*x+y*y);
+    double da_dfi=-sqrt(x*x+y*y)*sfi;
+    double dfi_dx=-x*z/((x*x+y*y+z*z)*sqrt(x*x+y*y));
+    double dfi_dy=-y*z/((x*x+y*y+z*z)*sqrt(x*x+y*y));
+    double dfi_dz=sqrt(x*x+y*y)*z/(x*x+y*y+z*z);
+   
+    dfdth[0]=df_da*(da_dx*Jacbi[leg][0][0]+da_dy*Jacbi[leg][1][0]+da_dfi*(dfi_dx*Jacbi[leg][0][0]+dfi_dy*Jacbi[leg][1][0]+dfi_dz*Jacbi[leg][2][0]))
+             +df_dfi*(dfi_dx*Jacbi[leg][0][0]+dfi_dy*Jacbi[leg][1][0]+dfi_dz*Jacbi[leg][2][0]); 
+             
+    dfdth[1]=df_da*(da_dx*Jacbi[leg][0][1]+da_dy*Jacbi[leg][1][1]+da_dfi*(dfi_dx*Jacbi[leg][0][1]+dfi_dy*Jacbi[leg][1][1]+dfi_dz*Jacbi[leg][2][1]))
+             +df_dfi*(dfi_dx*Jacbi[leg][0][1]+dfi_dy*Jacbi[leg][1][1]+dfi_dz*Jacbi[leg][2][1]);
+              
+    dfdth[2]=df_da*(da_dx*Jacbi[leg][0][2]+da_dy*Jacbi[leg][1][2]+da_dfi*(dfi_dx*Jacbi[leg][0][2]+dfi_dy*Jacbi[leg][1][2]+dfi_dz*Jacbi[leg][2][2]))
+             +df_dfi*(dfi_dx*Jacbi[leg][0][2]+dfi_dy*Jacbi[leg][1][2]+dfi_dz*Jacbi[leg][2][2]); 
+    
+    dfdth[3]=df_da*(da_dx*Jacbi[leg][0][3]+da_dy*Jacbi[leg][1][3]+da_dfi*(dfi_dx*Jacbi[leg][0][3]+dfi_dy*Jacbi[leg][1][3]+dfi_dz*Jacbi[leg][2][3]))
+             +df_dfi*(dfi_dx*Jacbi[leg][0][3]+dfi_dy*Jacbi[leg][1][3]+dfi_dz*Jacbi[leg][2][3]);                              
+   
+   
+    /////////////////////////////以下専攻科1年/////////////////////////////////////////////////////////////////////////////////////////////
+    /*double dadth[4];
     double daindth[4];
 
 
@@ -324,7 +351,8 @@
     dfdth[1] = dadth[1] * (1 / (double)cos(fi) - tan(fi)) + a * (sfi / (cfi * cfi) * dfidth[1] + a / (cfi * cfi) * dfidth[1]);
     dfdth[2] = dadth[2] * (1 / (double)cos(fi) - tan(fi)) + a * (sfi / (cfi * cfi) * dfidth[2] + a / (cfi * cfi) * dfidth[2]);
     dfdth[3] = dadth[3] * (1 / (double)cos(fi) - tan(fi)) + a * (sfi / (cfi * cfi) * dfidth[3] + a / (cfi * cfi) * dfidth[3]);
-    pc2.printf("%lf, %lf, %lf, %lf\r\n",dfdth[0],dfdth[1],dfdth[2],dfdth[3]);
+    */
+    //pc2.printf("%lf, %lf, %lf, %lf\r\n",dfdth[0],dfdth[1],dfdth[2],dfdth[3]);
     //printf("評価関数微分完了\n");
 }
 double search(int leg) {
@@ -335,7 +363,7 @@
     //double dfdth_sum=0.0, dfdth_sum_before=0.0;
     double dth_return=0.0;
     double e=500.0;
-    double e_min = 0.0;
+    double e_min = 100000000000000000000000000.0;
     double dfd_nolm = 0.0;
     dfd(leg);
     dfd_nolm = sqrt(dfdth[0]* dfdth[0]+ dfdth[1]* dfdth[1]+ dfdth[2]* dfdth[2]+ dfdth[3]* dfdth[3]);
@@ -348,9 +376,9 @@
 
    //以下総当たりの探索for文
    //0.5度ずつでdthをずらしながら2000回の探索を行う
-        for (int i = 0; i < 2000; i++) {
+        for (int i = 0; i < 1000; i++) {
             double th0_nolm = 0.0;
-            dth = dth + (double)(0.5 * PI / 180);
+            dth = dth + (double)(1.0 * PI / 180);
             solve(dth, leg, 2);//後退代入でほかの3つのパラメータを導出
             e = 0.0;
             th0_nolm = sqrt(th0[leg][0] * th0[leg][0]+ th0[leg][1]* th0[leg][1]+ th0[leg][2]* th0[leg][2]+ th0[leg][3]*th0[leg][3]);
@@ -403,6 +431,7 @@
     //}
     //printf("探索関数終了");
         //printf("%lf\n", e_min);
+        //
         //pc2.printf("%lf\n\r", dth_return);
         return dth_return;