spdcal.asm

PMSM的速度检测程序 基于2407的 参考别人的

    .include  	"F2407REGS.H" 
    .global _para_init	
	.global _calcvel 
	.bss s_tmp,1 
	.bss speedstep,1
	.bss encod_old,1 
	.bss vol_increa,1
	.bss Accum_Cnt,1
	.bss _Vel_Mech,1 
	.global  _Vel_Mech
	.data 
SPEEDSTEP .set 6
Kspeed .set  3A9h  
    .text 
_para_init: 
      POPD  *+
      SAR   AR0,*+
      SAR   AR1,*
      LARK  AR0,9
      LAR   AR0,*0+ 
      SAR   AR6,*+
      SAR   AR7,*+ 
      MAR   *,AR1 
   
     ldp #speedstep 
     lacc   #6
     sacl speedstep
     lacc  #0
     sacl  s_tmp
     sacl  encod_old
     sacl  vol_increa
     sacl  Accum_Cnt
     sacl  _Vel_Mech
     clrc   CNF
     setc   SXM
     setc   OVM
      LAR   AR7,*-
      LAR   AR6,*-
      SBRK  10
      LAR   AR0,*-
      PSHD  * 
      MAR   *,AR1 
      RET
       
_calcvel:
      POPD  *+
      SAR   AR0,*+
      SAR   AR1,*
      LARK  AR0,9
      LAR   AR0,*0+ 
      SAR   AR6,*+
      SAR   AR7,*+  
      
      MAR   *,AR1  
      clrc   CNF
      setc   SXM
      setc   OVM
      
      ldp   #0E8h
      lacc  T2CNT 
      ldp   #s_tmp
      sacl  s_tmp           ;temp valiable
      sub   encod_old       ;last velocity
      sacl  vol_increa      ;error velocity
      lacc  s_tmp
      sacl  encod_old
;******************************************************
;end of encoder pulses reading
;canculate speed  and updata reference speed variables
;******************************************************    
      lacc  speedstep
      sub   #1
      sacl  speedstep
      bcnd  nocalc,GT
;speed calculate from encoder pulses
      spm   3;p regist right shift 6 bit
      ldp   #Accum_Cnt
      lt    Accum_Cnt
      mpy   #Kspeed  ;Q6 format
      pac 
      ;sfr 
      ;sfr
      sacl  _Vel_Mech  ;represent the motor; speed Q12 format (0-FFF)-->(0-1500r/m)
      lacc  #0
      sacl  Accum_Cnt
      lacc  #SPEEDSTEP
      sacl  speedstep 
      spm   0   ;remian and not shift 
      b     exitspd
nocalc:
      LACC  Accum_Cnt
      ADD   vol_increa
      SACL  Accum_Cnt
exitspd:      
      ;MAR   *,AR1
      LAR   AR7,*-
      LAR   AR6,*-
      SBRK  10
      LAR   AR0,*-
      PSHD  *
      MAR   *,AR1
      RET