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

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

//▼▼▼変数宣言
struct{
  unsigned int gain;	//
  signed int offset;	//
  signed int bias;	//-512 to +512
}servo[8]={
  {1,0,0},
  {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;
  //bit0   =位相(1=H 0=L)
  //bit1-7 =サーボID

int main(void){
  
  //▼▼▼IO設定
  IO.PCR8=0xff;
  //▼▼▼割り込みタイマ設定
  TW.TIERW.BIT.IMIEA=1;	//コンペアマッチ割り込み
  TW.TCRW.BIT.CKS=0;	//16MHz / 1 = 16MHzでカウント
  TW.GRA=1000;		//すぐに発生させる。
  TW.TCRW.BIT.CCLR=1;	//コンペアマッチでクリア
  TW.TMRW.BIT.CTS=1;	//タイマカウンタW始動
  EI;			//割り込み許可
  
  while(1);
  return 0;
}

void int_timerw(void){
  char servo_ID;
  servo_ID=servo_cnt>>1;
  if(servo_cnt & 0x01){
    servo_out(servo_ID,0);
		// TCNTW の 1[cnt] は 1/16M = 0.0625[us]
		// なので 2.5[ms] は 2500/0.0625 = 40000[cnt]
    TW.GRA= 40000-TW.GRA;
  }else{
    servo_out(servo_ID,1);
		// TCNTW の 1[cnt] は 1/16M = 0.0625[us]
		// なので 1.5[ms] は 1500/0.0625 = 24000[cnt]
    TW.GRA= 24000 + servo[servo_ID].offset
	+(servo[servo_ID].gain*servo[servo_ID].bias);
  }
  servo_cnt++;
  if(servo_cnt>16) servo_cnt=0;
  if(TW.TSRW.BIT.IMFA) TW.TSRW.BIT.IMFA=0;		//割り込み後続
}

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