dcmotor.asm

利用凌阳单片机开发电机,方便制作与学习凌阳单片机

//======================================================
//  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.
//========================================================

//============================================================
// 文件名称：DCMotor.asm
// 实现功能：SPGT62C19B的直流电机驱动程序，使用IRQ4_4KHz中断
// 日期：    2005/7/18
//============================================================

//================================================
//	SPGT62C19B控制端口设定
//================================================

/////-----       若使用IOA则使用下面的定义	    -----/////
//.define P_Motor_Data	0x7000
//.define P_Motor_Buf		0x7001
//.define P_Motor_Dir		0x7002
//.define P_Motor_Attr	0x7003

/////-----       若使用IOB则使用下面的定义	    -----/////
.define P_Motor_Data	0x7005
.define P_Motor_Buf		0x7006
.define P_Motor_Dir		0x7007
.define P_Motor_Attr	0x7008

//================================================
//	SPGT62C19B控制引脚设定
//================================================

//			bit:	FEDCBA9876543210
.define PIN_I01		0000000000001000b		//IO PIN 3
.define PIN_I11		0000000000100000b		//IO PIN 5
.define PIN_PH1		0000000000000001b		//IO PIN 0
.define PIN_I02		0000000000010000b		//IO PIN 4
.define PIN_I12		0000000001000000b		//IO PIN 6
.define PIN_PH2		0000000000000010b		//IO PIN 1
.define PIN_SPD		0000000000000100b		//IO PIN 2
.define PIN_MOTOR	PIN_I01+PIN_I11+PIN_PH1+PIN_I02+PIN_I12+PIN_PH2

//================================================
//	速度检测去抖动时间设定，单位为1/4096秒
//================================================

.define MOTOR_DEBOUNCE	10

//================================================
//	中断常量定义
//================================================

.define P_INT_Mask		0x702d
.define C_IRQ4_4KHz		0x0040

//================================================
//	变量定义
//================================================

.iram
/////-----       电机驱动状态变量	    -----/////
.var Motor_Status = 0x00ff	// b15: Enable Motor2
							// b14: Enable Motor1
							// b13: Motor2 Direction. 0-Fwd, 1-Bwd
							// b12: Motor1 Direction
							// b11\
							//  |   Motor2 Speed Level(0~63)
							// b06/
							// b05\
							//  |   Motor1 Speed Level(0~63)
							// b00/
.var Motor_PWM_Cnt = 0x0000	// IO模拟PWM宽度计数，b11~b6:Motor2; b5~b0:Motor1

/////-----       电机速度检测相关变量	    -----/////
.var Motor_Timer_Cnt = 0	// 定时器计数值
.var Motor_Pulse_Cnt = 0	// 速度检测脉冲计数值
.var Motor_Pulse_Deb = 0	// 速度检测脉冲去抖动计数
.var Motor_Speed = 0x00		// 电机速度

//================================================
//	公有函数声明
//================================================
.public _DCMotor_Init
.public F_DCMotor_Init
.public _DCMotor_SetSpeed
.public F_DCMotor_SetSpeed
.public _DCMotor_Forward
.public F_DCMotor_Forward
.public _DCMotor_Backward
.public F_DCMotor_Backward
.public _DCMotor_Drive
.public F_DCMotor_Drive
.public _DCMotor_Stop
.public F_DCMotor_Stop
.public _DCMotor_GetSpeed
.public F_DCMotor_GetSpeed

.code
//======================================================
// 函数名称：	DCMotor_Init
// C调用：		void DCMotor_Init(void)
// 汇编调用：	F_DCMotor_Init
// 实现功能：   直流电机驱动初始化
// 入口参数：	无
// 出口参数：	无
// 破坏寄存器：	无
//======================================================
_DCMotor_Init:
F_DCMotor_Init:
	push r1 to [sp]

	r1 = [P_Motor_Attr]						// 初始化IO
	r1 |= PIN_MOTOR
	r1 &= ~PIN_SPD
	[P_Motor_Attr] = r1
	r1 = [P_Motor_Dir]
	r1 |= PIN_MOTOR
	r1 &= ~PIN_SPD
	[P_Motor_Dir] = r1
	r1 = [P_Motor_Buf]
	r1 |= PIN_MOTOR
	r1 &= ~PIN_SPD
	[P_Motor_Data] = r1

	r1 = 0x0000
	[Motor_Status] = r1
	[Motor_PWM_Cnt] = r1
	[Motor_Speed] = r1
	[Motor_Pulse_Cnt] = r1
	[Motor_Pulse_Deb] = r1
	[Motor_Timer_Cnt] = r1

	r1 = [P_INT_Mask]
	r1 |= C_IRQ4_4KHz
	[P_INT_Mask] = r1
	IRQ ON

	pop r1 from [sp]
retf

//======================================================
// 函数名称：	DCMotor_SetSpeed
// C调用：		void DCMotor_SetSpeed(unsigned Motor,unsigned Speed)
// 汇编调用：	F_DCMotor_SetSpeed
// 实现功能：   设定电机转速等级
// 入口参数：	Motor(R1) - 选择对哪个直流电机接口进行转速等级设定：
//					MOTOR_1		直流电机接口1
//					MOTOR_2		直流电机接口1
//				Speed(R2) - PWM占空比，范围0-63，单位1/64
// 出口参数：	无
// 破坏寄存器：	无
//======================================================
_DCMotor_SetSpeed:
	push r1,r2 to [sp]
	r1 = sp + 6
	r2 = [r1--]
	r1 = [r1]
	call F_DCMotor_SetSpeed
	pop r1,r2 from [sp]
retf

F_DCMotor_SetSpeed:
	push r3 to [sp]
	r2 &= 0x003f
	r3 = [Motor_Status]
	cmp r1,1
	je ?Motor1
	cmp r1,2
	je ?Motor2
	cmp r1,3
	jne ?Exit
?Motor1:
	r3 &= ~0x003f
	r3 |= r2
	cmp r1,3
	jne ?Motor_SaveStatus
?Motor2:
	r2 = r2 LSL 4
	r2 = r2 LSL 2
	r3 &= ~0x0FC0
	r3 |= r2
?Motor_SaveStatus:
	[Motor_Status] = r3
	r3 = 0
	[Motor_PWM_Cnt] = r3
?Exit:	
	pop r3 from [sp]
retf

//======================================================
// 函数名称：	DCMotor_Forward
// C调用：		void DCMotor_Forward(unsigned Motor)
// 汇编调用：	F_DCMotor_Forward
// 实现功能：   使电机正向转动
// 入口参数：	Motor(R1) - 设定正向转动的电机：
//					MOTOR_1		电机1
//					MOTOR_2		电机2
// 出口参数：	无
// 破坏寄存器：	无
//======================================================
_DCMotor_Forward:
	push r1 to [sp]
	r1 = sp + 4
	r1 = [r1]
	call F_DCMotor_Forward
	pop r1 from [sp]
retf

F_DCMotor_Forward:
	push r2,r3 to [sp]
	r2 = [Motor_Status]
	cmp r1,1
	je ?Motor1
	cmp r1,2
	je ?Motor2
	cmp r1,3
	jne ?Exit
?Motor1:
	r3 = [P_Motor_Buf]
	r3 &= ~PIN_PH1
	[P_Motor_Data] = r3
	r2 &= ~0x1000
	r2 |= 0x4000
	cmp r1,3
	jne ?Motor_SaveStatus
?Motor2:
	r3 = [P_Motor_Buf]
	r3 &= ~PIN_PH2
	[P_Motor_Data] = r3
	r2 &= ~0x2000
	r2 |= 0x8000
?Motor_SaveStatus:
	[Motor_Status] = r2
	r2 = 0
	[Motor_PWM_Cnt] = r2
?Exit:	
	pop r2,r3 from [sp]
retf

//======================================================
// 函数名称：	DCMotor_Backward
// C调用：		void DCMotor_Backward(unsigned Motor)
// 汇编调用：	F_DCMotor_Backward
// 实现功能：   使电机反向转动
// 入口参数：	Motor(R1) - 设定反向转动的电机：
//					MOTOR_1		电机1
//					MOTOR_2		电机2
// 出口参数：	无
// 破坏寄存器：	无
//======================================================
_DCMotor_Backward:
	push r1 to [sp]
	r1 = sp + 4
	r1 = [r1]
	call F_DCMotor_Backward
	pop r1 from [sp]
retf

F_DCMotor_Backward:
	push r2,r3 to [sp]
	r2 = [Motor_Status]
	cmp r1,1
	je ?Motor1
	cmp r1,2
	je ?Motor2
	cmp r1,3
	jne ?Exit
?Motor1:
	r3 = [P_Motor_Buf]
	r3 |= PIN_PH1
	[P_Motor_Data] = r3
	r2 |= 0x5000
	cmp r1,3
	jne ?Motor_SaveStatus
?Motor2:
	r3 = [P_Motor_Buf]
	r3 |= PIN_PH2
	[P_Motor_Data] = r3
	r2 |= 0xA000
?Motor_SaveStatus:
	[Motor_Status] = r2
	r2 = 0
	[Motor_PWM_Cnt] = r2
?Exit:	
	pop r2,r3 from [sp]
retf

//======================================================
// 函数名称：	DCMotor_Stop
// C调用：		void DCMotor_Stop(unsigned Motor)
// 汇编调用：	F_DCMotor_Stop
// 实现功能：   直流电机停止转动
// 入口参数：	Motor(R1) - 需要停转的电机
//					MOTOR_1		电机1
//					MOTOR_2		电机2
// 出口参数：	无
// 破坏寄存器：	无
//======================================================
_DCMotor_Stop:
	push r1 to [sp]
	r1 = sp + 4
	r1 = [r1]
	call F_DCMotor_Stop
	pop r1 from [sp]
retf

F_DCMotor_Stop:
	push r2 to [sp]
	cmp r1,1
	je	?Motor1
	cmp r1,2
	je	?Motor2
	cmp r1,3
	jne ?Exit
?Motor1:
	r2 = [P_Motor_Buf]
	r2 |= PIN_I01 + PIN_I02
	[P_Motor_Buf] = r2
	r2 = [Motor_Status]
	r2 &= ~0x4000
	[Motor_Status] = r2
	cmp r1,3
	jne ?ClearSpeed
?Motor2:
	r2 = [P_Motor_Buf]
	r2 |= PIN_I11 + PIN_I12
	[P_Motor_Buf] = r2
	r2 = [Motor_Status]
	r2 &= ~0x8000
	[Motor_Status] = r2
?ClearSpeed:
	r2 = 0
	[Motor_Speed] = r2
?Exit:
	pop r2 from [sp]
retf

//======================================================
// 函数名称：	DCMotor_Drive
// C调用：		void DCMotor_Drive(void)
// 汇编调用：	F_DCMotor_Drive
// 实现功能：   产生PWM波形，在IRQ4_4KHz中断服务程序中调用
// 入口参数：	无
// 出口参数：	无
// 破坏寄存器：	无
//======================================================
_DCMotor_Drive:
F_DCMotor_Drive:
	push r1,r5 to [sp]
	r1 = [Motor_Status]
	test r1,0xC000
	jnz	?Motor_ChkPulse
	goto ?Exit
?Motor_ChkPulse:							// 速度检测
	r2 = [P_Motor_Data]
	r2 &= PIN_SPD
	jz	?Motor_Pulse
	r2 = 0
	[Motor_Pulse_Deb] = r2
	jmp ?Motor_GetSpeed
?Motor_Pulse:
	r2 = [Motor_Pulse_Deb]
	jnz	?Motor_Pulse_Old
	r3 = [Motor_Pulse_Cnt]
	r3 += 1
	[Motor_Pulse_Cnt] = r3
?Motor_Pulse_Old:
	r2 += 1
	[Motor_Pulse_Deb] = r2
?Motor_GetSpeed:
	r4 = [Motor_Timer_Cnt]
	r4 += 1
	cmp r4, 2048
	jb ?Motor_Drive
	r5 = [Motor_Pulse_Cnt]
	r5 = r5 LSR 1
	[Motor_Speed] = r5
	r5 = 0
	[Motor_Pulse_Cnt] = r5
	r4 = 0
?Motor_Drive:								// PWM发生
	[Motor_Timer_Cnt] = r4
	r4 = [Motor_PWM_Cnt]
	r5 = [P_Motor_Buf]
?Motor1:
	r2 = r1&0x003f
	r3 = r4&0x003f
	cmp r3,r2
	jae ?Motor1_PWM_Low
?Motor1_PWM_High:
	r5 &= ~(PIN_I11+PIN_I01)
	jmp ?Motor1_PWM_Next
?Motor1_PWM_Low:
	r5 |= (PIN_I11+PIN_I01)
?Motor1_PWM_Next:
	r3 += 1
	r3 &= 0x003f
	r4 &= ~0x003f
	r4 |= r3
?Motor2:
	r2 = r1 & 0x0fc0
	r3 = r4 & 0x0fc0
	cmp r3,r2
	jae ?Motor2_PWM_Low
?Motor2_PWM_High:
	r5 &= ~(PIN_I12+PIN_I02)
	jmp ?Motor2_PWM_Next
?Motor2_PWM_Low:
	r5 |= PIN_I12+PIN_I02
?Motor2_PWM_Next:
	r3 += 0x0040
	r3 &= 0x0fc0
	r4 &= ~0x0fc0
	r4 |= r3
?Motor_Update:
	[Motor_PWM_Cnt] = r4
	[P_Motor_Data] = r5
?Exit:
	pop r1,r5 from [sp]
retf

//======================================================
// 函数名称：	DCMotor_GetSpeed
// C调用：		unsigned DCMotor_GetSpeed(void)
// 汇编调用：	F_DCMotor_GetSpeed
// 实现功能：   获取电机转速
// 入口参数：	无
// 出口参数：	r1 - 测得的电机转速值,单位(转/秒)
// 破坏寄存器：	无
//======================================================
_DCMotor_GetSpeed:
F_DCMotor_GetSpeed:
	r1 = [Motor_Speed]
retf