//  ipend03_sample.c		2011/3/8
//  1重倒立振子の安定化制御（プーリーベルト駆動，レール長0.3m）
#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 1001	// 実験データの記録長
#define DATA_SAMP 60		// DATA_SAMPサンプル毎に実験データを記録
#define Comp 2.1		// 駆動系の不感帯補償のためのオフセット電圧[V]
#define Td 0.01			// 近似微分器の時定数[s]
#define Fv 1.5			// 駆動系の速度フィードバック補償のゲイン
#define Ka 1.			// Pa(s)のゲイン
#define C_r 0.0161731		// 台車のポテンショメータの電圧[V]--変位[m]間の変換係数　pi*0.03438[m]*60/16*345/360/24[V]
#define C_th 1.388		// 振子のポテンショメータの電圧[V]--角度[rad]間の変換係数 実測値[rad/V]
#define C_rdot 0.018		// タコジェネレータ電圧の変換係数[m/s/V]
#define pi 3.1416		// 円周率
#define g 9.80665		// 重力加速度[m/s/s]

//	大域変数の定義．割り込み関数内で再利用する変数はvolatile宣言する．
volatile int counter1=0;		// DATA_SAMP毎に0にセットするカウンター
volatile long datanum=0;		// データ番号	
volatile double r0;			// 台車の初期値[V]
volatile double a;			// 台車の振幅制限[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[6][1610];	// 実験データ
volatile double th0up1;			// 振子角1の初期値（倒立状態）[V]
volatile double r;			// 台車位置
volatile double th1;			// 振子角1
volatile double ks;			// 飽和制御のパラメータ
volatile double f[5];			// 〃
volatile double T;			// 補償後の台車系の時定数
volatile double a0[5][5],b0[5],c0[3][5],ko[5][3],ao[5][5],xh[5];	// オブザーバ用変数
volatile int cart_limit=0;


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,i,j,k;			// ローカル変数の定義
	double ur1,dr,ur_v,v,rdot;
	double s4x;
	double dxh[5],y[3];

	r = (double)sbox_AdGet(0)/CONV-r0;			// 台車位置[V]（初期値を0とする）
	th1 = C_th*((double)sbox_AdGet(1)/CONV-th0up1);		// 振子角1[rad]（倒立状態を0とする）
	rdot=-C_rdot*(double)sbox_AdGet(2)/CONV;		// タコジェネレータによる台車速度[m/s]
		
	y[1]=C_r*r;
	y[2]=th1;

	// 停止条件
	if(fabs(r+r0)>4.0)				// 台車は許容範囲外か
		cart_limit=1;

	x_d+=(r-x_d)*dt/Td;				// rの近似微分drの計算（使用していない）
	dr=(r-x_d)/Td;

	s4x=f[1]*y[1]+f[2]*xh[2]+f[3]*y[2]+f[4]*xh[4];
	v=-sat(ks*s4x,a);					// 飽和制御

	// オブザーバの計算
	for(i=1;i<=4;i++){
		dxh[i]=b0[i]*v;
		for(k=1;k<=2;k++)
			dxh[i]+=ko[i][k]*y[k];
		for(j=1;j<=4;j++)
			dxh[i]+=ao[i][j]*xh[j];
	}
	for(i=1;i<=4;i++)
		xh[i]+=dxh[i]*dt;

	x_f+=((v/C_r-r)/(Ka*T*T)-2.*x_f/T)*dt;			// 駆動系の補償器C(s)

	//ur1=Fv*(x_f-dr);					// 駆動系の速度フィードバック補償（近似微分）
	ur1=Fv*(x_f-rdot/C_r);					// 駆動系の速度フィードバック補償（タコジェネレータ）

	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チャンネルへ出力

	if(counter1==DATA_SAMP){				// 指定サンプリング毎に実験結果を記録
		if(datanum<DATA_NUM_MAX){
			exp_data[1][datanum]=C_r*r;		// 台車位置[m]
			exp_data[2][datanum]=dr*C_r;		// 台車速度[m/s]（近似微分）
			exp_data[3][datanum]=rdot;		// 台車速度[m/s]（タコジェネレータ）
			exp_data[4][datanum]=xh[2];		// 台車速度の推定値[m/s]
			exp_data[5][datanum]=ur_v;		// ドライバへの速度指令電圧[V]
		}
		datanum++;
		counter1=0;
	}
	++counter1;
	t+=dt;							// 時間軸の計算

}

/*********************************************************************************
		main function（ここからプログラムが実行される）
**********************************************************************************/
void main()
{
	int i,j,k,cont_start;			// ローカル変数の定義
	FILE *fp1,*fr1;				// 実験データ出力用ファイル

	printf("Enter a[m] ");			// 台車の振幅制限．（例 a=0.1）
	scanf("%lf",&a);
	if(fabs(a)>0.1)
		a=0.1;				// aの上限を0.1[m]とする．

	T=0.15;					// 補償後の台車系の時定数[s]

	// オブザーバデータ
	for(i=1;i<=4;i++)
		for(j=1;j<=2;j++)
			c0[j][i]=0.;
	c0[1][1]=1.;
	c0[2][3]=1.;
	
	fr1=fopen("C:\\CCStudio_v3.1\\MyProjects\\obs_data.txt","r");	// ファイルからの読み込み
	if (fr1 == NULL)
		printf("can't open file\n");

	for(i=1;i<=4;i++)
		for(j=1;j<=4;j++)
			fscanf(fr1," %lf ",&a0[i][j]);

	for(i=1;i<=4;i++)
		fscanf(fr1," %lf ",&b0[i]);

	for(i=1;i<=4;i++)
		for(j=1;j<=2;j++)
			fscanf(fr1," %lf ",&ko[i][j]);

	for(i=1;i<=4;i++)
		fscanf(fr1," %lf ",&f[i]);
	fclose(fr1);
	
	printf("Matrix A\n");
	for(i=1;i<=4;i++){
		for(j=1;j<=4;j++)
			printf("%f  ",a0[i][j]);
		printf("\n");
	}

	printf("Matrix B\n");
	for(i=1;i<=4;i++)
		printf("%f\n",b0[i]);

	printf("Matrix K\n");
	for(i=1;i<=4;i++){
		for(j=1;j<=2;j++)
			printf("%f  ",ko[i][j]);
		printf("\n");
	}
	// フィードバックゲイン
	printf("Matrix F\n");
	for(i=1;i<=4;i++)
		printf("%f  ",f[i]);
	printf("\n");
	
 
	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++)			// オブザーバの状態をすべて0にする。
		xh[i]=0.0;

	printf("Enter ks>0 ");			// 飽和制御の設計パラメータ
	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]
	
	//th0up1=(double)sbox_AdGet(1)/CONV;
	th0up1 = 2.484207;			// 倒立状態の振子用ポテンショメータの電圧[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("th0up1 = %f\n",th0up1);

	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 %f\n",
		exp_data[0][i],exp_data[1][i],exp_data[2][i],exp_data[3][i],exp_data[4][i],exp_data[5][i]); 
	}
	fclose(fp1);
	printf("end of writing\n");
}