//  struct_sample.c			2009/08/09
#include <stdlib.h>
#include <stdio.h>
#include <math.h>
#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 1501		// 実験データの記録長
#define DATA_SAMP 10			// DATA_SAMPサンプル毎に実験データを記録
#define Comp 1.8			// 駆動系の不感帯補償のためのオフセット電圧[V]
#define T0 0.2				// 駆動系の1次遅れ補償の時定数
#define K0 1.0				// 〃　ゲイン
#define Td 0.01				// 近似微分器の時定数
#define Fv 0.5				// 駆動系の速度フィードバック補償のゲイン
#define T0_actual 0.2			// 補償後の駆動系の実際の時定数
#define K0_actual 0.8			// 〃　ゲイン
#define T 0.1				// 閉ループ系の時定数
#define C_r 0.019635			// 台車のポテンショメータの電圧[V]--変位[m]間の変換係数　pi*0.015[m]*10[rev]/24[V]
#define m 0.34				// 台車の質量[kg]
#define M 0.6				// 床の質量[kg]
#define H 0.988				// ビーム長[m]
#define K 22.32				// ビームのばね係数[Nm/rad]
#define pi 3.1416
#define C_ddtheta 11.95			// 加速度の電圧[V]を[m/s/s]に変える変換係数

//	大域変数の定義．割り込み関数内で再利用する変数はvolatile宣言する．
volatile int counter1=0;				// DATA_SAMP毎に0にセットするカウンター
volatile long datanum=0;				// データ番号	
volatile double r0;					// 台車の初期位置[V]
volatile double a_ref;					// 目標信号の振幅[m]
volatile double t=0.;					// 時間[s]
volatile double dt=1./SAMP_FREQ;			// サンプリング周期[s]
volatile double x_d=0.;					// 近似微分要素の状態
volatile double x_f=0.;					// 1次遅れ補償要素の状態
volatile double exp_data[5][1510];			// 実験データ
volatile double wn,Tn;					// 構造系の固有角振動数と固有周期
volatile double a0[5][5],b0[5],c0[3][5],d0[3],ko[5][3],ao[5][5],kd[5],xh[5];	// 構造系のオブザーバ変数
volatile double f0[5];					// 飽和制御のフィードバックゲイン

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 r,ur,ur1,dr,ur_v,mu,v;
	double phi;
	double ddtheta,f0x;
	double dxh[5],y[3];

	r = (double)sbox_AdGet(0)/CONV-r0;			// 台車位置[V]（初期値を0とする）
	phi = (double)sbox_AdGet(1)/CONV- 2.0226036;		// 加速度センサの電圧[V]（静止時を0とする）
	ddtheta=C_ddtheta*phi/H;				// 振れ角θの角加速度[rad/s/s]
	
	y[1]=C_r*r;						// 出力1（台車の位置[m]）
	y[2]=ddtheta;						// 出力2
	
	f0x=f0[1]*y[1]+f0[2]*xh[2]+f0[3]*xh[3]+f0[4]*xh[4];
	v = sat(f0x,a_ref);					// 飽和制御

	if(t<2.*Tn)
		v=0.03*sin(wn*t);				// 加振入力

	x_d+=(r-x_d)*dt/Td;					// rの近似微分drの計算
	dr=(r-x_d)/Td;

	mu = -1./(T*T)*y[1]-2./T*C_r*dr+v/(T*T);		// 制御入力mu
	ur = (T0_actual*mu+C_r*dr)/(C_r*K0_actual);		// 補償された駆動系への入力

	// オブザーバの計算
	dxh[1]=(ao[1][1]*xh[1]+ao[1][2]*xh[2]+ao[1][3]*xh[3]+ao[1][4]*xh[4]
			+ko[1][1]*y[1]+ko[1][2]*y[2]+(b0[1]-kd[1])*v)*dt;
	dxh[2]=(ao[2][1]*xh[1]+ao[2][2]*xh[2]+ao[2][3]*xh[3]+ao[2][4]*xh[4]
			+ko[2][1]*y[1]+ko[2][2]*y[2]+(b0[2]-kd[2])*v)*dt;
	dxh[3]=(ao[3][1]*xh[1]+ao[3][2]*xh[2]+ao[3][3]*xh[3]+ao[3][4]*xh[4]
			+ko[3][1]*y[1]+ko[3][2]*y[2]+(b0[3]-kd[3])*v)*dt;
	dxh[4]=(ao[4][1]*xh[1]+ao[4][2]*xh[2]+ao[4][3]*xh[3]+ao[4][4]*xh[4]
			+ko[4][1]*y[1]+ko[4][2]*y[2]+(b0[4]-kd[4])*v)*dt;
	xh[1]+=dxh[1];
	xh[2]+=dxh[2];
	xh[3]+=dxh[3];
	xh[4]+=dxh[4];

	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;
	}

	sbox_DaPut( 0, V_abs);					// 速度指令電圧をDA0チャンネルへ出力
	sbox_DaPut( 1, V_dir);					// 回転方向指令電圧をDA1チャンネルへ出力

	if(counter1==DATA_SAMP){				// 指定サンプリング毎に実験結果を記録
		if(datanum<DATA_NUM_MAX){
			exp_data[1][datanum]=r*C_r;		// 台車の位置[m]
			exp_data[2][datanum]=phi;		// 加速度センサ電圧[V]
			exp_data[3][datanum]=xh[3];		// 振れ角の推定値[rad]
			exp_data[4][datanum]=ur_v;		// ドライバへの速度指令電圧[V]
		}
		datanum++;
		counter1=0;
	}
	++counter1;
	t+=dt;							// 時間軸の計算
}

/*********************************************************************************
		main function（ここからプログラムが実行される）
**********************************************************************************/
void main()
{
	int i,j,k,cont_start;
	double a41,a42,wn2,b4;
	FILE *fp1;					// 実験データ出力用ファイル

	printf("Enter a_ref[m] ");			// 台車振幅の目標値
	scanf("%lf",&a_ref);
	if(fabs(a_ref)>0.1)
		a_ref=0.1;				// a_refの上限を0.1[m]とする．

	a41=m/((m+M)*H*T*T);				// 構造系の状態方程式パラメータ
	a42=2.*m/((m+M)*H*T);
	wn2=K/((m+M)*H*H);
	wn=sqrt(wn2);
	b4=-a41;
	
	Tn=2.*pi/wn;					// 固有周期
	
	// 構造系のオブザーバデータ
	for(i=1;i<=4;i++){
		b0[i]=0.;
		for(j=1;j<=2;j++)
			c0[j][i]=0.;
	}
	
	for(i=1;i<=4;i++)
		for(j=1;j<=4;j++)
			a0[i][j]=0.;
			
	a0[1][2]=1.;					// 構造系の状態方程式のA,B,C,D行列
	a0[2][1]=-1./T/T; a0[2][2]=-2./T;
	a0[3][4]=1.;
	a0[4][1]=a41; a0[4][2]=a42; a0[4][3]=-wn2;
	b0[2]=1./T/T;
	b0[4]=b4;
	c0[1][1]=1.;
	c0[2][1]=a41; c0[2][2]=a42; c0[2][3]=-wn2;
	d0[1]=0.;
	d0[2]=b4;
	
	ko[1][1]=1.4483161;  ko[1][2]=-0.0082885;	// オブザーバゲイン（すべての極を-10に配置）
	ko[2][1]=-14.483161; ko[2][2]=0.1095557;
	ko[3][1]=-65.609715; ko[3][2]=-0.7421575;
	ko[4][1]=-549.19019; ko[4][2]=-2.3212587;
 
	for(i=1;i<=4;i++)				// オブザーバの行列
		for(j=1;j<=4;j++){
			ao[i][j]=a0[i][j];
			for(k=1;k<=2;k++)
				ao[i][j]-=ko[i][k]*c0[k][j];
	}
	
	for(i=1;i<=4;i++){				// オブザーバの行列
		kd[i]=0.;
		for(j=1;j<=2;j++)
			kd[i]+=ko[i][j]*d0[j];
	}
	
	for(i=1;i<=4;i++)				// オブザーバの状態をすべて0にする。
		xh[i]=0.0;

	// 飽和制御のフィードバックゲイン T=0.1 zeta=0.7
	f0[1]=0.;
	f0[2]=-0.2838584;
	f0[3]=3.7721603;
	f0[4]=- 0.5867597;
	
	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]

	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");

	fp1=fopen("data1.txt","w");			// 記録した実験データのファイルへの書き込み
	for(i=0;i<DATA_NUM_MAX;i++)
		exp_data[0][i]=DATA_SAMP*dt*(i);	// 時間軸
	for(i=0;i<DATA_NUM_MAX;++i){
		fprintf(fp1,"%f %f %f %f %f\n",
		exp_data[0][i],exp_data[1][i],exp_data[2][i],exp_data[3][i],exp_data[4][i]); 
	}
	fclose(fp1);
	printf("end of writing\n");
}