![](/media/cache/img/default_profile.jpg.50x50_q85.jpg)
2021.12.22.16:06
Dependencies: mbed pca9685_2021_12_22 Eigen
Diff: main.cpp
- Revision:
- 5:f225e0c61cfc
- Parent:
- 4:8a50c7822dac
diff -r 8a50c7822dac -r f225e0c61cfc main.cpp --- a/main.cpp Sun Dec 26 07:43:22 2021 +0000 +++ b/main.cpp Thu Feb 24 06:12:53 2022 +0000 @@ -23,44 +23,45 @@ Serial pc2(USBTX,USBRX); Timer tim; Timer manage; -int times= 100;//実行回数:実行時間は (sampling)*(times)秒 +int times= 200;//実行回数:実行時間は (sampling)*(times)秒 using namespace Eigen; //以下変数定義 //brent法に必要な変数 -double ax=-0.5*PI/180.0,bx=-0.2*PI/180.0,cx=0.0; +double ax=-0.1*PI/180.0,bx=0.1*PI/180.0,cx=0.0; double fa,fb,fc; //サーボの書き込みに必要な変数 -double servo0[16]={7800.0, 7250.0, 6000.0, 7150.0, 6100.0, 6300.0, 8400.0, 7200.0, 6800.0, 7000.0, 5700.0, 8350.0, 6100.0, 8500.0, 5600.0, 6570.0};//servoの初期値 +double servo0[16]={7700.0, 5850.0, 7050.0, 6850.0, 6000.0, 6300.0, 8400.0, 7200.0, 6700.0, 7000.0, 5650.0, 8400.0, 6000.0, 5100.0, 5600.0, 6570.0};//servoの初期値 int ch[4][4]={{0 ,1 ,2 ,3} , {4 ,5 ,6 ,7} , {8 ,9 ,10,11}, {12,13,14,15} }; -double r=50*PI/180;//斜面の傾き[°] -double sampling=0.1;//δtの時間[s] -double L[4] = {50.0,50.0,50.0,50.0};//4本のリンク長 後から足したのでL[3]を理論中のL0に対応させる +double r=0.0*PI/180;//斜面の傾き[°] +double sampling=0.01;//δtの時間[s] +double L[4] = {65.0,35.0,110.0,38.0};//4本のリンク長 後から足したのでL[3]を理論中のL0に対応させる double tip[4][3];//足先座標 -double con[4][3] = { 50.0, 50.0,0, - -50.0, 50.0,0, - -50.0,-50.0,0, - 50.0,-50.0,0};//脚のコーナー座標,zは必ず0 -double th[4][4] = { {135 * PI / 180, 60 * PI / 180, -30 * PI / 180, -30 * PI / 180}, - {45 * PI / 180, 60 * PI / 180, -30 * PI / 180, -30 * PI / 180}, - {-45 * PI / 180, 60 * PI / 180, -30 * PI / 180, -30 * PI / 180}, - {-135 * PI / 180, 60 * PI / 180, -30 * PI / 180, -30 * PI / 180} };//サーボに入力する角度 +double con[4][3] = { {-55.0, 55.0,0.0}, + { 55.0, 55.0,0.0}, + { 55.0,-55.0,0.0}, + {-55.0,-55.0,0.0}};//脚のコーナー座標,zは必ず0 +double th_ready[4][4] = { {135 * PI / 180, 30 * PI / 180, -75 * PI / 180, -30 * PI / 180}, + {45 * PI / 180, 30 * PI / 180, -75 * PI / 180, -30 * PI / 180}, + {-45 * PI / 180, 30 * PI / 180, -75 * PI / 180, -30 * PI / 180}, + {-135 * PI / 180, 30 * PI / 180, -75 * PI / 180, -30 * PI / 180} };//初期状態 double th0[4][4]= { 0.0,0.0,0.0,0.0, 0.0,0.0, 0.0,0.0, 0.0,0.0, 0.0,0.0, 0.0,0.0, 0.0,0.0 }; //計算用の関節角度 +double th[4][4];//サーボに入力する角度 double Jacbi[4][3][4];//ヤコビアン 脚数×関節×次元 double a,a0, h,fi;//評価関数内の変数 fi=φ double X,tan_u, tan_d;//計算用 //ねじ軸 //Lin:方向, L0:原点座標, vin:ねじ時に沿った速度, win:角速度ベクトルの大きさ -double Lin[3], L0[3], vin,v[3],wg[3],win,nol;//ねじ軸条件 +double Lin[3], L0[3]={0.0,0.0,0.0}, vin,v[3],wg[3],win,nol;//ねじ軸条件 double dfdth[4];//評価関数のナブラ @@ -93,35 +94,46 @@ void servo_write(int ch,double ang);//angに void servo_write7(int ch, double ang); void servo_calib();//全ての角度を0度にする - +void servo_ready();//初期状態 -//以下9軸センサ関係 int main() { double t; pc2.baud(921600); - setup_servo(); - servo_calib(); + //setup_servo(); + for(int u=0; u<4; u++) + { + for(int i=0; i<4; i++) + { + th[u][i]=th_ready[u][i]; + } + } + for(int i=0; i<4; i++) + { + fwd(i); + } + //servo_ready(); wait(2); - - for(int u=0; u<4; u++) + //while(1); + /*for(int u=0; u<4; u++) { for(int i=0; i<4; i++) { servo_write(ch[u][i],th[u][i]); } - } + }*/ + //while(1); wait(2); int count = 0; //入力したねじ運動を換算する - Lin[0] = 0.0; //ねじ軸x - Lin[1] = 0.0; //ねじ軸y - Lin[2] = 1.0;//ねじ軸z + Lin[0] = cos(30.0*PI/180); //ねじ軸x + Lin[1] = sin(30.0*PI/180); //ねじ軸y + Lin[2] = 0.0;//ねじ軸z L0[0] = 0.0;//ねじ軸原点座標 L0[1] = 0.0; L0[2] = 0.0; vin = 0.0; - win = 3.0; + win = 2.0; printf("\r\n\r\n"); nol = (double)sqrt(Lin[0] * Lin[0] + Lin[1] * Lin[1] + Lin[2] * Lin[2]); for (int i = 0; i < 3; i++) @@ -129,52 +141,61 @@ wg[i] = Lin[i] * win / nol; v[i] = Lin[i] * vin / nol; } - - for (int i=0; i < 4; i++) { - fwd(i); - vp(i); - } //printf("%lf , %lf , %lf",vP[0](0,0), vP[0](1, 0), vP[0](2, 0)); //times*δtの時間だけサーボを動かす tim.start(); + manage.start(); - for (int i = 0; i < times;i++){ - manage.reset(); - count = count + 1; + //for (int i = 0; i < times;i++){ + while(tim.read()<=5.0) { + int l=2; + //count = count + 1; double dth; + //for(int l=0;l<4;l++) + //{ //////////計算部///////////////// - fwd(0); - vp(0); - Jac(0); - QR(0); - deff(0); - dfd(0); - ax=-0.5*PI/180.0;bx=-0.49*PI/180.0;cx=0.0; - mnbrak(0,2); - dth=brent(0,ax,bx,cx,0.0001,2); - solve(dth, 0, 1); + t=tim.read(); + fwd(l); + pc2.printf("%3.3lf %3.4lf %3.4lf %3.4lf\r\n",t,tip[l][0],tip[l][1],tip[l][2]); + vp(l); + Jac(l); + QR(l); + deff(l); + dfd(l); + ax=-0.15*PI/180.0;bx=0.15*PI/180.0;cx=0.0; + //2:谷,1:山 + mnbrak(l,2); + dth=brent(l,ax,bx,cx,0.01,2); + solve(dth, l, 1); //////////////////////////////// - - for(int u=0; u<4; u++) + ///* + //} + /*for(int u=0; u<4; u++) { for(int i=0; i<4; i++) { servo_write(ch[u][i],th[u][i]); } - } - t=tim.read(); - 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:( %3.3lf, %3.3lf, %3.3lf, %3.3lf )\n\r",t,th[0][0]*180/PI, th[1][0]*180/PI , th[2][0]*180/PI , th[3][0]*180/PI ); + //pc2.printf("%2.4lf:( %3.3lf, %3.3lf, %3.3lf, %3.3lf )\n\r",t,th[0][1]*180/PI, th[1][1]*180/PI , th[2][1]*180/PI , th[3][1]*180/PI ); + //pc2.printf("%2.4lf:( %3.3lf, %3.3lf, %3.3lf, %3.3lf )\n\r",t,th[0][2]*180/PI, th[1][2]*180/PI , th[2][2]*180/PI , th[3][2]*180/PI ); + //pc2.printf("%2.4lf:( %3.3lf, %3.3lf, %3.3lf, %3.3lf )\n\r",t,th[0][3]*180/PI, th[1][3]*180/PI , th[2][3]*180/PI , th[3][3]*180/PI ); + //pc2.printf("%2.4lf:x( %3.3lf, %3.3lf, %3.3lf, %3.3lf )y( %3.3lf, %3.3lf, %3.3lf, %3.3lf )z( %3.3lf, %3.3lf, %3.3lf, %3.3lf )\n\r", + //t,tip[0][0],tip[1][0],tip[2][0],tip[3][0],tip[0][1],tip[1][1],tip[2][1],tip[3][1],tip[0][2],tip[1][2],tip[2][2],,tip[3][2]); + //pc2.printf("\r\n"); while(1) { if(manage.read()>sampling)break; } + manage.reset(); } t=tim.read(); wait(3); - servo_calib(); + servo_ready(); return 0; // ソフトの終了 } @@ -229,7 +250,7 @@ void vp(int leg) {//5年生の時に作成したもの double crosx, crosy, crosz; - double wA[3] = { (double)(-wg[0] * PI / 180.0),(double)(-wg[1] * PI / 180.0),(double)(-wg[2] * PI / 180.0) }; + double wA[3] = { (double)(wg[0] * PI / 180.0),(double)(wg[1] * PI / 180.0),(double)(wg[2] * PI / 180.0) }; double vA[3] = { (-v[0]),(-v[1]) ,(-v[2]) }; double AP[3] = { (tip[leg][0] - L0[0]),(tip[leg][1] - L0[1]),tip[leg][2] - L0[2] }; if (Lin[2] != 0.0) @@ -254,7 +275,7 @@ tip[leg][0] = (L[3]+L[0] * c1 + L[1] * c12 + L[2]*c123) * c0 + con[leg][0]; //x tip[leg][1] = (L[3]+L[0] * c1 + L[1] * c12 + L[2] * c123) * s0 + con[leg][1]; //y tip[leg][2] = L[0] * s1 + L[1] * s12+L[2]*s123; //z - //printf("fwd finish\n"); + //pc2.printf("leg=%d,( %3.3lf, %3.3lf, %3.3lf)) ",leg,tip[leg][0],tip[leg][1],tip[leg][2]); } void Jac(int leg) { //printf("Jac start\n"); @@ -590,7 +611,6 @@ th0[leg][u] = dth[u]; } } - else {pc2.printf("value det in function solve is incorrect\r\n");} } @@ -625,3 +645,13 @@ } } } +void servo_ready() +{ + for(int u=0; u<4; u++) + { + for(int i=0; i<4; i++) + { + servo_write(ch[u][i],th_ready[u][i]); + } + } + }