// crane_sample.c 2010/3/22 #include #include #include #include "sbox.h" // sBOXライブラリヘッダファイル // 諸定数の定義 #define CONV 3276.7 // AD/DAデータ（整数）--電圧[V]間の変換係数　データ/CONV=電圧[V] #define DAMAX 4 // DAのチャンネル数 #define ADMAX 8 // ADのチャンネル数 #define SAMP_FREQ 1000. // サンプリング周波数[Hz] #define DATA_NUM_MAX 1601 // 実験データの記録長 #define DATA_SAMP 10 // DATA_SAMPサンプル毎に実験データを記録 #define Comp 1.6 // 台車駆動系の不感帯補償のためのオフセット電圧[V] #define T0 0.1 // 台車駆動系の1次遅れ補償の時定数[s] #define K0 1.0 // 〃　ゲイン #define Td 0.01 // 近似微分器の時定数[s] #define Fv 0.6 // 台車駆動系の速度フィードバック補償のゲイン #define T0_actual 0.1 // 補償後の台車駆動系の実際の時定数[s] #define K0_actual 1. // 〃　ゲイン #define C_r 0.0543595 // 台車のポテンショメータの電圧[V]--変位[m]間の変換係数　345/360*(44/16)^2*pi*Dp[m]/24[V] [m/V],Dp=0.0573[m] #define C_th 1.2964805 // 振子のポテンショメータの電圧[V]--角度[rad]間の変換係数 実測値 [rad/V] #define pi 3.1416 // 円周率 #define g 9.80665 // 重力加速度[m/s/s] #define Grav 0.25 // 荷駆動系の重力補償[V] #define Comp_L 1.5 // 〃　不感帯補償のためのオフセット電圧[V] #define T0_L 0.05 // 〃　1次遅れ補償の時定数[s] #define K0_L 1.0 // 〃　ゲイン #define Fv_L 0.8 // 〃　速度フィードバック補償のゲイン #define au 4. // 荷の巻上げ巻き下ろし速度[V] #define a_L 0.075 // L[m] = a_L*l[V] + b_L #define b_L 0.43 #define L_bar 0.094 // 振子角測定リンクの支点から糸の接触点までの距離[m] #define L_center 0.06 // 糸のプーリ接触点から振子角測定リンク接触点までの距離[m] #define T_th 0.1 // 振子角測定信号に対する一次遅れフィルタ時定数[s] #define au_r 5. // 台車の移動速度[V]（制振制御なしの場合） // 大域変数の定義．割り込み関数内で再利用する変数はvolatile宣言する． volatile int counter1=0; // DATA_SAMP毎に0にセットするカウンター volatile long datanum=0; // データ番号 volatile double r0; // 台車の初期値[V] volatile double a; // 荷の振れ角の振幅制限[rad] volatile double t=0.; // 時間[s] volatile double dt=1./SAMP_FREQ; // サンプリング周期[s] volatile double x_d=0.; // 近似微分要素の状態(dr用) volatile double x_f=0.; // 1次遅れ補償要素の状態(rの制御用) volatile double exp_data[6][1610]; // 実験データ volatile double th0; // 振子角の初期値[V] volatile double r; // 台車位置 volatile double th=0.; // 平滑化された振子角[rad] volatile double ks; // 飽和制御のパラメータ volatile double T; // 補償後の台車系の時定数[s] volatile int cart_limit=0; // 台車が許容範囲外であれば1 volatile double x_d_L; // 近似微分要素の状態(dl用) volatile double x_f_L=0.; // 1次遅れ補償要素の状態(lの制御用) volatile double l; // ロープ長[V] volatile double K_L=1./2./T0_L; // Lのサーボ系の減衰比をζ=0.707とするゲイン volatile double L; // ロープ長[m] volatile double lx[2]={-2.,-2.}; // ロープ長の目標値[V] L[m] = a_L*lx[V] + b_L volatile double tl[1]={5.}; volatile double x_d_th=0.; // 近似微分要素の状態(dth用) volatile double s31,s33,s34,s41,s42,s44; // 座標変換行列Sの要素 volatile double rf; // 台車の目標移動距離[m] volatile double fopt1,fopt2; // 飽和制御のゲイン volatile double vmax; // 台車速度制限のパラメータ volatile double L0; // L[m]の最大値 volatile double K_r=1./2./T0; // rのサーボ系の減衰比をζ=0.707とするゲイン（制振制御なしの場合） double sgn(zz) // 符号関数の定義 double zz; { double x=1; if (zz<0) x=-1; if (zz==0) x=0; return(x); } double sat(zz,aa) // 飽和関数の定義 double zz,aa; { double x; x=zz; if(fabs(x)>aa) x=aa*sgn(x); return(x); } /************************************************************************ 割り込み関数（ここに制御則を記述する） *************************************************************************/ interrupt void c_int02() { int V_dir,V_abs; // ローカル変数の定義 double ur,ur1,dr,ur_v,mu,v; double y,dy; int V_dir_L,V_abs_L; double ul,ul1,dl,ul_v,ref_L=1.; double dL,dth,w3,w4,th_m; r = (double)sbox_AdGet(0)/CONV-r0; // 台車位置[V]（初期値を0とする） th_m = L_bar/L_center*C_th*((double)sbox_AdGet(1)/CONV-th0); // 振子角[rad]（垂下状態を0とする） l = -(double)sbox_AdGet(2)/CONV; // ロープ長[V] th+=(th_m-th)*dt/T_th; // 振子角を1次遅れフィルタにより平滑化 if(fabs(r+r0)>7.) // 台車が可動範囲外ならば停止 cart_limit=1; if(fabs(l)>4.) // 荷が可動範囲外ならば停止 cart_limit=1; x_d+=(r-x_d)*dt/Td; dr=(r-x_d)/Td; // rの近似微分 x_d_L+=(l-x_d_L)*dt/Td; dl=(l-x_d_L)/Td; // lの近似微分 x_d_th+=(th-x_d_th)*dt/Td; dth=(th-x_d_th)/Td; // thの近似微分 L = a_L*l+b_L; // ロープ長[m]の計算 dL= a_L*dl; // Lの近似微分[m/s] y=C_r*r-L*th-rf; // rの目標値rfに対するy dy=C_r*dr-dL*th-L*dth; // yの近似微分 w3=s31*th+s33*y+s34*dy; // 不安定部分系の状態（座標変換後） w4=s41*th+s42*dth+s44*dy; v=-sat(ks*(fopt1*w3+fopt2*w4),a); // 飽和制御 if(sgn(v)*w4>vmax*0.98) // 台車の速度制限を考慮 v=0.; mu = L*(g*sin(th)/L-th/(T*T)-2.*dth/T+v/(T*T)); // 制御入力mu ur = (T0_actual*mu+C_r*dr)/(C_r*K0_actual); // 補償された駆動系への入力 //ur=sat(K_r*(rf/C_r-r),au_r); // 制振制御なしの場合この行を有効にする． x_f+=(K0*ur-x_f)*dt/T0; // 1次遅れフィルタ ur1=Fv*(x_f-dr); // 駆動系の速度フィードバック補償 V_dir=(int)(5.0*CONV); // ドライバへの回転方向指令電圧．5Vで正回転，0Vで逆回転． if(ur1<0.0) V_dir=(int)(0.0*CONV); ur_v=sat(fabs(ur1)+Comp,5.0); // ドライバへの速度指令電圧（5V以下）．Compは不感帯補償のための一定電圧． V_abs=(int)(ur_v*CONV); // ur_vのDA出力への変換 if(datanum>=DATA_NUM_MAX-1){ // 終了処理 V_abs=0; V_dir=0; } if(cart_limit==1){ // 終了処理 V_abs=0; V_dir=0; } sbox_DaPut( 0, V_abs); // 速度指令電圧をDA0チャンネルへ出力 sbox_DaPut( 1, V_dir); // 回転方向指令電圧をDA1チャンネルへ出力 ref_L=lx[0]; // Lの目標値 if(t>tl[0]) ref_L=lx[1]; ul=sat(K_L*(ref_L-l),au); // auでldotを指定 x_f_L+=(K0_L*ul-x_f_L)*dt/T0_L; // 1次遅れフィルタ ul1=Fv_L*(x_f_L-dl); V_dir_L=(int)(0.*CONV); // up // ドライバへの回転方向指令電圧．5Vで正回転，0Vで逆回転． ul_v=sat(fabs(ul1)+Comp_L+Grav,5.0); // 巻上げ時に重力補償 if(ul1>0.0){ V_dir_L=(int)(5.*CONV); // down ul_v=sat(fabs(ul1)+Comp_L,5.0); // ドライバへの速度指令電圧（5V以下）．Compは不感帯補償のための一定電圧． } V_abs_L=(int)(ul_v*CONV); // ur_vのDA出力への変換 if(datanum>=DATA_NUM_MAX-1){ // 終了処理 V_abs_L=0; V_dir_L=0; } if(cart_limit==1){ // 終了処理 V_abs_L=0; V_dir_L=0; } sbox_DaPut( 2, V_abs_L); // 速度指令電圧をDA0チャンネルへ出力 sbox_DaPut( 3, V_dir_L); // 回転方向指令電圧をDA1チャンネルへ出力 if(counter1==DATA_SAMP){ // 指定サンプリング毎に実験結果を記録 if(datanum0.5) a=0.5; // aの上限を0.5[m]とする． L0=a_L*3.+b_L; // Lの最大値[m]（l=3[V]） printf("L0 = %f\n",L0); T=sqrt(L0/g); // 補償された台車系の時定数[s] // 座標変換行列Sの要素 s31=T*T; s33=1./g; s34=2.*T/g; s41=2.*T; s42=s31; s44=s33; printf("Enter rf[m] "); // 台車の目標値（台車の初期位置を0とする） scanf("%lf",&rf); // フィードバックゲインFの計算 fopt1=sqrt(a*fabs(rf)/g); fopt2=0.5*fabs(rf)/g; fnorm=sqrt(fopt1*fopt1+fopt2*fopt2); fopt1/=fnorm; // Fの正規化 fopt2/=fnorm; printf("fopt1 = %f fopt2 = %f\n",fopt1,fopt2); printf("Enter rdotmax[m/s] "); // 台車速度の上限 scanf("%lf",&rdotmax); vmax=rdotmax/g; printf("Enter ks>0 "); // 飽和制御のゲインks=20.） scanf("%lf",&ks); printf("start=1, stop=0 "); // 1で制御開始．1以外の整数で制御中止 scanf("%d",&cont_start); if(cont_start!=1) exit(-1); sbox_Init(); // システムの設定 if(0 != sbox_AdTrgSet( TRG_SOFT )) // ソフトでのADトリガ設定 exit( -1 ); while( sbox_AdDone() != 1 ); // AD変換終了まで待つ． r0 = (double)sbox_AdGet(0)/CONV; // 台車の初期値[V] l = -(double)sbox_AdGet(2)/CONV; // 初期ロープ長[V] x_d_L = l; th0=(double)sbox_AdGet(1)/CONV; // 初期振子角の電圧[V]（振子は真下） if( 0 != sbox_AdTrgSet( TRG_TIMER0 ) ) // ADの設定 exit( -1 ); if( 0 != sbox_IntSet( AD_DONE, EINT4, c_int02 ) ) // ADの割り込み設定 exit( -1 ); sbox_DaCtrl( ON ); // DAの設定 if( 0 != sbox_DaTrgSet( TRG_TIMER0 )) exit( -1 ); clock_set( SAMP_FREQ, TIMER_0 ); // 内部クロックの周波数設定と出力開始 while(1) if(datanum==DATA_NUM_MAX){ clock_stop(TIMER_0); // 内部クロック出力の停止 break; } printf("end of control\n"); printf("th0 = %f\n",th0); fp1=fopen("data1.txt","w"); // 記録した実験データのファイルへの書き込み for(i=0;i