#define SYS_CLOCK_24_576
#include <3048.h>

//▼▼▼プロトタイプ宣言

void reset(void);
void send_BYTE(unsigned char data);
void send_WORD(unsigned int data);
void PWM_L(unsigned int duty);
void PWM_R(unsigned int duty);
void AD_all(void);
void GetPosition(void);
void onIRLED(unsigned char index);
unsigned int getADdata(unsigned char index);
void onIRLED(unsigned char index);
unsigned char GetDipSW(void);
void Going(unsigned int GoingVelocity);
//▼▼▼グローバル変数宣言
unsigned char phase=0;		//状態番号記憶
unsigned char debug_cnt=0;	//debugカウンタ
unsigned int datas[20];
/*	0  AD0(right)			0-1024
	1  AD1				0-1024
	2  AD2				0-1024
	3  AD3				0-1024
	4  AD4				0-1024
	5  AD5(left)	予備		0-65536
	6  Position	中央線位置	0-2048 中央=1024 
	7  Differential	中央線位置微分	0-2048
	8  Velocity	目標速度	0-4056
	9  Speed	現在速度	0-4056
	10 PWM_R	右電動機	0-4056
	11 PWM_L	左電動機	0-4056
	12 		右輪回転数	0-65536
	13 		左輪回転数	0-65536
	14
	15 Batt		電池電圧
*/
unsigned int Position=1024;
unsigned int PPosition=1024;
unsigned int joint=1024;		//中心センサ値誤差補整初期値
unsigned int Differential=1024;
unsigned int PWMgeta;			//PWMの下駄
unsigned int Velocity;			//目標速度[mm/s]
unsigned int Speed;			//現在速度[mm/s]
unsigned long t=0;
unsigned char DT=5,PG=3,DG=8;	//■微分時間(t%DT==0) ■比例gain ■微分gain
#define white 380 		//■クロスLine,エラー,闘値
#define gray 330 		//■クランク
#define CrankVelocity 1024	//■クランク通過速度
signed int ControlRod;
unsigned int EncR;		//エンコーダ値記憶用
unsigned int EncL;
unsigned int tSpeed;		//速度検出専用時間変数
unsigned int EncRSpeed;		//速度検出専用エンコーダ値記憶変数
unsigned int PWMoffset=0;		//モータ特性補正値
//▼▼▼▼▼▼▼▼▼▼
//メイン
void main(void){
	reset();
	
	
	if(GetDipSW()==0x01){
		
	}
	

	while(1){
		
	}
}


//▼▼▼▼▼▼▼▼▼▼
// IRQ 割り込み


//▼▼▼▼▼▼▼▼▼▼
// ITU4 割り込み(コンペアマッチ/インプットキャプチャA4)
void int_imia4 (void){
	t++;
	AD_all();
	
	//▼▼▼ゲイン変更確認
	if(!P5.DR.BIT.B0){
		Velocity=1024+(unsigned int)(GetDipSW()*128);//1024-3072
	}
	if(!P5.DR.BIT.B1){
		PG=GetDipSW();
	}
	if(!P5.DR.BIT.B2){
		DG=GetDipSW();
	}
	if(!P5.DR.BIT.B3){
		DT=GetDipSW();
	}
	
	
	//▼【phase】に応じた処理及びphaseシフト
	switch(phase){
	case  0:	//▼▼▼初期加速
		if(Velocity<512){
			Velocity++;
		}
		phase++;
	case  1:	//▼▼▼通常走行
		//▼クロスLine検査
		if(datas[0]>white && datas[4]>white) phase++;//クロスLine闘値

		//▼通常走行制御るーちん
		Going(Velocity);
		break;
	case  2:	//▼▼▼クロスLine検出
		PA.DR.BIT.B6=0;			//ブレーキON
		PA.DR.BIT.B7=0;
		EncR=ITU2.TCNT;			//エンコーダ値を記憶
		EncL=ITU3.TCNT;			//エンコーダ値を記憶

		tSpeed=0;
		EncRSpeed=ITU2.TCNT;
		phase++;
	case  3:	//▼▼▼クロスLine通過待ち
		if(ITU2.TCNT-EncR>=6 && ITU3.TCNT-EncL>=6){
			phase++;
		}else{
			tSpeed++;
			if(ITU2.TCNT == EncRSpeed){
				if(tSpeed>10){ // 200[mm/s]→10[mm]に10/200[s]=50[ms]
			 		//50ms待ってもエンコーダがカウントされてない時
					PA.DR.BIT.B6=1;			//ブレーキoff
					PA.DR.BIT.B7=1;
					PWM_R(CrankVelocity);
					PWM_L(CrankVelocity);
					tSpeed=0;
				}
			}else{
		 		//エンコーダがカウントされてた時
				tSpeed=0;	
				EncRSpeed=ITU2.TCNT;
			}
		}
		break;
	case  4:	//▼▼▼クロスLine通過｜減速
		if(datas[0]>white && datas[4]>white){	//コースアウトでした
			phase=99;
		}else{					//クロスLineでした
			tSpeed=0;
			EncRSpeed=ITU2.TCNT;
			phase++;
		}
		break;
	case  5:	//▼▼▼減速(ブレーキon)｜低速走行(ブレーキ0ff)
		tSpeed++;
		if(ITU2.TCNT == EncRSpeed){
			if(tSpeed>5){
		 		//25ms待ってもエンコーダがカウントされてない時
				PA.DR.BIT.B6=1;			//ブレーキoff
				PA.DR.BIT.B7=1;
				tSpeed=0;
				phase++;
			}
		}else{
	 		//エンコーダがカウントされてた時
			tSpeed=0;
			EncRSpeed=ITU2.TCNT;
		}
		Going(0);					//走行制御るーちん
		break;
	case  6:	//▼▼▼低速走行制御
		Going(CrankVelocity);				//走行制御るーちん

		//▼クランク検出
		if(datas[0]>gray){	//右
			PA.DR.BIT.B6=0;			//右ブレーキ
			PWM_L(CrankVelocity);		//
			EncL=ITU3.TCNT;			//左エンコーダ値を記憶
			phase+=1;
		}
		if(datas[4]>gray){	//左
			phase+=3;
			PA.DR.BIT.B7=0;			//左ブレーキ
			PWM_R(CrankVelocity);			//
			EncR=ITU2.TCNT;			//右エンコーダ値を記憶
		}
		break;

	//▼▼▼クランク検出｜90度右旋回初頭
	case  7:	//▼▼▼右クランク検出｜90度右旋回初頭
		phase++;
	case  8:	//▼▼▼90度右旋回｜中央復帰
		if(	ITU3.TCNT-EncL>7 &&
			datas[2]>datas[0]+30 && 
			datas[2]>datas[1]+30 && 
			datas[2]>datas[3]+30 && 
			datas[2]>datas[4]+30	
			){
			phase=1;//通常走行
		}else if(
			ITU3.TCNT-EncL>9 &&
			datas[4]>white &&
			datas[2]>white
			){
			PA.DR.BIT.B7=0;			//左ブレーキ
			PA.DR.BIT.B6=1;			//右ブレーキ解除
			PWM_R(CrankVelocity);		//
			phase=11;
		}
		break;
	case  9:	//▼▼▼左クランク検出｜90度左旋回初頭
		phase++;
	case 10:	//▼▼▼90度左旋回｜中央復帰
		if(	ITU2.TCNT-EncR>7 &&
			datas[2]>datas[0]+30 && 
			datas[2]>datas[1]+30 && 
			datas[2]>datas[3]+30 && 
			datas[2]>datas[4]+30	
			){
			phase=1;//通常走行
		}else if(
			ITU2.TCNT-EncR>9 &&
			datas[1]>white &&
			datas[2]>white
			){
			PA.DR.BIT.B6=0;			//右ブレーキ
			PA.DR.BIT.B7=1;			//左ブレーキ解除
			PWM_L(CrankVelocity);		//
			phase++;
		}
		break;
	case  11:	//▼▼▼中央復帰
		if(	datas[2]>datas[0]+30 && 
			datas[2]>datas[1]+30 && 
			datas[2]>datas[3]+30 && 
			datas[2]>datas[4]+30	
			){
			PA.DR.BIT.B6=1;			//右ブレーキ解除
			PA.DR.BIT.B7=1;			//左ブレーキ解除

			phase=1;//通常走行
		}
	
		break;
	case 99:	//▼▼▼停止コード検出
		PA.DR.BIT.B6=0;
		PA.DR.BIT.B7=0;
		while(1);
		break;
	}
	
	P4.DR.BYTE++;		//■■テスト用
	datas[5]++;		//■■テスト用
	
	//▼内部データ垂れ流し
	datas[6] =Position;
	datas[7] =Differential;
	datas[8] =Velocity;
	datas[9] =Speed;
	datas[10]=ITU0.GRB;
	datas[11]=ITU1.GRB;
	datas[12]=ITU2.TCNT;
	datas[13]=ITU3.TCNT;
	if (debug_cnt<=15){
		send_WORD(datas[debug_cnt]);
		debug_cnt++;
	}else{
		debug_cnt=0;
		while(!SCI1.SSR.BIT.TDRE);
		SCI1.TDR=0x0D;			//改行コード
		SCI1.SSR.BIT.TDRE=0;
	}
}

//▼▼▼クランク中の姿勢制御
void Going(unsigned int GoingVelocity){
	if(t%DT==0){			//微分時間
		GetPosition();
		Differential=1024+Position-PPosition;
		//右右右右右右右右右右右PWM変更
		ControlRod=	((signed int)(Position-1024)
				+(signed int)(Differential-1024)*(signed int)DG
				)*(signed int)PG;
		
		if(ControlRod > 0){		//加速
			ControlRod = ControlRod/2 + (signed int)GoingVelocity;
		}else{				//減速
			ControlRod = ControlRod + (signed int)GoingVelocity;
		}
		if(ControlRod<=0){		//下限
			PA.DR.BIT.B6=0;			//右ブレーキ設定
			ControlRod=1;	
		}else{PA.DR.BIT.B6=1;}			//右ブレーキ解除
		if(ControlRod>=4056){		//上限
			ControlRod=4055;
		}
		PWM_R((unsigned int)ControlRod);
		
		//左左左左左左左左左左左PWM変更
		ControlRod=	((signed int)(Position-1024)
				+(signed int)(Differential-1024)*(signed int)DG
				)*(signed int)PG;
		
		if(ControlRod > 0){		//加速
			ControlRod = -ControlRod/2 + (signed int)GoingVelocity;
		}else{				//減速
			ControlRod = -ControlRod + (signed int)GoingVelocity;
		}
		if(ControlRod<=0){
			PA.DR.BIT.B7=0;			//左ブレーキ
			ControlRod=1;
		}else{PA.DR.BIT.B7=1;}			//左ブレーキ解除
		if(ControlRod>=4056) ControlRod=4055;	//上限
		PWM_L((unsigned int)ControlRod);
	}
}
//▼▼▼PWMduty変更(右:FDIIOC)
void PWM_R(unsigned int duty){
	ITU0.GRB=duty;
}
//▼▼▼PWMduty変更(左:FDIIOB)
void PWM_L(unsigned int duty){
	ITU1.GRB=duty;
}

//▼▼▼▼▼▼▼▼▼▼
//RS232 送信
void send_BYTE(unsigned char data){
	while (!SCI1.SSR.BIT.TDRE);
	SCI1.TDR=data & 0xF0;		//上位4bit
	SCI1.SSR.BIT.TDRE=0;
	while (!SCI1.SSR.BIT.TDRE); 
	SCI1.TDR=(data<<4) & 0xF0;	//下位4bit
	SCI1.SSR.BIT.TDRE=0;
	}
void send_WORD(unsigned int data){
	while (!SCI1.SSR.BIT.TDRE);
	SCI1.TDR=(char)((data>>12)&0x000F)+0x30;	//最上位4bit
	SCI1.SSR.BIT.TDRE=0;
	while (!SCI1.SSR.BIT.TDRE);
	SCI1.TDR=(char)((data>>8)&0x000F)+0x30;	//
	SCI1.SSR.BIT.TDRE=0;
	while (!SCI1.SSR.BIT.TDRE);
	SCI1.TDR=(char)((data>>4)&0x000F)+0x30;	//
	SCI1.SSR.BIT.TDRE=0;
	while (!SCI1.SSR.BIT.TDRE);
	SCI1.TDR=(char)((data)&0x000F)+0x30;	//最下位4bit
	SCI1.SSR.BIT.TDRE=0;
}

//▼▼▼▼▼▼▼▼▼▼
//AD関係
void AD_all(void){			//AD変換
	volatile static unsigned char j;
	volatile unsigned char i;
	for (i=0;i<=4;i++){
		AD.ADCSR.BIT.CH=i;	//ADのチャンネル
		onIRLED(i);		//LED on
		AD.ADCSR.BIT.ADST=1;		//だみーへんかん
		while(!AD.ADCSR.BIT.ADF);	//だみーへんかん
		AD.ADCSR.BIT.ADF=0;		//だみーへんかん
		for (j=0;j<255;j++) asm("nop");
		AD.ADCSR.BIT.ADST=1;
		while(!AD.ADCSR.BIT.ADF);
		PB.DR.BYTE = 0x80;	//LED全off
		AD.ADCSR.BIT.ADF=0;
		datas[i]=getADdata(i)>>6;
	}

	//▼▼▼バッテリーの電圧
	AD.ADCSR.BIT.CH=7;
	AD.ADCSR.BIT.ADST=1;
	while(!AD.ADCSR.BIT.ADF);
	AD.ADCSR.BIT.ADF=0;
	datas[15]=getADdata(7)>>6;
}
unsigned int getADdata(unsigned char index){
	if(index == 0) return AD.ADDRA;
	if(index == 1) return AD.ADDRB;
	if(index == 2) return AD.ADDRC;
	if(index == 3) return AD.ADDRD;
	if(index == 4) return AD.ADDRA;
	if(index == 5) return AD.ADDRB;
	if(index == 6) return AD.ADDRC;
	if(index == 7) return AD.ADDRD;
	return 0;
}
void onIRLED(unsigned char index){
	if(index == 0) PB.DR.BIT.B0=1;
	if(index == 1) PB.DR.BIT.B1=1;
	if(index == 2) PB.DR.BIT.B2=1;
	if(index == 3) PB.DR.BIT.B3=1;
	if(index == 4) PB.DR.BIT.B4=1;
	if(index == 5) PB.DR.BIT.B5=1;
}

//▼▼▼▼▼▼▼▼▼▼
//マイコンボード上DIPSW値取得
unsigned char GetDipSW(){
	return P6.DR.BYTE & 0x0F;
}

//▼▼▼▼▼▼▼▼▼▼
//センサの値【datas[]】から【Position】にライン位置を得る
void GetPosition(void){
	//unsigned char best1,best2;
	//0┐		左過ぎ0────1024───2048右すぎ					
	//1┴┐	
	//2┐├┐   		左検知器 4 3 2 1 0 右検知器
	//3┴┘├トーナメント
	//4──┘
	/*
	if(datas[    0]>datas[    1])	best1=0;	else	best1=1;
	if(datas[    2]>datas[    3])	best2=2;	else	best2=3;
	if(datas[best1]>datas[best2]);			else	best1=best2;
	if(datas[best1]>datas[    4]);			else	best1=4;	
	
	if(best1==0 || best1==1) Position=2048;return;
	if(best1==3 || best1==4) Position=2048;return;
	Position=1024+datas[3]-datas[1];
	*/
	PPosition=Position;
	if(datas[1]>datas[2]){
		if(datas[1]>datas[3]){		//右センサが最大の時
			if(joint==1024-45) joint=Position+datas[0]-datas[2]+Differential-1024;
			Position=joint+datas[2]-datas[0];
		}
	}else{
		if(datas[2]>datas[3]){		//中央センサが最大のとき
			joint=1024-45;
			Position=joint+datas[3]-datas[1];
		}else{				//左センサが最大のとき
			if(joint==1024-45) joint=Position+datas[2]-datas[4]+Differential-1024;
			Position=joint+datas[4]-datas[2];
		}
	}
}
//▼▼▼▼▼▼▼▼▼▼
//各種初期化
void reset(void){
	unsigned int i;
	//▼IO初期化
	P4.DDR=0xFF;
	P4.DR.BYTE=0x00;
	//P6.DDR=0x00;		//でふぉると
	//P6.DR.BYTE=0x00;	//でふぉると
	//P5.DDR=0x00;		//でふぉると
	//P5.DR.BYTE=0x00;	//でふぉると
	P5.PCR.BYTE=0x07;	//プルアップ
	PA.DDR=0xFC;
	PA.DR.BYTE=0xE0;
	PB.DDR=0xFF;
	PB.DR.BYTE=0x00;
	
	//▼PWM初期化
	ITU.TMDR.BIT.PWM0=1;//▼右
	ITU0.TCR.BIT.CCLR=1;	//GRAとマッチでクリア
	ITU0.GRA=4096;		//■PWM最大値
	ITU0.GRB=1;
	ITU.TSTR.BIT.STR0=1;
	ITU.TMDR.BIT.PWM1=1;//▼左
	ITU1.TCR.BIT.CCLR=1;	//GRAとマッチでクリア
	ITU1.GRA=4096;		//■PWM最大値
	ITU1.GRB=1;
	ITU.TSTR.BIT.STR1=1;

	//▼エンコーダ初期化
	ITU2.TCR.BIT.TPSC=4;	//TCLKAをカウント//▼右
	ITU2.TCR.BIT.CKEG=0;	//立ち上エッヂカウント
	ITU.TSTR.BIT.STR2=1;	//カウントスタート
	
	ITU3.TCR.BIT.TPSC=5;	//TCLKBをカウント//▼左
	ITU3.TCR.BIT.CKEG=0;	//立ち上エッヂカウント
	ITU.TSTR.BIT.STR3=1;	//カウントスタート

	PB.DR.BIT.B7=1;		//両エンコーダ動作許可

	//▼ITU4インターバルタイマ初期化
	ITU4.GRA=15360;		//■周期
	ITU4.TCR.BIT.CCLR=1;	//GRAとマッチでクリア
	ITU4.TCR.BIT.TPSC=3;	//プリスケーラ 0=CK,1=CK/2,2=CK/4,3=CK/8
	ITU4.TIER.BIT.IMIEA=1;	//GRAマッチで割り込み発生
	ITU.TSTR.BIT.STR4=1;	//カウントスタート

	//▼AD初期化
	AD.ADCSR.BIT.SCAN=0;	//単一モード
	AD.ADCSR.BIT.CKS=0;	//じっくり変換
//	AD.ADCSR.BIT.CH=;	//チャンネル選択
//	AD.ADCSR.ADST=1;	//変換開始
//	while(AD.ADCSR.ADF);	//変換完了待ち
//	AD.ADCSR.ADDRA

	//▼SCI1初期化
	SCI1.SCR.BIT.TE=0;		//
	SCI1.SCR.BIT.RE=0;		//
	SCI1.SMR.BIT.CKS=0;
	SCI1.BRR=18;		//■18=38400bps 38=19200bps
	for (i=0;i<=40000;i++);
	SCI1.SCR.BIT.TE=1;		//送信許可
	EI;
}