www.pudn.com > DC_SpeedCtrl.rar > 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 // 电机速度。 .var Motor_SpeedRdy = 0 // 电机转速测量完成标志 //================================================ // 公有函数声明 //================================================ .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 .public _DCMotor_IsSpeedRdy .public F_DCMotor_IsSpeedRdy .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 [Motor_SpeedRdy] = 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, 1024 jb ?Motor_Drive r5 = [Motor_Pulse_Cnt] // r5 = r5 LSR 1 [Motor_Speed] = r5 r5 = 0 [Motor_Pulse_Cnt] = r5 r5 = 1 [Motor_SpeedRdy] = 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 = 0 [Motor_SpeedRdy] = r1 r1 = [Motor_Speed] retf //====================================================== // 函数名称： DCMotor_IsSpeedRdy // C调用： unsigned DCMotor_IsSpeedRdy(void) // 汇编调用： F_DCMotor_IsSpeedRdy // 实现功能： 是否已完成一次电机转速测量 // 入口参数： 无 // 出口参数： r1 - 结果, 0:否, 1:是 // 破坏寄存器： 无 //====================================================== _DCMotor_IsSpeedRdy: F_DCMotor_IsSpeedRdy: r1 = [Motor_SpeedRdy] retf