//  pid_sample.c		2009/08/01
#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 100.		// サンプリング周波数[Hz]
#define DATA_NUM_MAX 401	// 実験データの記録長
#define DATA_SAMP 10		// DATA_SAMPサンプル毎に実験データを記録
#define DELAY 400		// むだ時間 L=DELAY/SAMP_FREQ[s]
#define EPS 0.1			// 近似微分要素のパラメータε

//	大域変数の定義．割り込み関数内で再利用する変数はvolatile宣言する．
volatile int counter1=DATA_SAMP;		// DATA_SAMP毎に0にセットするカウンター
volatile long datanum=0;			// データ番号	
volatile double t=0.;				// 時間[s]
volatile double dt=1./SAMP_FREQ;		// サンプリング周期[s]
volatile double x_d=0.;				// 近似微分要素の状態
volatile double exp_data[5][1510];		// 実験データ
volatile double uz[2001];			// 制御入力遅延用配列
volatile double kappa_P,tau_I,tau_D;		// PID補償器のパラメータ
volatile double ref;				// 目標入力[V]
volatile double Td;				// 近似微分器の時定数
volatile double u2=0.;				// 積分器の状態
volatile double vo_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 i,Vin;				// ローカル変数の定義
	double vo,vi,u,e,e1,de1,u1,u3;

	vo = (double)sbox_AdGet(0)/CONV-vo_0;	// プラントの出力[V]（初期値を0とする）
	e=ref-vo;				// 偏差
	e1=-vo;					// 出力フィードバック

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

	u1=e;					// 比例
	u2+=e*dt/tau_I;				// 積分
	u3=tau_D*de1;				// 微分（目標値の微分を含まない）
	u=sat(kappa_P*(u1+u2+u3),8.0);		// 制御入力（8Vまで）

	for (i=0;i<=DELAY;i++)			// uの遅延（むだ時間 L=DELAY*dt[s]）
		uz[DELAY+1-i]=uz[DELAY-i];
	uz[0]=u;
	vi=uz[DELAY+1];				// プラント入力

	Vin=(int)(vi*CONV);			// viのDA出力への変換
	if(datanum>=DATA_NUM_MAX)		// 終了処理
		Vin=0;

	sbox_DaPut( 0, Vin);			// VinをDA0チャンネルに出力

					
	if(counter1==DATA_SAMP){		// 指定サンプリング毎に実験結果を記録
		if(datanum<DATA_NUM_MAX){
			exp_data[1][datanum]=u;		// 制御入力[V]
			exp_data[2][datanum]=vo;	// プラントの出力[V]
		}
		datanum++;
		counter1=0;
	}
	++counter1;
	t+=dt;					// 時間軸の計算
}

/*********************************************************************************
		main function
**********************************************************************************/
void main()
{
	int i,cont_start;
	FILE *fp1;				// 実験データ出力用ファイル

	printf("Enter ref (0<r<5):");		// 目標入力
	scanf("%lf",&ref);

	if(ref > 5.)				// 目標入力の制限（回路保護のため）
		ref=5.;
	if(ref < 0.)
		ref=0.;

	for (i=0;i<=DELAY;i++)			// 制御入力遅延用配列の初期化
		uz[i]=0.;

	// PID補償器のパラメータ（シミュレーションで求める）
	kappa_P= 0.9482511;				// 比例ゲイン
	tau_I=6.5063676;				// 積分時間
	tau_D=1.2346955;				// 微分時間

	Td=EPS*tau_D;				// 近似微分要素の時定数
	if(Td<5.*dt)				// サンプリング周期を考慮してTdを制限
		Td=5.*dt;

	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変換終了まで待つ．

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