細かいロボットの理論が合っているか聞きたいわけではなく、エラーが起きてそうな記述があれば指摘していただきたいと思っています。
例えば、この記述は０で割ってしまう可能性がある記述であるとか、些細な書き間違いなどです。
些細なことでも構いませんので、宜しくお願いします。
#include <stdio.h>
#include <math.h>
double dob (double,double,double,double,double,double,double,double,double,double);
//PDゲイン
double kp = 60; //位置ゲイン
double kv = 12; //速度ゲイン
//運動モデル
const double dt = 0.01; //サンプリングタイム
const double kt = 33.0; //推力定数
const double omega = 500; //カットオフ周波数 100
double tor_fb[3] = {0,0,0}; //DOB feedback
double tor4_old[3] = {0,0,0};
double tor5_old[3] = {0,0,0};
//エンドパートパラメータ
double L_AC = 0.07;
double L_BE = 0.1;
double L_HF = 0.1;
double L_HG = 0.07;
double L_OH = 0.13;
double MC = 0.05;
double ME = 0.07;
double MF = 0.05;
double MG = 0.07;
double MH = 0.07;
int main(void) {
file = fopen("/Users/tamuragou/Desktop/sample.txt","w");
double t; //時間
double g = 9.8; //重力加速度
double pie = 3.1416; //π
double xw_cmd, yw_cmd, zw_cmd;
double dxw_cmd, dyw_cmd, dzw_cmd;;
double ddxw_ref, ddyw_ref, ddzw_ref;
double xw_res = 0, yw_res = -0.00707, zw_res = 0.137;
double dxw_res = 0, dyw_res = 0, dzw_res =0;
double j[3][3];
double j_inv[3][3];
double d = 0.01, the = pie/4, phi = 0; //モータ角度(初期値)
double delta;
double ddd_ref, ddthe_ref, ddphi_ref;
double A,B;
double m[3];
double h_tor[3];
double g_tor[3];
double d_res = 0, the_res = 0, phi_res = 0;
double dd_res = 0, dthe_res = 0, dphi_res = 0;
double ddd_res = 0, ddthe_res = 0, ddphi_res = 0;
double d_res_old = 0, the_res_old = 0, phi_res_old = 0;
double dd_res_old = 0, dthe_res_old = 0, dphi_res_old = 0;
double ddd_res_old = 0, ddthe_res_old = 0, ddphi_res_old = 0;
double tor_1[3];
double tor_2[3];
double tor_3[3];
double ia[3];
double t_dis[3];
double t_dis_hat[3];
double ia_cmp[3] = {0,0,0};
for (t = 0;t <= 2;t += dt){
//Target position
xw_cmd = 0.0001*t;
yw_cmd = -0.01*sin(pie/4 + 0.01*t);
zw_cmd = 0.13 + 0.01*cos(pie/4 + 0.01*t);
dxw_cmd = 0.0001;
dyw_cmd = -0.001*cos(0.785398 + 0.01*t);
dzw_cmd = -0.001*sin(0.785398 + 0.01*t);
//controller(PD制御)
ddxw_ref = kp*(xw_cmd - xw_res)+kv*(dxw_cmd - dxw_res);
ddyw_ref = kp*(yw_cmd - yw_res)+kv*(dyw_cmd - dyw_res);
ddzw_ref = kp*(zw_cmd - zw_res)+kv*(dzw_cmd - dzw_res);
//Forward Kinematics
//ヤコビアンJ
j[0][0] = sin(the)*sin(phi);
j[0][1] = d*cos(the)*sin(phi);
j[0][2] = d*sin(the)*cos(phi);
j[1][0] = -sin(the)*cos(phi);
j[1][1] = -d*cos(the)*cos(phi);
j[1][2] = d*sin(the)*sin(phi);
j[2][0] = cos(the);
j[2][1] = -d*sin(the);
j[2][2] = 0;
//Inverse Kinematics
delta = j[0][1]*j[1][2]*j[2][0] + j[0][2]*j[1][0]*j[2][1] - j[0][2]*j[1][1]*j[2][0] - j[0][0]*j[1][2]*j[2][1];
j_inv[0][0] = ( -j[1][2]*j[2][1] ) / delta;
j_inv[0][1] = ( j[0][2]*j[2][1] ) / delta;
j_inv[0][2] = ( j[0][1]*j[1][2] - j[0][2]*j[1][1] ) /delta;
j_inv[1][0] = ( j[1][2]*j[2][0] ) / delta;
j_inv[1][1] = ( -j[0][2]*j[2][0] ) / delta;
j_inv[1][2] = -( j[0][0]*j[1][2] - j[0][2]*j[1][0] ) /delta;
j_inv[2][0] = ( j[1][0]*j[2][1] - j[1][1]*j[2][0] ) /delta;
j_inv[2][1] = -( j[0][0]*j[2][1] - j[0][1]*j[2][0] ) /delta;
j_inv[2][2] = ( j[0][0]*j[1][1] - j[0][1]*j[1][0] ) /delta;
//θ” = J^-1 * x"
ddd_ref = j_inv[0][0]*ddxw_ref + j_inv[0][1]*ddyw_ref + j_inv[0][2]*ddzw_ref;
ddthe_ref = j_inv[1][0]*ddxw_ref + j_inv[1][1]*ddyw_ref + j_inv[1][2]*ddzw_ref;
ddphi_ref = j_inv[2][0]*ddxw_ref + j_inv[2][1]*ddyw_ref + j_inv[2][2]*ddzw_ref;
A = MC*L_AC*L_AC + ME*L_BE*L_BE + MF*L_HF*L_HF + MG*L_HG*L_HG;
B = MC*L_AC + ME*L_BE + MF*L_HF + MG*L_HG;
//Inverse dynamics T = Mθ” + H + G
//M
m[0] = MH;
m[1] = A + MH*d*d;
m[2] = ( A + MH*d*d )*sin(the)*sin(the);
//H
h_tor[0] = -MH*d*( ( dthe_res*dthe_res ) + ( sin(the)*sin(the) )*( dphi_res*dphi_res ) );
h_tor[1] = -( A + MH*d*d )*sin(the)*cos(the)*( dphi_res*dphi_res );
h_tor[2] = 0;
//G
g_tor[0] = -MH*g*sin(the)*cos(phi);
g_tor[1] = ( B - MH*d )*g*cos(the)*cos(phi);
g_tor[2] = -(B - MH*d )*g*sin(the)*sin(phi);
//T1 = M*θ”ref
tor_1[0] = m[0]*ddd_ref;
tor_1[1] = m[1]*ddthe_ref;
tor_1[2] = m[2]*ddphi_ref;
//Ia = T1/Kt
ia[0] = tor_1[0]/kt;
ia[1] = tor_1[1]/kt;
ia[2] = tor_1[2]/kt;
//外乱補償電流を加える
ia[0] += ia_cmp[0];
ia[1] += ia_cmp[1];
ia[2] += ia_cmp[2];
//T2 = Kt*Ia
tor_2[0] = kt*ia[0];
tor_2[1] = kt*ia[1];
tor_2[2] = kt*ia[2];
//外乱 Tdis = H + G
t_dis[0] = h_tor[0] + g_tor[0];
t_dis[1] = h_tor[1] + g_tor[1];
t_dis[2] = h_tor[2] + g_tor[2];
//T3 = T2 + Tdis
tor_3[0] = tor_2[0] + t_dis[0];
tor_3[1] = tor_2[1] + t_dis[1];
tor_3[2] = tor_2[2] + t_dis[2];
//θ"res = T3/M
ddd_res = tor_3[0]/m[0];
ddthe_res = tor_3[1]/m[1];
ddphi_res = tor_3[2]/m[2];
//θres = 1/s * θ'res
d_res = d_res_old + (dd_res + dd_res_old)*dt/2.0;
the_res = the_res_old + (dthe_res + dthe_res_old)*dt/2.0;
phi_res = phi_res_old + (dphi_res + dphi_res_old)*dt/2.0;
//θ'res = 1/s * θ"res
dd_res = dd_res_old + (ddd_res + ddd_res_old)*dt/2.0;
dthe_res = dthe_res_old + (ddthe_res + ddthe_res_old)*dt/2.0;
dphi_res = dphi_res_old + (ddphi_res + ddphi_res_old)*dt/2.0;
//前回値としてθ"resを記憶
ddd_res_old = ddd_res;
ddthe_res_old = ddthe_res;
ddphi_res_old = ddphi_res;
//前回値としてθ'resを記憶
dd_res_old = dd_res;
dthe_res_old = dthe_res;
dphi_res_old = dphi_res;
//前回値としてθresを記憶
d_res_old = d_res;
the_res_old = the_res;
phi_res_old = phi_res;
//Tdis^ (DOB)
t_dis_hat[0] = dob(ia[0], ia[1], ia[2], dd_res, dthe_res, dphi_res, m[0], m[1], m[2], 0);
t_dis_hat[1] = dob(ia[0], ia[1], ia[2], dd_res, dthe_res, dphi_res, m[0], m[1], m[2], 1);
t_dis_hat[2] = dob(ia[0], ia[1], ia[2], dd_res, dthe_res, dphi_res, m[0], m[1], m[2], 2);
//外乱補償電流Iacmp
ia_cmp[0] = (1/kt) * t_dis_hat[0];
ia_cmp[1] = (1/kt) * t_dis_hat[1];
ia_cmp[2] = (1/kt) * t_dis_hat[2];
//x'res = J * θ'res
dxw_res = j[0][0]*dd_res + j[0][1]*dthe_res + j[0][2]*dphi_res;
dyw_res = j[1][0]*dd_res + j[1][1]*dthe_res + j[1][2]*dphi_res;
dzw_res = j[2][0]*dd_res + j[2][1]*dthe_res + j[2][2]*dphi_res;
//xresをθresから求める
xw_res = d_res*sin(the_res)*sin(phi_res);
yw_res = -d_res*sin(the_res)*cos(phi_res);
zw_res = L_OH + d_res*cos(the_res);
d = d_res;
the = the_res;
phi = phi_res;
// printf("%lf\t%lf \n",t,zw_res);
// fprintf(file,"%lf\t%lf \n",t,zw_res);
}
}
//外乱オブザーバー
double dob (double ia_dob0, double ia_dob1, double ia_dob2, double d_dob, double the_dob, double phi_dob, double m0, double m1, double m2, double count){
double tor_d1[3];
double tor_d2[3];
double tor_d3[3];
double tor_d4[3];
double tor_d5[3];
double tdis_dob[3];
//T1 = kt*Ia
tor_d1[0] = kt*ia_dob0;
tor_d1[1] = kt*ia_dob1;
tor_d1[2] = kt*ia_dob2;
//T2 = ωc*(MN * θ'res)
tor_d2[0] = omega*( m0*d_dob );
tor_d2[1] = omega*( m1*the_dob );
tor_d2[2] = omega*( m2*phi_dob );
//T3 = T1 + T2
tor_d3[0] = tor_d1[0] + tor_d2[0];
tor_d3[1] = tor_d1[1] + tor_d2[1];
tor_d3[2] = tor_d1[2] + tor_d2[2];
//T4 = ωc*T3 - T_feedback
tor_d4[0] = ( omega*tor_d3[0] ) - tor_fb[0];
tor_d4[1] = ( omega*tor_d3[1] ) - tor_fb[1];
tor_d4[2] = ( omega*tor_d3[2] ) - tor_fb[2];
//LPF///////////////////////////////////////
//T5 = 1/s * T4
tor_d5[0] = tor5_old[0] + ( tor_d4[0] + tor4_old[0] )*dt/2.0;
tor_d5[1] = tor5_old[1] + ( tor_d4[1] + tor4_old[1] )*dt/2.0;
tor_d5[2] = tor5_old[2] + ( tor_d4[2] + tor4_old[2] )*dt/2.0;
//T_feedback = ωc * T5 (フィードバック)
tor_fb[0] = omega*tor_d5[0];
tor_fb[1] = omega*tor_d5[1];
tor_fb[2] = omega*tor_d5[2];
//前回値としてT4を記憶
tor4_old[0] = tor_d4[0];
tor4_old[1] = tor_d4[1];
tor4_old[2] = tor_d4[2];
//前回値としてT5を記憶
tor5_old[0] = tor_d5[0];
tor5_old[1] = tor_d5[1];
tor5_old[2] = tor_d5[2];
//.//////////////////////////////////////////
//Tdis = T5 - T2
tdis_dob[0] = tor_d5[0] - tor_d2[0];
tdis_dob[1] = tor_d5[1] - tor_d2[1];
tdis_dob[2] = tor_d5[2] - tor_d2[2];
//出力
if(count == 0){
return tdis_dob[0];
}else if (count == 1){
return tdis_dob[1];
}else if (count == 2){ //default
return tdis_dob[2];
}else{ //default
return tdis_dob[2];
}
}
メニュー
カレンダー
Clock
最新メンバー
投稿数
統計データ
Link to us
カウンター