#include "iodefine.h"

#include "intrinsics.h"

#include <pwm_7125.h>
#include <sci_7125.h>
#include <ad_7125.h>

#define SRAM1 (void*)0xFFFFA100 // 0xFFFFA000～xFFFFBFFFまでの8Kバイト
#define SRAM2 (void*)0xFFFFAA00
#define SRAM3 (void*)0xFFFFB500
//5secデータで3変数が限界

#ifdef CPPAPP
//Initialize global constructors
extern "C" void __main()
{
  static int initialized;
  if (! initialized)
    {
      typedef void (*pfunc) ();
      extern pfunc __ctors[];
      extern pfunc __ctors_end[];
      pfunc *p;

      initialized = 1;
      for (p = __ctors_end; p > __ctors; )
    (*--p) ();

    }
}
#endif 

unsigned int cnt; // while ループ内のインクリメント用変数はグローバル宣言

void wait() // 時間稼ぎ
{
  int iLoop;
  //for( iLoop=0; iLoop<0x1000; iLoop++ ); // 2msec
  for( iLoop=0; iLoop<0x4000; iLoop++ ); // 8msec
}

int main(void)
{
	int i,loop;
	char strBuf[22],temp[14];
	long pulseCnt,input;
	float deg;
	unsigned int *anglebuf,*ADbuf;
	int *currentbuf;
	unsigned int ADtemp;

	anglebuf=SRAM1;
	ADbuf=SRAM2;
	currentbuf=SRAM3;

	PWM0_initMode1();       // PWM0をモード1で初期化
	AD0_TRG_init();
	PWM3_initADTRG();
	SCI1_init();
	PWM_initPulseCountMode(); // パルスカウント初期化

	PFC.PEIORL.BIT.B1 = 1;
	PFC.PEIORL.BIT.B13 = 1; // E Port B13を出力に設定
	PFC.PEIORL.BIT.B15 = 1; // E Port B15を出力に設定

	PE.DRL.BIT.B13 = 1;     // 正転 B13:H(IN1) B15:L(IN2)
	PE.DRL.BIT.B15 = 0;

	while(PB.DR.BIT.B1){ // PB.DR.BIT.B1
		PE.DRL.BIT.B1 ^= 1;

		pulseCnt=MTU21.TCNT;    // パルス計数
		deg=(float)pulseCnt/4;  // 4逓倍だから
		*anglebuf = deg*100;          // 小数点第2位を拾う

		*ADbuf=(ADDataBuf[1]>>6); // データ保存用unsigned int(16bit)

		// 5点平滑化
		if(cnt>3){
			ADtemp=(*ADbuf+(*(ADbuf-1))+(*(ADbuf-2))+(*(ADbuf-3))+(*(ADbuf-4)))/5;
		}
		else ADtemp=*ADbuf;

		// 電流値計算
		// floatではなく16bit整数値にしてメモリ節約
		*currentbuf= (ADtemp/1023.0*5-2.5)/2.5*125.0/30 * 1000;
		// 小数点以下3桁取得のため1000倍

		input=0.0*(360.0-deg); // 回転方向制御を行っていないので360degを過ぎてしまうと
							 // 戻れないので，これは定常偏差が残るアルゴリズム
		///////////////////////////
		// 試験で変更必要なのはここだけ // 上のゲインも変更（増加），inputにlong型が必要
		///////////////////////////
		if(input<0){
			PE.DRL.BIT.B13 = 0;     // 正転 B13:H(IN1) B15:L(IN2)
			PE.DRL.BIT.B15 = 1;
			input = -input;
		}
		else{
			PE.DRL.BIT.B13 = 1;     // 正転 B13:H(IN1) B15:L(IN2)
			PE.DRL.BIT.B15 = 0;
		}
		///////////////////////////
		///////////////////////////

		if(input>49999) MTU20.TGRB=49999; // この条件は常に必要
		else MTU20.TGRB=input;

		anglebuf++,ADbuf++;
		currentbuf++;
		cnt++;

		wait(); // 約2msec時間稼ぎ
	}
	MTU20.TGRB=0;
	PE.DRL.BIT.B1 =1;

	anglebuf=SRAM1;
	ADbuf=SRAM2;
	currentbuf=SRAM3;

	for(i=0;i<cnt;i++){

		/// モータ回転角度　保存
	    for( loop=5; loop>=0; loop-- ) // 桁位置を示すループ（小数点第2位～1000の位）
	    {
	      temp[loop] = *anglebuf%10;    // 各桁の値を抽出
	       *anglebuf/= 10;               // 次の桁へ移動
	    }

	    /// モータ電流のAD変換値（電圧）
	    for( loop=3; loop>=0; loop-- ) // 桁位置を示すループ（小数点第2位～1000の位）
	    {
	    	temp[loop+6] = *ADbuf%10;    // 各桁の値を抽出
	    	*ADbuf/= 10;               // 次の桁へ移動
	    }

	    /// モータ電流のAD変換値（電流変換）
	    if(*currentbuf<0){
	    	*currentbuf = -(*currentbuf);
	    	strBuf[13] = '-';
	    }
	    else strBuf[13] = ' ';

	    for( loop=3; loop>=0; loop-- ) // 桁位置を示すループ（小数点第2位～1000の位）
	    {
	       	temp[loop+10] = *currentbuf%10;    // 各桁の値を抽出
	       	*currentbuf/= 10;               // 次の桁へ移動
	    }

	    // 以下では数値を文字に変換して格納
	    // モータ回転角度
	    strBuf[0] = temp[0] + '0';  // 1000の位の値
	    strBuf[1] = temp[1] + '0';  // 100の位の値
	    strBuf[2] = temp[2] + '0';  // 10の位の値
	    strBuf[3] = temp[3] + '0';  // 1の位の値
	    strBuf[4] = '.';            // 小数点を格納
	    strBuf[5] = temp[4] + '0';  // 小数点第1位の値
	    strBuf[6] = temp[5] + '0';  // 小数点第2位の値
	    strBuf[7] = ' ';
	    // 電流AD変換値（電圧）
	    strBuf[8] = temp[6] + '0';  // 1000の位の値
	    strBuf[9] = temp[7] + '0';  // 100の位の値
	    strBuf[10] = temp[8] + '0';  // 10の位の値
	    strBuf[11] = temp[9] + '0';  // 1の位の値
	    strBuf[12] = ' ';
	    // 注意 strBuf[13]は上で判定
	    // 電流AD変換値（電流）
	    strBuf[14] = temp[10] + '0';  // 1000の位の値
	    strBuf[15] = '.';
	    strBuf[16] = temp[11] + '0';  // 100の位の値
	    strBuf[17] = temp[12] + '0';  // 10の位の値
	    strBuf[18] = temp[13] + '0';  // 1の位の値
	    strBuf[19] = '\r';           // 改行記号1を格納
	    strBuf[20] = '\n';           // 改行記号2を格納
	    strBuf[21] = '\0';           // 文字列の最終文字（null文字）

	    sendStringSCI1(strBuf);     // 文字列送信
	    anglebuf++,ADbuf++,currentbuf++;
	}
	PE.DRL.BIT.B1 =0;
	return 0;
}