focpmsm.asm

是关于交流电机、无刷电机、有刷电机、步进`电机等不同电机的dsp汇编代码

****************************************************************
* TEXAS INSTRUMENTS *
* Implementation of a Speed Field Orientated Control*
* of 3phase PMSM motor using the TMS320F240 *
* *
****************************************************************
* File Name: focpmsm.asm *
* Originator: Erwan SIMON *
* Description: PMSM Speed field oriented control *
* DSP development platform : TI TMS320F240 Evaluation Module *
* Power board : IR2130 demo board *
* Motor : Digiplan MD3450 *
* *
* Last modified: 28/07/1999 *
****************************************************************
* Auxiliary Register used *
* ar4 pointer for context save stack *
* ar5 used as general purpose table pointer *
****************************************************************
.include "c240app.h"
****************************************************************
* Interrupt vector table *
****************************************************************
.global _c_int0
.sect "vectors"
b _c_int0 ;reset vector
_c_int1 b _c_int1
b _c_int2 ; PWM interrupt handler
stack .usect "blockb2",15 ;space for ISR indirect context save
dac_val .usect "blockb2",5 ;space for dac values in Page 0
.sect "table"
sintab  .include   sine.tab
;sine wave look-up table for sine and cosine waves generation
;4.12 format
****************************************************************
* Variables and constants initializations
****************************************************************
.data
*** current sampling constants
Kcurrent .word 01383h ;8.8 format (*19.5) sampled currents normalization
constant
;ADCIN0 (ia current sampling)
;ADCIN8 (ib current sampling)
*** axis transformation constants
SQRT3inv .word 093dh ;1/SQRT(3) 4.12 format
SQRT32 .word 0ddbh ;SQRT(3)/2 4.12 format
*** PWM modulation constants
.bss _v_meas,1
PWMPRD .set 258h ;PWM Period=2*600 -> Tc=2*600*50ns=60us (50ns
; resolution)
Tonmax .set 0 ;minimum PWM duty cycle
MAXDUTY .set PWMPRD-2*Tonmax ;maximum utilization of the inverter
*** PI current regulators parameters
Ki .word 07Ah ;4.12 format = 0.03
Kpi .word 999h ;4.12 format = 0.60 (include period)
Kcor .word 0cch ;4.12 format = 0.05
;Kcor = Ki/Kpi
*** PI speed regulators parameters
Kispeed .word 7ah ;4.12 format = 0.03
Kpispeed .word 06800h ;4.12 format = 6.5
Kcorspeed .word 12h ;4.12 format = 0.0046
*** Vqr and Vdr limitations
Vbase .set 01000h ;BEMF at base speed
Vmin .set 0ec00h ;4.12 format = -1.25 pu
Vmax .set 01400h ;4.12 format = 1.25 pu
*** Is and Idr limitations
ismax .word 01199h ;4.12 format = 4.51A Inominal+10%,
iSdrefmin .set 0ee67h ;4.12 format = -4.51A (1000h = Ibase)
iSdrefmax .set 00000h ;4.12 format = 0A (1000h = Ibase)
zero .word 0h
*** Initialization phase Iqr
iSqrefinit .set 01000h ;4.12 format = 4.1A (1000h = Ibase)
*** Encoder variables and constants
Kencoder .word 3
;this constant is used to convert encoder pulses
;[0;4095] to an electric angle [0;360]=[0000h;1000h]
Encpulses .set 4096 ;number of encoder pulses per mechanical
;revolution
*** Speed and estimated speed calculation constants
Nbase .set 1000h ;Base speed
Kspeed .set 0be7h ;used to convert encoder pulses to a speed value
;8.8 format = 11.9 (see manual for details about
;this constant calculation)
;base speed 3000rpm, PWMPR 258h
SPEEDSTEP set 28 ;speed sampling period = current sampling period * 40
*** Speed and estimated speed calculation constants
.bss tmp,1 ;temporary variable (to use in ISR only !!!)
.bss option,1 ;virtual menu option number
.bss daout,1 ;address of the variable to send to the DACs
.bss daouttmp,1 ;value to send to the DACs
*** DAC displaying table starts here
.bss ia,1 ;phase current ia
.bss ib,1 ;phase current ib
.bss ic,1 ;phase current ic
.bss Ua,1 ; (not used)
.bss Ub,1 ; (not used)
.bss Uc,1 ; (not used)
.bss sin,1 ;generated sine wave value
.bss t1,1 ;SVPWM T1 (see SV PWM references for details)
.bss t2,1 ;SVPWM T2 (see SV PWM references for details)
.bss cos,1 ;generated cosine wave value
.bss Va,1 ;Phase 1 voltage for sector calculation
.bss Vb,1 ;Phase 2 voltage for sector calculation
.bss Vc,1 ;Phase 3 voltage for sector calculation
.bss VDC,1 ;DC Bus Voltage
.bss taon,1 ;PWM commutation instant phase 1
.bss tbon,1 ;PWM commutation instant phase 2
.bss tcon,1 ;PWM commutation instant phase 3
.bss teta_e,1 ;rotor electrical position in the range [0;1000h]
;4.12 format = [0;360] degrees
.bss iSalfa,1 ;alfa-axis current
.bss iSbeta,1 ;beta-axis current
.bss vSal_ref,1 ;alfa-axis reference voltage
.bss vSbe_ref,1 ;beta-axis reference voltage
.bss iSdref,1 ;d-axis reference current
.bss iSqref,1 ;q-axis reference current
.bss iSd,1 ;d-axis current
.bss iSq,1 ;q-axis current
.bss vSdref,1 ;d-axis reference voltage
.bss vSqref,1 ;q-axis reference voltage
.bss epiq,1 ;q-axis current regulator error
.bss epid,1 ;d-axis current regulator error
.bss xiq,1 ;q-axis current regulator integral component
.bss xid,1 ;d-axis current regulator integral component
.bss n,1 ;speed
.bss n_ref,1 ;speed reference
.bss epispeed,1 ;speed error (used in speed regulator)
.bss xispeed,1 ;speed regulator integral component
.bss X,1 ;SVPWM variable
.bss Y,1 ;SVPWM variable
.bss Z,1 ;SVPWM variable
.bss sectordisp,1 ;SVPWM sector for display
.bss initphase,1 ;flag for initialization phase
.bss teta_m,1
.bss Vr,1 ;(not used)
.bss iSqrefmin,1 ;iSq min limitation
.bss iSqrefmax,1 ;iSq max limitation
*** END DAC displaying table
.bss sector,1 ;SVPWM sector
.bss serialtmp,1 ;serial communication temporary variable
.bss da1,1 ;DAC displaying table offset for DAC1
.bss da2,1 ;DAC displaying table offset for DAC2
.bss da3,1 ;DAC displaying table offset for DAC3
.bss da4,1 ;DAC displaying table offset for DAC4
.bss VDCinvT,1 ;used in SVPWM
.bss index,1 ;pointer used to access sine look-up table
.bss upi,1 ;PI regulators (current and speed) output
.bss elpi,1 ;PI regulators (current and speed) limitation error
.bss tmp1,1 ;tmp word
.bss accb,2 ;2 words buffer
.bss acc_tmp,2 ;2 words to allow swapping of ACC
.bss encoderold,1 ;encoder pulses value stored in the previous
;sampling period
.bss encincr,1 ;encoder pulses increment between two
;consecutive sampling periods
.bss speedtmp,1 ;used to accumulate encoder pulses increments
;(to calculate the speed each speed sampling period)
.bss speedstep,1 ;sampling periods down counter used to
;define speed
;sampling period
*** END Variables and constants initializations
*** Software switches ***
interrupt_module .set 1
current_sensing .set 1
current_scaling .set 1
clarke .set 1
park .set 1
inv_park .set 1
isq_regulator .set 1
isd_regulator .set 1
speed_regulator .set 1
svpwm .set 1
sine_table .set 1
position_sensing .set 1
position_scaling .set 1
speed_scaling .set 1
virtual_menu .set 1
.bss stall_timer1,1
.bss stall_timer2,1
.text
****************************************************************
* Initialisation Module *
****************************************************************
_c_int0:
****************************
* C2xx core general settings
****************************
clrc CNF ;set Block B0 as Data RAM (default)
setc OVM ;saturate when overflow
spm 0 ;no accumulator shift after multiplication
setc sxm ;sign extension mode on
****************************************************************
* Initialize ar4 as the stack for context save
* space reserved: DARAM B2 60h-80h (page 0)
****************************************************************
lar ar4,#79h
lar ar5,#60h
****************************************************************
* Disable the watchdog timer *
****************************************************************
ldp #DP_PF1
splk #006Fh, WD_CNTL
splk #05555h, WD_KEY
splk #0AAAAh, WD_KEY
splk #006Fh, WD_CNTL
*****************************************
* Initialization of the TMS320F240 Clocks
*****************************************
splk #00000010b,CKCR0;PLL disabled
;LowPowerMode0
;ACLK enabled
;SYSCLK 5MHz
splk #10110001b,CKCR1;10MHz CLKIN
;Do not divide PLL
;PLL ratio x2 (CPUCLK=20MHz)
splk #10000011b,CKCR0;PLL enabled
;LPM0
;ACLK enabled
;SYSCLK 10MHz
splk #40C0h,SYSCR ;Set up CLKOUT to be SYSCLK
*****************************************
* F240 specific control register settings
*****************************************
; reset system control register
lacc SYSSR
and #69FFh
sacl SYSSR
****************************************************************
* A/D initialization
****************************************************************
splk #0003h,ADC_CNTL2;prescaler set for a 10MHz oscillator
lacc ADC_FIFO1 ;empty FIFO
lacc ADC_FIFO1
lacc ADC_FIFO2
lacc ADC_FIFO2
****************************************************************
* Serial communication initialization
****************************************************************
splk #00010111b,SCICCR ;one stop bit, no parity, 8bits
splk #0013h,SCICTL1 ;enable RX, TX, clk
splk #0000h,SCICTL2 ;disable SCI interrupts
splk #0000h,SCIHBAUD ;MSB |
splk #0082h,SCILBAUD ;LSB |9600 Baud for sysclk 10MHz
splk #0022h,SCIPC2 ;I/O setting
splk #0033h,SCICTL1 ;end initialization
****************************************************************
* PWM Channel enable
* 74HC541 chip enable connected to IOPC3 of Digital input/output
****************************************************************
; Configure IO\function MUXing of pins
ldp #DP_PF2 ;Enable Power Security Function
splk #0009h,OPCRA ;Ports A/B all IO except ADCs
splk #0038h,OPCRB ;Port C as non IO function except IOPC0&3
splk #0FF08h,PCDATDIR;bit IOPC3
****************************************************************
* Incremental encoder initialization
****************************************************************
ldp #DP_EV
splk #0000h,T3CNT ;configure counter register
splk #0ffffh,T3PER ;configure period register
splk #9870h,T3CON ;configure for QEP and enable Timer T3
splk #0E2F0h,CAPCON ;T3 is selected as Time base for QEP
********************************************
* Wait state generator init
********************************************
ldp #ia
splk #04h,tmp
out tmp,WSGR
***************************************************************
* Variables initialization
****************************************************************
ldp #ia
lacc ismax
sacl iSqrefmax
neg
sacl iSqrefmin
zac
sacl iSqref
sacl iSdref
sacl n_ref
sacl iSdref
sacl index
sacl xid
sacl xiq
sacl xispeed
sacl upi
sacl elpi
sacl Va
sacl Vb
sacl Vc
sacl initphase
sacl da1
lacc #1
sacl da2
lacc #2
sacl da3
lacc #3
sacl da4
splk #015Ch,VDCinvT
splk #07FFFh,stall_timer1
splk #07FFFh,stall_timer2
*****************************************
* Event manager settings
*****************************************
ldp #DP_EV
splk #0666h,ACTR ;Bits 15-12 not used, no space vector
;PWM compare actions
;PWM5/PWM6 - Active Low/Active High
;PWM3/PWM4 - Active Low/Active High
;PWM1/PWM2 - Active Low/Active High
splk #300,CMPR1 ;no current sent to the motor
splk #300,CMPR2
splk #300,CMPR3
splk #0000h,DBTCON ;no dead band
splk #0207h,COMCON ;Reload Full Compare when T1CNT=0
;Disable Space Vector
;Reload Full Compare Action when T1CNT=0
;Enable Full Compare Outputs
;Disable Simple Compare Outputs
;Select GP timer1 as time base
;Full Compare Units in PWM Mode
splk #8207h,COMCON ;enable compare operation
splk #PWMPRD,T1PER ;Set PWM interrupt period
splk #0,T1CNT
splk #0A800h,T1CON ;Ignore Emulation suspend
;Up/Down count mode
;x/1 prescalar
;Use own TENABLE
;Disable Timer
;Internal Clock Source
;Reload Compare Register when T1CNT=0
;Disable Timer Compare operation
; Enable Timer 1 operation
lacc T1CON
or #40h
sacl T1CON
****************************************************************
* Enable PWM control Interrupt
****************************************************************
; Clear EV IFR and IMR regs
splk #07FFh,IFRA
splk #00FFh,IFRB
splk #000Fh,IFRC
; Enable T1 Underflow Int
splk #0200h,IMRA
splk #0000h,IMRB
splk #0000h,IMRC
;Set IMR for INT2
ldp #0h
lacc #0FFh
sacl IFR ;clear interrupt flags
lacc #0000010b
sacl IMR
clrc INTM ;enable all interrupts
b menu ;branch to menu loop
*******************************************************
* _c_int2 Interrupt Service Routine
* synchronization of the control algorithm with the PWM
* underflow interrupt
*******************************************************
;中断服务子程序
_c_int2:
************************
* Context Saving
************************
mar *,ar4 ;AR4 active auxiliary reg (stack pointer)
mar *-
sst #1,*- ;save status register 1
sst #0,*- ;save status register 0
sach *- ;save MS word of accu
sacl *- ;save LS word of accu
* END Context Saving *
mar *,ar5 ;AR5 active auxiliary reg
ldp #DP_EV ;DP points to Event Manager control reg page
lacc IVRA ;read the interrupt vector
.if interrupt_module
ControlRoutine
.if current_sensing
****************************************************************
* Current sampling - AD conversions
* N.B. we will have to take only 10 bit (LSB)
****************************************************************
ldp #DP_PF1
splk #1801h,ADC_CNTL1;ia and ib conversion start
;ADCIN0 selected for ia A/D1
;ADCIN8 selected for ib A/D2
conversion
bit ADC_CNTL1,8
bcnd conversion,tc ;wait approximatly 6us
lacc ADC_FIFO1,10
ldp #ia
sach ia
ldp #DP_PF1
lacc ADC_FIFO2,10
ldp #ib
sach ib
.endif
*** Initialization phase
lacl initphase ;are we in initialization phase ?
bcnd Run,NEQ
lacc #0fc00h ;if yes, set teta = 0fc00h 4.12 format = -90
; degrees
;(align rotor with phase 1 flux)
sacl teta_e ;
lacc #iSqrefinit ;q-axis reference current = initialization
;q-axis reference current
sacl iSqref ;
lacc #0 ;zero some variables and flags
sacl iSdref ;
sacl teta_m ;
sacl encoderold ;
sacl n ;
sacl speedtmp ;
lacc #SPEEDSTEP ;restore speedstep to the value SPEEDSTEP
; for next speed
;control loop
sacl speedstep ;
ldp #DP_EV
splk #0,T3CNT ;zero Incremental Encoder value if
;initialization step
ldp #initphase
b Init ;there is no need to do position and
;calculation
;in initialization phase (the rotor is locked)
Run
.if position_sensing
*** Encoder pulses reading
ldp #DP_EV
lacc T3CNT ;we read the encoder pulses and ...
;neg ;encoder plug in the opposite direction ?
ldp #ia
sacl tmp
sub encoderold ;subtract the previous sampling period value
;to have the increment that we'll
;accumulate in encoder
sacl encincr ;
add teta_m ;
bcnd encmagzero,GT,EQ;here we start to normalize teta_m
; value to the range [0;Encpulses-1]
add #Encpulses ;the value of teta_m could be negative
;it depends on the rotating direction
;(depends on motor windings
;to PWM Channels connections)
encmagzero
sacl teta_m ;now teta_m value is positive but could be
;greater than Encpulses-1
sub #Encpulses ;we subtract Encpulses and we check whether
;the difference is negative. If it is we