steerdrive.asm

基于凌阳spce061a的舵机pwm驱动程序

.define P_TimerA_Data      0x700A
.define P_TimerA_Ctrl      0x700B
.define P_IOB_Data	       0x7005
.define P_IOB_Buf		   0x7006
.define P_IOB_Dir		   0x7007
.define P_IOB_Attrib       0x7008
.define P_INT_Ctrl		   0x7010
.define IRQ1_8KHz	       0x1000
.define P_Watchdog_Clear   0x7012
.define P_SystemClock      0x7013


.define PIN_Steer		0x0101      //定义舵机控制引脚
.define STPWM_MiddleCnt 0x000e      //定义舵机居中时的计数值
.define Steer_Pwm_Cycle 0x00a8      //定义计数周期值164

.public Steer_Pwm_Cnt          //声明舵机脉宽计数变量
.public Motor_Status           //声明电机状态变量
.iram
 .var Steer_Pwm_Cnt=0
 .var Motor_Status = 0x0000 // b0: 
							// b1:
							// b2: 舵机方向，100－left1   110-left2   001－right1    011-right2  其它－middle
							

.code
//======================================================
// 函数名称：	STMotor_Init
// C调用：		void STMotor_Init(void)
// 汇编调用：	F_STMotor_Init
// 实现功能：   舵机驱动初始化
// 入口参数：	无
// 出口参数：	无
// 破坏寄存器：	无
.public _STMotor_Init
.public F_STMotor_Init
.public _STMotor_Drive
.public F_STMotor_Drive
_STMotor_Init:
F_STMotor_Init:
    push r1 to [sp]          //初始化舵机驱动引脚为同相低电平输出，具体引脚通过定义PIN_Steer实现
    r1=[P_IOB_Dir]
    r1|=PIN_Steer
    [P_IOB_Dir]=r1
    r1=[P_IOB_Attrib]
    r1|=PIN_Steer
    [P_IOB_Attrib]=r1
    r1=[P_IOB_Data]
    r1&=~PIN_Steer
    [P_IOB_Data]=r1
    r1=0x01f3
    [P_TimerA_Ctrl]=r1
    r1 = 0xffff //定义定时器A预置数
    [P_TimerA_Data]=r1
    r1 = [P_INT_Ctrl]
	r1 |= IRQ1_8KHz
	[P_INT_Ctrl] = r1            //开TMA定时器中断
	IRQ ON
	r1=0x0002
	[Motor_Status]=r1
    pop r1 from [sp]
retf
//======================================================
// 函数名称：	STMotor_Drive
// C调用：		void STMotor_Drive(void)
// 汇编调用：	F_STMotor_Drive
// 实现功能：   产生PWM波形，在IRQ1_TMA中断服务程序中调用
// 入口参数：	无
// 出口参数：	无
// 破坏寄存器：	无
//======================================================	
.public _STMotor_TurnR1
.public F_STMotor_TurnR1
_STMotor_TurnR1:
F_STMotor_TurnR1:
    push r1 to[sp]
    r1=[Motor_Status]
    r1&=0xfff8
    r1|=0x0001
    [Motor_Status]=r1
    pop r1 from [sp]
retf
.public _STMotor_TurnR2
.public F_STMotor_TurnR2
_STMotor_TurnR2:
F_STMotor_TurnR2:
    push r1 to[sp]
    r1=[Motor_Status]
    r1&=0xfff8
    r1|=0x0003
    [Motor_Status]=r1
    pop r1 from [sp]
retf
.public _STMotor_TurnL1
.public F_STMotor_TurnL1
_STMotor_TurnL1:
F_STMotor_TurnL1:
    push r1 to[sp]
    r1=[Motor_Status]
    r1&=0xfff8
    r1|=0x0004
    [Motor_Status]=r1
    pop r1 from [sp]
retf
.public _STMotor_TurnL2
.public F_STMotor_TurnL2
_STMotor_TurnL2:
F_STMotor_TurnL2:
    push r1 to[sp]
    r1=[Motor_Status]
    r1&=0xfff8
    r1|=0x0006
    [Motor_Status]=r1
    pop r1 from [sp]
retf
.public _STMotor_Middle
.public F_STMotor_Middle
_STMotor_Middle:
F_STMotor_Middle:
    push r1 to[sp]
    r1=[Motor_Status]
    r1&=0xfff8
    r1|=0x0002
    [Motor_Status]=r1
    pop r1 from [sp]
retf
_STMotor_Drive:
F_STMotor_Drive:
	push r1,r5 to [sp]
	r3=[Steer_Pwm_Cnt]    //读pwm计数值
	r3+=1                 //计数值加1
	r1 = [Motor_Status]       //读取电机状态
	r5=[P_IOB_Data]           //读取输出状态
	cmp r3,STPWM_MiddleCnt+3
	jae ?STMotor_PWM_Low
	test r1,0x0004            //是否左转，舵机状态位为100或110则跳转
	jnz	?STMotor_left1
	test r1,0x0001             //是否右转，舵机状态为001或011则跳转
	jnz ?STMoto_right1
?STMoto_middle:
    cmp r3,STPWM_MiddleCnt       //舵机居中，比较当前pwm计数值舵机居中的计数值，若大于等于居中计数值则调低电压
    jae ?STMotor_PWM_Low         
    r5| = PIN_Steer              //否则继续输出高电平
    jmp ?STMotor_PWM_Next
?STMotor_left1:
	test r1,0x0002              //判断是否左满舵
	jnz ?STMoto_left2
	cmp r3,STPWM_MiddleCnt-1
	jae ?STMotor_PWM_Low
	r5| = PIN_Steer
	jmp ?STMotor_PWM_Next
?STMoto_left2:
    cmp r3,STPWM_MiddleCnt-2
    jae ?STMotor_PWM_Low
    r5| = PIN_Steer
	jmp ?STMotor_PWM_Next
?STMoto_right1:
    test r1,0x0002
    jnz ?STMoto_right2
	cmp r3,STPWM_MiddleCnt+1
	jae ?STMotor_PWM_Low
	R5| = PIN_Steer
	jmp ?STMotor_PWM_Next
?STMoto_right2:
    cmp r3,STPWM_MiddleCnt+2
    jae ?STMotor_PWM_Low
    r5| = PIN_Steer
	jmp ?STMotor_PWM_Next
?STMotor_PWM_Low:
	r5& =~PIN_Steer
?STMotor_PWM_Next:
	cmp r3,Steer_Pwm_Cycle
	jae ?STMotoCnt_Reset
	jmp ?STMotor_Update
?STMotoCnt_Reset:
    r3=0x0000
?STMotor_Update:
	[Steer_Pwm_Cnt] = r3
	[P_IOB_Data] = r5
?Exit:
	pop r1,r5 from [sp]
retf