//  swingup_sample.c		2009/08/23
#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 1601	// 実験データの記録長
#define DATA_SAMP 14		// DATA_SAMPサンプル毎に実験データを記録
#define Comp 1.85		// 駆動系の不感帯補償のためのオフセット電圧[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 1.		// 〃　ゲイン
#define C_r 0.019635		// 台車のポテンショメータの電圧[V]--変位[m]間の変換係数　pi*0.015[m]*10[rev]/24[V]
#define C_th 0.3054326 		// 振子のポテンショメータの電圧[V]--角度[rad]間の変換係数 350[degree]*pi/180/20[V]  [rad/V]
#define pi 3.1416		// 円周率
#define g 9.81			// 重力加速度[m/s/s]
#define wn 5.36			// 振子の固有角振動数[rad/s]（振子の自由振動から同定）
#define T_v 0.2			// エネルギVの1次遅れフィルタの時定数[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[5][1610];	// 実験データ
volatile double le;			// 等価振子長[m]
volatile double th0;			// 振子角の初期値[V]
volatile double r[3];			// 台車位置および差分のための遅延用変数
volatile double th[3];			// 振子角　〃
volatile double k;			// 飽和制御のパラメータ
volatile double s4[5];			// 〃
volatile double T;			// 補償後の台車系の時定数
volatile double phi_n;			// エネルギポンピングのための位相
volatile double vd;			// 倒立状態の振子の（正規化）エネルギ
volatile double e1,at;			// 制御則の切り換え基準
volatile double b0,b01;			// 〃　パラメータ
volatile double th0up;			// 倒立状態でのポテンショメータの電圧[V]
volatile double V=0.;			// 平滑化された振子のエネルギ
volatile int mode=2;			// 制御モード．1 = 安定化，2 = 振り上げ

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 rdot,thdot,x[5],s4x,thdash,V0,a1,phi;

	r[2]=r[1];				// 差分のための信号遅延
	r[1]=r[0];
	th[2]=th[1];
	th[1]=th[0];
		
	r[0] = (double)sbox_AdGet(0)/CONV-r0;		// 台車位置[V]（初期値を0とする）
	th[0]=-C_th*((double)sbox_AdGet(1)/CONV-th0);	// 振子角[rad]（垂下状態を0とする）
	thdash=-C_th*((double)sbox_AdGet(1)/CONV-th0up);// 安定化制御のための振子角[rad]（倒立状態を0とする）

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

	thdot=(th[0]-th[2])/(2.*dt);			// 2dt間の差分による近似微分
	rdot=(r[0]-r[2])/(2.*dt);
	
	x[1]=C_r*r[0];					// 状態
	x[2]=C_r*rdot;
	x[3]=C_r*r[0]+le*thdash;
	x[4]=C_r*rdot+le*thdot;

	V0=0.5*le*le*thdot*thdot+g*le*(1-cos(th[0]));	// 振子の（正規化）エネルギ
	V+=(V0-V)*dt/T_v;				// 1次遅れフィルタによるV0の平滑化

	s4x=s4[1]*x[1]+s4[2]*x[2]+s4[3]*x[3]+s4[4]*x[4];

	if(fabs(vd-V)<e1 && fabs(s4x)<at)
		mode=1;						// 安定化モード
	if(mode=1 && pi-fabs(th[0])>0.45)
		mode=2;						// 振り上げモード

	if(mode==2){
		if(fabs(vd-V)>=b01)
			a1=a*sgn(vd-V);
		else
			a1=a*(vd-V)/b01;

		phi=-atan2(thdot/wn,th[0]);
		if(t<15.*dt) phi=0.; 				// 振子の垂下状態から始めると初期のphiが決まらないため（台車を逆から動かしたい場合，phi=piとする）
		v=a1*sin(phi-phi_n);				// 振り上げ制御
	}
	else{
		v=-sat((wn+k)*s4x,a);				// 安定化のための飽和制御
	}

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

	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[0]*C_r;		// 台車の位置[m]
			exp_data[2][datanum]=rdot*C_r;		// 台車速度（差分近似）[m/s]
			exp_data[3][datanum]=th[0];		// 振子角[rad]
			exp_data[4][datanum]=V;			// 振子の（正規化）エネルギ
		}
		datanum++;
		counter1=0;
	}
	++counter1;
	t+=dt;							// 時間軸の計算

}

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

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

	printf("Enter c0[m] ");			// 台車系の時定数を決める設計パラメータ（c0=1.2とした）
	scanf("%lf",&c0);

	r[1]=0.;	r[0]=0.;		// 差分のための変数の初期化
	th[1]=0.;	th[0]=0.;

	le=g/(wn*wn);				// 等価振子長

	T=1./(c0*wn);

	s4[1]=T*(T*wn+2.);			// 座標変換行列Sの第4行
	s4[2]=T*T;
	s4[3]=-(T*wn+1.)*(T*wn+1.)/wn;
	s4[4]=s4[3]/wn;

	printf("Enter k>0 ");			// 設計パラメータ．-kは閉ループ極の一つとなる．（k=wnとした）
	scanf("%lf",&k);

	th0up=9.148;				// 倒立状態での振子用ポテンショメータの電圧[V]（これがずれていると倒立安定化時に台車位置が偏る）
	y1=2.*T*wn;
	y2=1.-T*T*wn*wn;
	phi_n=-atan2(y1,y2);			// エネルギポンピングのための位相
	vd=2.*g*le;				// 目標エネルギ
	e1=vd*0.1;				// 切り換え基準
	at=a/wn;				// 〃

	printf("Enter b0 ");			// 切り換えパラメータ．|vd-V|<b01 ならば台車の振幅を減少．（b0=0.085とした）
	scanf("%lf",&b0);
	b01=vd*b0;

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

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