//========================================================
//	The information contained herein is the exclusive property of
//	Sunnnorth Technology Co. And shall not be distributed, reproduced,
//	or disclosed in whole in part without prior written permission.
//	(C) COPYRIGHT 2003 SUNNORTH TECHNOLOGY CO.
//	ALL RIGHTS RESERVED
//	The entire notice above must be reproduced on all authorized copies.
//========================================================

//========================================================
//	工程名稱：	DC_SpeedCtrl
//	功能描述：	直流電機速度測量與調節
//	涉及的庫：	CMacro1016.lib
//	組成文件：	main.c
//              isr.asm, Dig.asm, Key.asm, DCMotor.asm
//              Dig.inc, Key.inc, DCMotor.inc, SPCE061A.inc
//              Dig.h,Key.h, DCMotor.h, SPCE061A.h
//	硬件連接：	61板IOB高8位與電機模組J1連接
//				61板IOA高8位與電機模組J2連接
//				61板IOB低8位與電機模組J3連接
//	維護記錄：	2005-12-2	v1.0
//========================================================

//========================================================
//	文件名稱：	main.c
//	功能描述：	數碼管顯示
//	維護記錄：	2005-09-12	v1.0
//========================================================
#include "SPCE061A.h"
#include "DCMotor.h"
#include "Dig.h"
#include "Key.h"

#define SPEED_MAX	60									// 最高設定速度
#define	SPEED_MIN	30									// 最低設定速度
#define SPEED_STEP	2									// 速度設定步進值

#define CP	8											// 比例常數 Proportional Const
#define CI	3											// 積分常數 Integral Const
#define	CD	1											// 微分常數 Derivative Const

unsigned int g_Direction, g_Speed;						// 全局變量，保存設定的轉動方向和速度


//========================================================================
//	語法格式：	void System_Init(void)
//	實現功能：	系統初始化（時鐘、變量等）
//	參數：		無
//	返回值：	無
//========================================================================
void System_Init(void)
{
	*P_SystemClock = C_Fosc_49M|C_StrongMode;			// 系統時鐘

	DCMotor_Init();
	Key_Init();
	DIG_Init();

	g_Direction = 1;									// 初始設置為正轉
	g_Speed = 40;
}

//========================================================================
//	語法格式：	void DispNum(unsigned int Num)
//	實現功能：	在4位數碼管上顯示整數
//	參數：		Num - 要顯示的數值，范圍0~9999
//	返回值：	無
//========================================================================
const unsigned int DigNum[]={							// 0~9十個數字對應的數碼管顯示編碼
	0x3f,0x06,0x5b,0x4f,0x66,
	0x6d,0x7d,0x27,0x7f,0x6f
	};

void DispNum(unsigned int Num)
{
	unsigned int TempNum = Num, k;

	DIG_Clear();										// 清除數碼管顯示
	if(Num>=1000)										// 千位
	{
		k = TempNum/1000;
		DIG_Set(1,DigNum[k]);
		TempNum = TempNum - k * 1000;
	}
	if(Num>=100)										// 百位
	{
		k = TempNum/100;
		DIG_Set(2,DigNum[k]);
		TempNum = TempNum - k * 100;
	}
	if(Num>=10)											// 十位
	{
		k = TempNum/10;
		DIG_Set(3,DigNum[k]);
		TempNum = TempNum - k * 10;
	}
	DIG_Set(4,DigNum[TempNum]);							// 個位
}

//========================================================================
//	語法格式：	void DelayMS(unsigned ms)
//	實現功能：	延時若干毫秒(不精確)
//	參數：		ms - 毫秒數@49MHz
//	返回值：	無
//========================================================================
void DelayMS(unsigned ms)
{
	unsigned i;
	while(ms--)	
	{
		for(i = 0; i < 1044; i++)
			*P_Watchdog_Clear = 1;
	}
}

//========================================================================
//	語法格式：	int DCMotor_Speed_PID(int SetSpeed, int CurSpeed)
//	實現功能：	增量式PID計算: u0 = u1 + CP*(e0-e1) + CI*e0 + CD*(e0-2*e1+e2)
//	參數：		SetSpeed:	設定的轉速
//				CurSpeed:	實際測得轉速
//	返回值：	直流電機速度等級(PWM占空比)
//========================================================================
int DCMotor_Speed_PID(int SetSpeed, int CurSpeed)
{
	int e0, u0;
	static int u1 = 0;									// 前一次輸出值（為避免浮點運算將該值擴大16倍）
	static int e1 = 0, e2 = 0;
	
	e0 = SetSpeed - CurSpeed;							// 當前速度偏差
	u0 = u1 											
		 + CP * (e0 - e1)								// 比例項
		 + CI * e0										// 積分項
		 + CD * (e0 - e1 - e1 + e2);					// 微分項
	
	if(u0 > 1023) u0 = 1023;							// 輸出值應小于64（擴大16倍）
	if(u0 < 16) u0 = 16;								// 輸出值應不小于1（擴大16倍）

	u1 = u0;											// 更新u1、e1和e2
	e2 = e1;
	e1 = e0;
	
	return u0>>4;										// 返回u0/16
}

//========================================================================
//	語法格式：	void SetSpeed(void)
//	實現功能：	設定轉速
//	參數：		無
//	返回值：	無
//========================================================================
void SetSpeed(void)
{
	unsigned int KeyCode;

	while(1)
	{
		DispNum(g_Speed);								// 閃爍顯示當前設定的轉速
		DelayMS(300);
		DIG_Clear();
		DelayMS(300);
		
		KeyCode = Key_Get();
		switch(KeyCode)
		{
			case KEY_1:									// Key1, 設定值增加
				g_Speed += SPEED_STEP;
				if(g_Speed > SPEED_MAX)g_Speed = SPEED_MIN;
				break;
			case KEY_2:									// Key2, 設定值減小
				g_Speed -= SPEED_STEP;
				if(g_Speed < SPEED_MIN)g_Speed = SPEED_MAX;
				break;
			case KEY_3:									// Key3, 確定
				return;
			default:
		}
	}
}

//========================================================================
//	語法格式：	void SetDirection(void)
//	實現功能：	設定轉動方向
//	參數：		無
//	返回值：	無
//========================================================================
void SetDirection(void)
{
	unsigned int KeyCode;
	unsigned int i;

	i = 1;
	while(1)
	{
		/// 在數碼管上顯示滾動的"-"，表示電機轉動方向
		DIG_Clear();									// 清除顯示
		DIG_Set(i,0x40);
		DelayMS(200);
		i += g_Direction;
		if(i>4)i=1;
		if(i<1)i=4;
		
		KeyCode = Key_Get();
		switch(KeyCode)
		{
			case KEY_1:									// Key1, 正轉
				g_Direction = 1;
				break;
			case KEY_2:									// Key2, 反轉
				g_Direction = -1;
				break;
			case KEY_3:									// Key3, 確定
				return;
			default:
				break;
		}
	}		

}

//========================================================================
//	語法格式：	void StartMotor(void)
//	實現功能：	直流電機轉動控制
//	參數：		無
//	返回值：	無
//========================================================================
void StartMotor(void)
{
	unsigned int KeyCode;
	unsigned int SpeedLevel, CurSpeed=0, PreSpeed=0;
		
	if(g_Direction==1)									// 確定轉動方向
		DCMotor_Forward(MOTOR_1+MOTOR_2);
	else
		DCMotor_Backward(MOTOR_1+MOTOR_2);

	DispNum(CurSpeed);
	while(1)
	{
		if(CurSpeed != PreSpeed)						// 如果轉速改變則更新顯示
		{
			DispNum(CurSpeed);							// 顯示當前轉速
			PreSpeed = CurSpeed;
		}
		if(DCMotor_IsSpeedRdy())						// 如果已完成一次測速
		{
			CurSpeed = DCMotor_GetSpeed();				// 獲取轉速
			SpeedLevel = DCMotor_Speed_PID(g_Speed, CurSpeed);	// PID計算，得到新的PWM占空比
			DCMotor_SetSpeed(MOTOR_1+MOTOR_2, SpeedLevel);		// 更新PWM占空比，調節轉速
		}
		KeyCode = Key_Get();
		switch(KeyCode)	
		{
			case KEY_1:									// Key1，速度增加
				g_Speed += SPEED_STEP;
				if(g_Speed > SPEED_MAX)g_Speed = SPEED_MIN;
				break;
			case KEY_2:									// Key2，速度減小
				g_Speed -= SPEED_STEP;
				if(g_Speed < SPEED_MIN)g_Speed = SPEED_MAX;
				break;
			case KEY_3:									// Key3，停止
				DCMotor_Stop(MOTOR_1+MOTOR_2);
				return;
			default:
				break;
		}
	}
}

//========================================================================
//	語法格式：	int main(void)
//	實現功能：	主函數，通過按鍵選擇不同功能
//	參數：		無
//	返回值：	無
//========================================================================
int main(void)
{
	unsigned int KeyCode, Flag_KeyEvent;
	
	System_Init();										// 系統初始化
	Flag_KeyEvent = 1;
	
	while(1)
	{
		if(Flag_KeyEvent==1)
		{
			DispNum(g_Speed);							// 顯示當前設定的轉速
			Flag_KeyEvent = 0;
		}
		KeyCode = Key_Get();
		switch(KeyCode)
		{
			case KEY_1:									// Key1, 轉速設定
				SetSpeed();
				Flag_KeyEvent = 1;
				break;
			case KEY_2:									// Key2, 方向設定
				SetDirection();
				Flag_KeyEvent = 1;
				break;
			case KEY_3:									// Key3, 啟動電機
				StartMotor();
				Flag_KeyEvent = 1;
				break;
			default:
				break;
		}
	}
}