foc.asm

本程序为电机控制系统中

         BANZ       INIT_TBL,AR0        ;AR1=0结束 
         
        
        
        ;mar * AR0
       ; LAR  AR0 ,#8500h
       ; lacc #0
       ; rpt #7
       ; sacl *+ 
         
        ;mar * AR0
       ; LAR  AR0 ,#8900h
        ;lacc #0
        ;rpt #7
        ;sacl *+ 
        
               
                             
         LAR   AR1  ,#BUF_START
         LAR   AR2  ,#BUF_START1
         LAR   AR3  ,#BUF_START2
         
         
         
         LDP   #232
         SPLK   #0000100001000010B,T1CON  ;使能T1
         CLRC  INTM                     ; 开总中断         
CHECK       LDP #225
            LACL PFDATDIR
            LDP #6
           SACL  KEYDATA
            BIT   KEYDATA ,BIT1
            BCND  STOP ,TC
            B  RUN            
            
STOP        LED_D7_ON   
            STOP_PWM      
            B  CHECK

;-----------------------初始化结束-------------------------------------------------------------------

;----------------------主程序略----------------------------------------------------------------------
MAIN 
CHECK1      LDP #225
            LACL PFDATDIR
            LDP #6
            SACL  KEYDATA
            BIT   KEYDATA ,BIT1
            BCND  STOP1 ,TC
            B RUN
STOP1       LED_D7_ON  
            STOP_PWM      
            B  CHECK1
                   
RUN         LED_D7_OFF 
            START_PWM  
            B MAIN

;--------------------------------------假中断处理----------------------------------------------------------
PHANTOM   
            CLRC     INTM
            RET
;------------------------PDPINTA Interrupt Service Routine------------------------------------------------  
_PDP_ISR
            SETC  INTM
            STOP_PWM
            LED_D10_ON
            RET
;-----------------------T1下溢中断处理子程序-----------------------------------------------------
_C_INT2

            SST   #0, ST0_SAVE          ; 保存现场ST0
            SST   #1, ST1_SAVE          ; 保存ST1
            LDP   #6 
            SACH  ACCH 
            SACL  ACCL                 ; 保存ACC
            SPH    P_HI 
            SPL   P_LO                    ; 保存 P
            MPY   #1                      ; P<=T
            SPL   T_SAVE                 ; 保存 T 
            SAR   AR0,AR0_SAVE          ; 保存AR0
            CLRC  SXM
            LDP   #224  
            LACC  PIVR                  ;读偏移地址
            SUB   #029H                  ; T1 下溢中断?
            BCND    T1UF_ISR,EQ           ;是下溢中断跳T1UF_ISR
REST       
            LDP   #6                     ;否则恢复现场
            LAR   AR0, AR0_SAVE          ; 恢复AR0
            LT    P_LO                     ;恢复P
            MPY   #1  
            LPH   P_HI  
            LT    T_SAVE                  ;恢复T
            LACC  ACCH,16 
            ADDS  ACCL                  ;恢复ACC
            LDP   #0                      ;指向B2
            LST   #1, ST1_SAVE           ; 恢复ST1
            LST   #0, ST0_SAVE           ; 恢复ST0
            CLRC  INTM                   ; 开中断
            RET                            ; 返回
T1UF_ISR 

            LDP   #232
            SPLK   #0FFFH,EVAIFRA         ; 清中断标
; /*=============设置几个有用的计数器=============*/   
        LDP  #6
        LACC PWMCNT_LED
        ADD #1H
        SACL PWMCNT_LED
        
        LACL PWMCNT_LED
 
        SUB #1H 

        BCND  ON   ,EQ
        LACC PWMCNT_LED
        SUB #800H
        BZ OFF 
        LACC PWMCNT_LED
        SUB #PWMCNT_LED_
        BZ   NEWCNT 
        B IN
ON
      
        LED_D8_ON  
        B   IN
OFF  
 
        LED_D8_OFF
        B IN
NEWCNT   
        SPLK #0H ,PWMCNT_LED 
      
IN      LDP #6
        LACL  N_REF
        SACL  N_REF_former
        
        LDP #6
       	LACL PWMCNT_N_REF
        ADD #1
        SACL PWMCNT_N_REF
        LACL PWMCNT_N_REF
        SUB  #1
        BZ  REGULATOR
        
        LACL N_REF_former
        SACL N_REF
        B IN1
REGULATOR  LACL N_REF_former
           SUB   N_REF
           BLZ   INCREASE
           LACL  N_REF_former
           SUB N_REF_STEP
           SACL N_REF
           B IN2
INCREASE
           LACL N_REF_former
           ADD N_REF_STEP
           SACL N_REF
           B IN2
IN1       LACL PWMCNT_N_REF
          SUB  #5
          BZ NEWCNT1
          B IN2
NEWCNT1 
         SPLK #0 ,PWMCNT_N_REF         
         LACL N_REF_former
         SACL N_REF
    
IN2      
;-----------------------电流采样和AD转换------------------------------------------------------
   
ADC_measure   LDP #225
              SPLK    #2000H,ADCTRL2  ;start conversion
              NOP
CONVERSION  
            BIT  ADCTRL2 ,3  ; Wait for SEQ1 Busy bit to
            BCND CONVERSION ,TC ; If TC=1, keep looping.
            RSXM                	;/*读取转换结果*/
            LACC RESULT0,10
            LDP #6
            SACH UDC
            LDP #225
            LACC RESULT1,10
            LDP #6
            SACH IA
            LDP #225
            LACC RESULT2,10
            LDP #6
            SACH IB
            LDP #225
            LACC RESULT3,10
            LDP #6
            SACH CTRL
            LDP #225
            LACC RESULT4,10
            LDP #6 
            SACH TEMPER
            SSXM
            LDP #225  
            SPLK #4000H ,ADCTRL2   ;/*停止转换*/ 
             
            LDP #6
            LT  CTRL
            MPY  #K_N_REF
            PAC 
            rpt #7
            SFR
            SACL  Speed_set
            LACL  Speed_set
            SUB  #82
            BLZ MIN
            LACL  Speed_set
            SUB  #3878
            BGZ MAX
            LACL  Speed_set
            SACL N_REF
            b WAIT 
MIN         SPLK  #82, N_REF    ;1hz
             b WAIT 
MAX         SPLK  #3878 , N_REF

WAIT  
        
;-----------------------读编码器脉冲数,计算转角增量------------------------------------------------    
         LDP        #0E8H
         LACC       T2CNT                ;读编码脉冲
         NEG                              ;编码器反接线时
         ;BIT GPTCONA ,1
         ;BCND DECREASE ,NTC      ;减计数
         ;;NOP
        ; B SOLVE1
;DECREASE    NEG
                 
SOLVE1         
         LDP        #6
         SACL       TMP
         SUB       ENCODEROLD          ;减去前一个周期测的脉冲数
         SACL      ENCINCR             ;得到编码增量
         LACL     ENCINCR  
         BCND SOLVE2 ,LT
         B  SOLVE3
SOLVE2   LACL  #0FFFFH 
         ADD ENCINCR
         SACL ENCINCR
SOLVE3   LACC      TMP                    
         SACL      ENCODEROLD         ;更新ENCODEROLD
;-----------------------计算转速---------------------------------------------------------------------------
        
         LACC      SPEEDSTEP            ;检测是否该采样速度
         SUB       #1 
         SACL     SPEEDSTEP
         BCND     NOCALC,GT            ;没到采样时刻,退出
         LT       SPEEDTMP              ;到采样时刻,计算编码器增量累计值
         MPY      KSPEED               ;Q8格式
         PAC
         SFL
         SACH     N,7                    ;相当于右移8位,Q12格式的PU值
         LACC     #0 
         SACL     SPEEDTMP             ;SPEEDTMP清零
         LACC     #SPEEDSTEP20          ;SPEEDSTEP=20
         SACL     SPEEDSTEP            ;SPEEDSTEP重新赋初值
;-----------------------转速PI调节,输出ITREF----------------------------------------------------------
         LACC     N_REF                 ;转速给定值,外部输入,已转化成Q12格式
         SUB      N
         SACL      EPISPEED             ;转速偏差
         LACC      XISPEED,12            ;转速调节器积分累计量
         LT         EPISPEED
         MPY       KPN                   ;乘比例系数,Q12格式
         APAC                             ;累加
         SACH      UPI,4                  ;相当于右移12位