//h8 tiny 3664F
#include"3664.h"

//▼▼▼プロトタイプ宣言
void servo_out(char,char);
void servo_driveA(void);
void servo_driveB(void);
void servo_driveC(void);
void servo_driveD(void);

//▼▼▼変数宣言
struct{
  int gain;	//
  signed int offset;	//
  signed int bias;	//-512 to +512
}servo[24]={		//▼▼サーボの個性
  {	1,	0,	0},
  {	1,	0,	0},
  {	1,	0,	0},
  {	1,	0,	0},

  {	1,	0,	0},
  {	1,	0,	0},
  {	1,	0,	0},
  {	1,	0,	0},

  {	1,	0,	1000},
  {	1,	0,	0},
  {	1,	0,	0},
  {	1,	0,	0},

  {	1,	0,	0},
  {	1,	0,	0},
  {	1,	0,	0},
  {	1,	0,	0},

  {	1,	0,	2000},
  {	1,	0,	0},
  {	1,	0,	0},
  {	1,	0,	0},

  {	1,	0,	0},
  {	1,	0,	0},
  {	1,	0,	0},
  {	1,	0,	0}
};
unsigned char servo_cnt=0;

int main(void){
  
  //▼▼▼IO設定
  IO.PCR1=0xf7;	// 1111 0111
  IO.PCR2=0x05;	// 0000 0101
  IO.PCR5=0x3f;	// 0011 1111
  IO.PCR7=0x70;	// 0111 0000
  IO.PCR8=0xff;	// 1111 1111
  //▼▼▼割り込みタイマ設定
  TW.TIERW.BIT.IMIEA=1;	//コンペアマッチ割り込み
  TW.TIERW.BIT.IMIEB=1;	//コンペアマッチ割り込み
  TW.TIERW.BIT.IMIEC=1;	//コンペアマッチ割り込み
  TW.TIERW.BIT.IMIED=1;	//コンペアマッチ割り込み
  TW.TCRW.BIT.CKS=0;	//16MHz / 1 = 16MHzでカウント
  TW.GRB=1000;		//すぐに発生させる。
  TW.GRC=1000;		//すぐに発生させる。
  TW.GRD=1000;		//すぐに発生させる。
  TW.GRA=40000;		// TCNTW の 1[cnt] は 1/16M = 0.0625[us]
			// なので 2.5[ms] は 2500/0.0625 = 40000[cnt]
  TW.TCRW.BIT.CCLR=1;	//コンペアマッチAでクリア
  TW.TMRW.BIT.CTS=1;	//タイマカウンタW始動
  EI;			//割り込み許可
  
  while(1);
  return 0;
}

void int_timerw(void){
  if(TW.TSRW.BIT.IMFA){
    servo_driveA();
    TW.TSRW.BIT.IMFA=0;		//割り込み後続
  }
  if(TW.TSRW.BIT.IMFB){
    servo_out(servo_cnt,0);	//立ち下げ
    TW.TSRW.BIT.IMFB=0;		//割り込み後続
  }
  if(TW.TSRW.BIT.IMFC){
    servo_out(servo_cnt+8,0);	//立ち下げ
    TW.TSRW.BIT.IMFC=0;		//割り込み後続
  }
  if(TW.TSRW.BIT.IMFD){
    servo_out(servo_cnt+16,0);	//立ち下げ
    TW.TSRW.BIT.IMFD=0;		//割り込み後続
  }
}

//▼▼▼RCサーボ用PWM生成
void servo_driveA(void){
  servo_cnt++;
  if(servo_cnt>=8) servo_cnt=0;
  char servo_ID=servo_cnt;
  servo_out(servo_ID,1);	//立ち上げ
  TW.GRB= 24000 + servo[servo_ID].offset
	+(servo[servo_ID].gain*servo[servo_ID].bias);
  servo_ID+=8;
  servo_out(servo_ID,1);	//立ち上げ
  TW.GRC= 24000 + servo[servo_ID].offset
	+(servo[servo_ID].gain*servo[servo_ID].bias);
  servo_ID+=8;
  servo_out(servo_ID,1);	//立ち上げ
  TW.GRD= 24000 + servo[servo_ID].offset
	+(servo[servo_ID].gain*servo[servo_ID].bias);
		// TCNTW の 1[cnt] は 1/16M = 0.0625[us]
		// なので 1.5[ms] は 1500/0.0625 = 24000[cnt]
}
void servo_out(char index,char out){
  switch(index){		//サーボが繋がってる足
    case 0:IO.PDR8.BIT.B0=out;break;
    case 1:IO.PDR8.BIT.B1=out;break;
    case 2:IO.PDR8.BIT.B2=out;break;
    case 3:IO.PDR8.BIT.B3=out;break;
    case 4:IO.PDR8.BIT.B4=out;break;
    case 5:IO.PDR8.BIT.B5=out;break;
    case 6:IO.PDR8.BIT.B6=out;break;
    case 7:IO.PDR8.BIT.B7=out;break;

    case 8:IO.PDR1.BIT.B7=out;break;
    case 9:IO.PDR1.BIT.B6=out;break;
    case 10:IO.PDR1.BIT.B5=out;break;
    case 11:IO.PDR1.BIT.B4=out;break;
    case 12:IO.PDR2.BIT.B0=out;break;
    case 13:IO.PDR7.BIT.B6=out;break;
    case 14:IO.PDR7.BIT.B5=out;break;
    case 15:IO.PDR7.BIT.B4=out;break;

    case 16:IO.PDR5.BIT.B0=out;break;
    case 17:IO.PDR5.BIT.B1=out;break;
    case 18:IO.PDR5.BIT.B2=out;break;
    case 19:IO.PDR5.BIT.B3=out;break;
    case 20:IO.PDR5.BIT.B4=out;break;
    case 21:IO.PDR5.BIT.B5=out;break;
    case 22:IO.PDR1.BIT.B0=out;break;
    case 23:IO.PDR1.BIT.B1=out;break;
  }
}