/********************************************************************
*****	DCモーター制御　サンプルプログラム						*****
*****	YSML使用サンプル										*****
********************************************************************/
#include "ysml.h"
#include "ysml-ex.h"
#include <stdio.h>
#include <sysio.h>

#define	WAIT_COUNT	30000

/********************************************************************
*****	モーターを止める										*****
********************************************************************/
void MotorStop(void)
{
	PulseStop(0);		//タイマーを止める
	PulseStop(2);		//タイマーを止める
	//タイマーを止めただけでは出力がでているのでモータは回転し続ける
	//そこで0を出力させる
	PulseInit(0, 6000, 0, TIM_CLOCK4);
	PulseStart(0);
	PulseInit(2, 6000, 0, TIM_CLOCK4);
	PulseStart(2);
}

int limit_flag;
/********************************************************************
*****	リミッター1												*****
********************************************************************/
void interrupt Irq2Int(void)
{
	IrqIntClear(2);
	puts("IRQ2 int");
	MotorStop();		//モータを停止させる
	limit_flag = 1;
}

/********************************************************************
*****	リミッター2												*****
********************************************************************/
void interrupt Irq3Int(void)
{
	IrqIntClear(3);
	puts("IRQ3 int");
	MotorStop();		//モータを停止させる
	limit_flag = 1;
}

/********************************************************************
*****	モーター正回転											*****
********************************************************************/
void MotorStart(int level)
{
	PulseStop(0);
	PulseStop(2);
	PulseInit(0, 6000, 0, TIM_CLOCK4);
	PulseStart(0);
	PulseInit(2, 6000, level, TIM_CLOCK4);
	PulseStart(2);
}

/********************************************************************
*****	モーター逆回転											*****
********************************************************************/
void MotorRev(int level)
{
	PulseStop(0);
	PulseStop(2);
	PulseInit(0, 6000, level, TIM_CLOCK4);
	PulseStart(0);
	PulseInit(2, 6000, 0, TIM_CLOCK4);
	PulseStart(2);
}

/********************************************************************
*****	モーターを指定距離だけ正回転させる						*****
*********************************************************************/
void MotorMoveS(int offset)
{
	int speed, next_speed, data, data2;

	data = PhaseCountRead(1, NULL, 0) + offset;	//目標位置

	next_speed = 0;
	speed = 1;
	limit_flag = 0;
	
	while ((data2=PhaseCountRead(1, NULL, 0)) < data) {
		if (limit_flag == 1) {
			break;
		}
		if (data - data2 < 450) {
			next_speed = 6000 - (450-(data-data2))*10;
			if (next_speed != speed) {
				MotorStart(speed = next_speed);
			}
		}
		else if (speed == 1) {	
			MotorStart(speed = 6000);
		}
	}
	MotorStop();
}

/********************************************************************
*****	モーターを指定距離だけ逆回転させる						*****
*********************************************************************/
void MotorMoveR(int offset)
{
	int speed, next_speed, data, data2;

	data = PhaseCountRead(1, NULL, 0) - offset;	//目標位置

	next_speed = 0;
	speed = 1;
	limit_flag = 0;
	while ((data2=PhaseCountRead(1, NULL, 0)) > data) {
		if (limit_flag == 1) {
			break;
		}
		if (data2 - data < 450) {
			next_speed = 6000 - (450-(data2-data))*10;
			if (next_speed != speed) {
				MotorRev(speed = next_speed);
			}
		}
		else if (speed == 1) {	
			MotorRev(speed = 6000);
		}
	}
	MotorStop();
}


/********************************************************************
*****	モーターを相対距離で移動させる							*****
*********************************************************************/
void MotorMove(int rel)
{
	if (rel > 0) {
		MotorMoveS(rel);
	}
	else if (rel < 0) {
		rel = -rel;
		MotorMoveR(rel);
	}	
	

}

/********************************************************************
*****	モーターを相対距離で移動させる							*****
*********************************************************************/
void MotorRel(int rel)
{
	long i;
	int pos = PhaseCountRead(1, NULL, 0);			//現在位置
	int tar = pos + rel;
	limit_flag = 0;								//目標位置
	while (tar - pos != 0) {
		if (limit_flag == 1) {
			break;
		}
		MotorMove(tar-pos);
		for (i = 0; i < WAIT_COUNT; i++) {
			;
		}
		pos = PhaseCountRead(1, NULL, 0);
	}

}

/********************************************************************
*****	モーターを絶対位置で移動させる							*****
*********************************************************************/
void MotorAbs(int abs)
{
	long i;
	//現在位置の取得
	int pos = PhaseCountRead(1, NULL, 0);
	limit_flag = 0;
	while (abs-pos != 0) {
		if (limit_flag == 1) {
			break;
		}
		MotorMove(abs-pos);
		for (i = 0; i < WAIT_COUNT; i++) {
			;
		}
		pos = PhaseCountRead(1, NULL, 0);
	}
}

/********************************************************************
*****	現在位置の表示											*****
********************************************************************/
void PrintCur(void)
{
	int data = PhaseCountRead(1, NULL, 0);
	printf("pos = %d\n", data);
}

/********************************************************************
*****	main関数												*****
********************************************************************/
void main(void)
{
	puts("Program Start");
	
	//位相計数
	PhaseCountInit(1, 1);
	PhaseCountStart(1);
	PhaseCountRead(1, NULL, 1);		//カウンタリセット

	IrqIntClear(2);
	IrqIntClear(3);
	IrqInit(2, IRQ_MODE_DOWN);
	IrqInit(3, IRQ_MODE_DOWN);
	_ei();
	
	while(1) {
		int c;
		unsigned short data, data2, speed, next_speed;
		char buf[256];
		fputc('>', stdout);
		gets(buf);
		switch (buf[0]) {
		case 'S': case 's':
			sscanf(buf, "%s%d", buf, &data);
			MotorRel(data);
			PrintCur();
			break;
		case 'R': case 'r':
			sscanf(buf, "%s%d", buf, &data);
			MotorRel(-data);
			PrintCur();
			break;
		case 'A': case 'a':
			sscanf(buf, "%s%d", buf, &data);
			MotorAbs(data);
			PrintCur();
			break;
		case 'T': case 't':
			PrintCur();
			break;
		case 'O': case 'o':
			PhaseCountRead(1, NULL, 1);
			break;
		}
	}
}