2021.12.22.16:06
Dependencies: mbed pca9685_2021_12_22 Eigen
Diff: main.cpp
- Revision:
- 1:5c2562adca7d
- Parent:
- 0:4a5272e014d8
- Child:
- 2:57237f0a4a34
diff -r 4a5272e014d8 -r 5c2562adca7d main.cpp --- 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;