*********************************************************************************
* Induction Motor Vector control *
*********************************************************************************
* Preleminaire ver based on TI appl. 10/07/00
* Special IMMC 240 / MSK243
*
* Interface RS232 pour communication IRDA -> DSP ____18/03/04
* IOPC1 et IOPC2 à 5V pour 9600 bds du 7001 RX ____18/03/04
* macro pour LCD / DAC
*
* version light minimum V et f constante mais qui communique avec le PC
* et qui sort les grandeur sur DAC via SPI
* VERSION en cours de developpement 15/11/00
* .CMD modifié now .data and .bss se trouvent ds le meme endroit B1_F page0
* table ds la memoire prog ;-)
* version 4 DAC à sorties succecives 27/11/00
* .CMD remodifiée, no more .data à cause des pbs de Flashage 28/11/00
* cntrl_n DP pbs fixed
* debug pour flashage en cours 09/01/01
* pb de lecture de la FLASH, recopie en RAM avant execution 09/07/01
* Version OK pour FLASH ou Run en RAM, voir 2 projets
*
* Vers Code Composer 2 (new prj)
* controle vectoriel en cours de dev 19/03/2003 sur banc MAS ex 300 W
* version V/f only lighted
* fast SPI SANS INTERRUPTION et IR via SCI... 20/04/2004
* et LCD avec affichage fs, I1, I2 23/04/2004
* Flashable (recopie de table LCD)
* conv hex to decimal (magic numbers) 27/05/2004
* Ecran LCD contraste, nouvelle routine 02/02/2007
*********************************************************************************
CS_DACon .macro
LDP #00E1h
LACC PADATDIR
AND #0FFFDh ; IOPA1 mise à 0 ie DAC activé
SACL PADATDIR
.endm
CS_DACoff .macro
LDP #00E1h
LACC PADATDIR
OR #0002h ; IOPA1 mise à 1 ie DAC desactivé
SACL PADATDIR
.endm
CS_LCDon .macro
LDP #00E1h
LACC PADATDIR
AND #0FFFBh ; IOPA2 mise à 0 ie LCD activé
SACL PADATDIR
.endm
CS_LCDoff .macro
LDP #00E1h
LACC PADATDIR
OR #0004h ; IOPA2 mise à 1 ie LCD desactivé
SACL PADATDIR
.endm
;****************************************************************
; SPI Configuration 1 pour DAC 2 pour LCD
;****************************************************************
SPI_Config .macro Conf
.if Conf=1 ; SPI initialization pour DSP-> DAC (clk pol 1, no clkphase, master, no INT)
LDP #0E0h
SPLK #00C7h,SPI_CNTL1 ; Reset SPI by writing 1 to SWRST !!!clk pol1
; SPLK #0087h,SPI_CNTL1 ; Reset SPI by writing 1 to SWRST !!!clk pol0
; SPLK #000Ch,SPI_CNTL2 ; Disable ints & TALK, normal clock, master mode clk phase 1
SPLK #0004h,SPI_CNTL2 ; Disable ints & TALK, normal clock, master mode clk phase 0
SPLK #0000h,SPI_PRI ; Set SPI interrupt to high priority.
; SPLK #0000h,SPI_BAUD ; Baudrate = Fatest as possible ! SYSCLK/4 et ça marche !
SPLK #0005h,SPI_BAUD ; Baudrate = 2 MHz! SYSCLK/5
SPLK #0000h,SPI_STATUS ; Clear the SPI interrupt status bits
; SPLK #000Eh,SPI_CNTL2 ; Enable TALK, CLK ph 1, master mode + disable SPI Int
SPLK #0006h,SPI_CNTL2 ; Enable TALK, CLK ph 0, master mode + disable SPI Int
SPLK #0052h,SPI_PORT_C1 ; Enable the SPICLK pin function. ; SPISTE as output pin et mise à 1 ie desactive
SPLK #0022h,SPI_PORT_C2 ; Set SIMO & SOMI functions to serial I/O
SPLK #0047h,SPI_CNTL1 ; Release SWRST, clock polarity 1, 8 bits
; SPLK #0007h,SPI_CNTL1 ; Release SWRST, clock polarity 0, 8 bits
.endif
.if Conf=2 ; SPI initialization pour DSP-> LCD
LDP #0E0h
SPLK #00C7h,SPI_CNTL1 ; Reset SPI by writing 1 to SWRST !!!clk pol1
SPLK #000Ch,SPI_CNTL2 ; Disable ints & TALK, normal clock, master mode
SPLK #0000h,SPI_PRI ; Set SPI interrupt to high priority.
SPLK #0000h,SPI_BAUD ; Baudrate = SYSCLK/4 ; accepté ie 2.5 MHz, alors que le max du LCD est 2 MHz
SPLK #0000h,SPI_STATUS ; Clear the SPI interrupt status bits
SPLK #0006h,SPI_CNTL2 ; Enable TALK, No CLK ph, master mode + DESABLE SPI Int
SPLK #0052h,SPI_PORT_C1 ; Enable the SPICLK pin function. ; SPISTE as output pin et mise à 1 ie desactive
SPLK #0022h,SPI_PORT_C2 ; Set SIMO & SOMI functions to serial I/O
SPLK #0047h,SPI_CNTL1 ; Release SWRST, clock polarity 1, 8 bits
.endif
.endm
;****************************************************************
; Routine trChar : transforme le char LCDChar en LCDChar_m (pour le LCD)
;****************************************************************
trChar .macro
LDP #LCDChar
SPLK #08h,tmp
SPLK #0000h,LCDChar_m
loop? LACC LCDChar
SFR
SACL LCDChar
LACC LCDChar_m
ROL
SACL LCDChar_m
LACC tmp
SUB #1
SACL tmp
BNZ loop?
LACC LCDChar_m
AND #00F0h
SACL LCDlo
LACC LCDChar_m,4
AND #00F0h
SACL LCDhi
.endm
DispLCD .macro Order ; 1=> ordre, 2 => Char, on utilise le LCDhi également pour les ordres ! 02/2007
CS_LCDon
LDP #0E0h
LACC SPI_BUF
.if Order=1 ; 1=> ordre, 2 => Char
SPLK #0F8h, SPI_DAT ; Transmit start Ordre
.endif
.if Order=2
SPLK #0FAh, SPI_DAT ; Transmit start Order
.endif
; LACC SPI_BUF
lcd1? BIT SPI_STATUS,BIT6
BCND lcd1?, NTC
LACC SPI_BUF
LDP #LCDlo
LACC LCDlo
LDP #0E0h
SACL SPI_DAT ; Transmit Low byte
; LACC SPI_BUF
lcd2? BIT SPI_STATUS,BIT6
BCND lcd2?, NTC
LACC SPI_BUF
LDP #LCDhi
LACC LCDhi
LDP #0E0h
SACL SPI_DAT ; Transmit hi byte
LACC SPI_BUF
lcd3? BIT SPI_STATUS,BIT6
BCND lcd3?, NTC
LACC SPI_BUF
CS_LCDoff
.endm
Attends .macro laps ; attends un "laps" de temps
LDP #i
SPLK #0,i
loopi? LACC i
ADD #1
SACL i
SUB #laps
BNZ loopi?
.endm
; Convertit un "Word" hexa en "4 chars Hexadécimaux"
ConvHexa .macro Var, offs ; attends un "offs" de 4, 8, 12 ou 16
LACC Var,offs
SACH tmp
LACC tmp
AND #0Fh
SACL tmp
SUB #9
BCND unHexa?, GT
LACC tmp
ADD #030h
B eviteHexa?
unHexa?
LACC tmp
ADD #037h
eviteHexa?
SACL *+
.endm
; Convertit un "Word" hexa en "4 chars Décimaux + signe"
ConvDec .macro Var
SETC SXM
LACC Var
BCND positif?, GEQ
ZAC
SUB Var
SACL Var
LACC #02Dh
B evitneg?
positif?
LACC #02Bh
evitneg?
SACL signe
CLRC SXM ; pas de sign extension
SPM 0 ;PM=00, no shift after multiplication, qqun le modifie par ailleurs !
; Magic=8389; s=7; // d=1000
; m=floor((Magic*ns>>W)>>s);
LT M8389
MPY Var
SPH tmp
LACC tmp,9 ;16-7=9
SACH tmp ;ie 7 LSR
; ici on aurait du checker le dépassement de m>9!
; ns-=1000*m;
LACC Var
LT tmp
MPY #1000
SPAC ;Subtract PREG from ACC
SACL Var
LACC tmp
ADD #030h ; table de charactères : chiffres
SACL millier ; ecrit char : millier
; Magic=5243; s=3; // d=100
; c=floor((Magic*ns>>W)>>s);
LT M5243
MPY Var
SPH tmp
LACC tmp,13 ;16-3=13
SACH tmp ;ie 3 LSR
; ns-=100*c;
LACC Var
LT tmp
MPY #100
SPAC ;Subtract PREG from ACC
SACL Var
LACC tmp
ADD #030h ; table de charactères : chiffres
SACL centaine ; ecrit char : centaine
; Magic=52429; s=3; // d=10
; d=floor((Magic*ns>>W)>>s);
LT M52429
MPYU Var ;MPY unsigned
SPH tmp
LACC tmp,13 ;16-3=13
SACH tmp ;ie 3 LSR
; ns-=10*m = u
LACC Var
LT tmp
MPY #10
SPAC ;Subtract PREG from ACC
SACL Var
LACC tmp
ADD #030h ; table de charactères : chiffres
SACL dizaine ; ecrit char : dizaine
LACC Var
ADD #030h ; table de charactères : chiffres
SACL unite ; ecrit char : unité
SETC SXM ; remet le sign extension
.endm
; Include the C240 MM Regs file
.include "c240.h"
.include "vects_a.h"
;- vector table specific à l'IMMC...
.globl _c_int0
;-------------------------------------------------
; Auxiliary Register used
; ar6 est utilisé in the interruption ADCINT mais il est d'abord sauve puis restoré
;-------------------------------------------------
; Numeric formats: all 4.12 fixed point format twos
; complement for negative values (4 integer & sign + 12 fractional) except otherwise specified
; Currents: 1000h (4.12)= 2.5 A = Ibase=1.41*In0=1.41*1.7
; Voltages: 1000h (4.12)= 325 V = Vbase=1.41*Vn0=1.41*230
; Angles : [0;ffffh] = [0;360] degrees
; Speed : [0;1000h] (4.12) = [0;1500] rpm
;-------------------------------------------------
; no Variables and constants initializations
;-------------------------------------------------
*** vSqref and VdSr limitations
Vmin .set 0F590h ;4.12 format=-0.652 pu
Vmax .set 0A70h ;4.12 format=0.652 pu
*** iSqref limitations
Iqsrefmin .set -3277 ;4.12 format=-0.8 pu
Iqsrefmax .set 3277 ;4.12 format=0.8 pu
*** Speed calculation constants
;Kspeed .set 0da7h ;this constant is needed only with encoder it is used to
;convert encoder pulses to a speed value.
;8.8 format = 13.65
Kspeed .set 01ABh ;8.8 format = 3.333/2 *2^8 voir cahier Rech. 09/03/2005 à cause des 200 us
Kspeed_tr_mn .set 09Dh ; cste =1.22/2 *2^8 (ie en 8.8) coef pour avoir Vitesse en tr/mn
Kspeed_tr_mn_pu .set 700 ; cste =4096/1500 *2^8 (ie en 8.8) coef pour Vitesse en tr/mn -> pu
SPEEDSTEP .set 30 ;speed samplig period =current sampling period * SPEEDSTEP
*** current sampling constants
.bss Kcurrent,1 ;8.8 format (19.51) sampled currents normalization constant
;Kr .set 0eh ;Kr=T/Tr=3.3117*10-3 (4.12 f) !!! (T -> 200us)
Kt .set 1b0h ;Kt=1/(Tr*wBase)=105.42*10-3 (f 4.12)
;K .set 148h ;K=65536/200, the K constant must take the rotor flux
K .set 28Fh ;K=65536/100, the K constant must take the rotor flux
;position from 0 to 65535 in 200 sample times
*** axis transformation constants
SQRT3inv .set 093dh ;1/SQRT(3) 4.12 format
SQRT32 .set 0ddbh ;SQRT(3)/2 4.12 format
.bss SR8BIT,1 ;used to shift bits 8 right
*** PWM modulation constant
PWMPRD .set 1000 ;PWM Period=2*1000 ->
;Tc=2*1000*50ns=100us (50ns resolution)
*** PI current regulators parameters
.bss Ki,1 ;4.12 format=0.0625
.bss Kpi,1 ;4.12 format=1
.bss Kcor,1 ;4.12 format=0.0625
*** PI speed regulators parameters
.bss Kin,1 ;4.12 format=0.012939453
.bss Kpin,1 ;4.12 format=4.510498047
.bss Kcorn,1 ;4.12 format=0.002685546
.bss tmp,1 ;temporary variable (to use in ISR only !!!)
.bss tmp1,1 ;temporary variable
; .bss n_ref8_8,1 ;8.8 format reference speed for Field Weakening behavior
.bss option,1 ;virtual menu option number
.bss ControlModeOld,1 ;ancienne valeur
.bss ControlMode,1 ;0 : V/f
;1 : Cntrl Vect. Cmde en courant
;2 : Cntrl Vect. Regul vitesse
.bss Task,1 ;Task=0 : Affichage LCD/DAC, Task=1 : Ctrl Vect, Regul I, calc vitesse
.bss ia,1 ;phase current ia
.bss ib,1 ;phase current ib
.bss ic,1 ;phase current ic
.bss Vref1,1 ;variable for sector calculation
.bss Vref2,1 ;variable for sector calculation
.bss Vref3,1 ;variable 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 Ialphas,1
.bss Ibetas,1
.bss Valphasref,1
.bss Vbetasref,1
.bss Vdsref,1
.bss Vqsref,1
.bss Idsref,1
.bss Ids,1
.bss Iqsref,1
.bss Iqs,1
.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 ;vitesse mécanique
.bss n_tr_mn,1 ;vitesse mécanique en tr/mn
.bss n_ref,1 ;speed reference
.bss nref_tr_mn,1 ;speed reference en tr/mn
.bss epin,1 ;speed error (used in speed regulator)
.bss xin,1 ;speed regulator integral component
.bss Teta_cm1,1 ;rotor flux position with current
;model used only in the
;communication program
.bss sinTeta_cm,1 ;sine rotor flux position with current model, 4.12 f
.bss cosTeta_cm,1 ;cosine rotor flux position with current model, 4.12 f
; .bss i_mr,1 ;magnetizing current (used only in the current model), 4.12 f
.bss fs,1 ;rotor flux speed 4.12 f
.bss K_V_fs,1 ; V/f factor in 4.12 f
; ADC variables
.bss _Imes1,1
.globl _Imes1
.bss _Imes2,1
.globl _Imes2
.bss _Ref1,1
.globl _Ref1
.bss _Ref2,1
.globl _Ref2
*** my DAC memory
.bss WhichDAC,1 ; Quelle donnee a transmettre now (1, 2, 3 ou 4)
.bss DAC_OR,1 ; OR value of numero DAC
.bss DataDAC,1 ; donnee à envoyer sur le DAC pour le LOW 8bits
.bss DAC1,1 ;DAC variable memory for DAC1
.bss DAC2,1 ;DAC variable memory for DAC2
.bss DAC3,1 ;DAC variable memory for DAC3
.bss DAC4,1 ;DAC variable memory for DAC4
; inutile .bss NewData,1 ; flag de donnee à envoyer
*** END my DAC memory
.bss run,1 ;initialization flag
.bss Teta_cm,1 ;real rotor flux position, output
;of the current model
.bss serialtmp,1 ;serial communication temporary
;variable
.bss VDCinvT,1 ;VDCinv*(T/2) (used in SVPWM)
.bss tetaincr,1 ;variable used in current model
.bss Index,1 ;pointer used to access sine look-up table
* PI regulators variable
.bss upi,1 ;PI regulators (current and speed) output
.bss elpi,1 ;PI regulators (current and speed)
;limitation error
.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
.bss _SpeedRef,1
; .bss SCItable,40h
.bss i,1
.bss j,1
.bss LCDRafj,1
.bss LCDlo,1
.bss LCDhi,1
.bss LCDupdate,1
.bss LCDline,1
.bss LCDcol,1
.bss LCDChar,1
.bss LCDChar_m,1
LCDRafrate .set 500 ; vitesse de rafraichissement de l'ecran : (500+32+1)*200 us
; .bss LCDtable,32
.data
.label LCDtable_src ;LOAD addy of the table(FLASH)
LCDtable ;run addy of the table
; .word "Salam Lotfi "
; .word "I1= , I2= "
; .word "SalamLB,fs=+00.0"
.word "Vd=+000,fs=+00.0"
.word "K=+0000,Wm=+0000"
Vdsrefoffs .set LCDtable+3
fsoffs .set LCDtable+11
Koffs .set LCDtable+18
Wmoffs .set LCDtable+27
.label LCDtable_srcI ;LOAD addy of the table(FLASH) for current control
.word "Iq*+0.0,Id*=+0.0"
.word "Iq=+0.0,Wm=+0000"
Iqsrefoffs .set LCDtable+3
Idsrefoffs .set LCDtable+12
Iqsoffs .set LCDtable+19
.label LCDtable_srcWm ;LOAD addy of the table(FLASH) for speed control
.word "Id*+0.0,W*=+0000"
.word "Iq=+0.0,Wm=+0000"
WmIdsrefoffs .set LCDtable+3
Wmrefoffs .set LCDtable+11
.bss Var,1
.bss signe,1
.bss millier,1
.bss centaine,1
.bss dizaine,1
.bss unite,1
.bss M8389,1
.bss M5243,1
.bss M52429,1
.bss M5000,1
.bss M300,1
.bss M250,1
.data
.label sintab ;LOAD addy of the sinetable(FLASH)
SinusRam ;run addy of the table
.include sinus.tab ;sine wave look-up table for sine and cosinewaves generation
;generated by Excel 4.12 format
;----- END look-up table .includes
;----- END Variables and constants
.text ;link in "text section"
; Start the program code HERE for FLASH reasons (entry point 1000h)
_c_int0:
;-------------------------------------------------
; Board general settings
;-------------------------------------------------
; SETC CNF
CLRC CNF ; B0 for data memory
CLRC OVM ; Reset overflow mode
SETC SXM ; Reset sign extension mode
CLRC XF
spm 0 ;no shift after multiplication
SETC INTM ; Set global interrupt mask (pas d'INT)
; avant les interruptions, recopie de la table sinus de flash en RAM
LDP #tmp
SAR AR6, tmp
MAR *,AR6
LAR AR6, #SinusRam
RPT #255
BLPD #sintab, *+
; recopie aussi la LCDtable de la flash vers la ExtDM(.data en run)
LAR AR6, #LCDtable
RPT #31
BLPD #LCDtable_src, *+
LAR AR6, tmp
MAR *, AR1 ;restore
LDP #M8389
SPLK #8389, M8389
SPLK #5243, M5243
SPLK #52429, M52429
SPLK #5000, M5000
SPLK #250, M250
SPLK #300, M300
;-------------------------------------------------
; Variables initialization
;-------------------------------------------------
ldp #run ;control variable page
ZAC
SACL run
SACL Task
SACL Index
SACL xid
SACL xiq
SACL xin
SACL upi
SACL elpi
SACL Vref1
SACL Vref2
SACL Vref3
SACL Iqsref
SACL Ids
SACL Iqs
SACL nref_tr_mn
SACL n_ref
SACL i
SACL j
SACL ControlModeOld
SACL ControlMode ; 0 : V/f initialement
; attention au ZAC
SPLK #1, ControlMode ; 2 : Regul I en Ctrl Vect initialement
; SPLK #2, ControlMode ; 2 : Regul Wm en Ctrl Vect initialement
; current regul / field weakening : noFieldWeakening
splk #0666h,Idsref ;Idsref=0.4 pu ie 1A pour 2.5 A de Ibase;Idsref 4.12 f without Field Weakening
SPLK #LCDRafrate, LCDRafj ; rafraichissement de l'ecran
splk #0EC4h, VDC ;The DC voltage is 300 V
;Vdc=0.923 in 4.12 with a Vbase=325 V
splk #043Bh, VDCinvT ;T/(Vdc*2) or PWMPRD/VDC=1083
;rescaled by 4.12
; special coeff / variable page
splk #1000h, Kcurrent ;8.8 format (64.00) sampled currents normalization constant
; 5A=512=200h, Ib=2.5A=64, =1 p.u.
*** axis transformation constants
splk #100h, SR8BIT ;used to shift bits 8 right
; Fluxage nominal
splk #1000h, K_V_fs
*** PI current regulators parameters
splk #0100h, Ki ;4.12 format=0.0625
splk #01000h, Kpi ;4.12 format=1
splk #0100h, Kcor ;4.12 format=0.0625
*** PI speed regulators parameters
splk #35h, Kin ;4.12 format=0.012939453
splk #482bh, Kpin ;4.12 format=4.510498047
splk #0bh, Kcorn ;4.12 format=0.002685546
;------- END Variables initialization ------------
; load ISR addresses to Interrupt Vector in on-chip block B2
; en special pour toutes les cartes... wrap, old and new
LDP #0
LACC #ADCINT
SACL adcintvec
; LACC #Phantom
; SACL xint1vec
; LACC #Phantom2
; SACL xint2vec
; LACC #Phantom2
; SACL xint3vec
; trap any phantom int !
LACC #Phantom2
LDP #0
SACL rxintvec
SACL txintvec
SACL xint1vec
SACL xint2vec
SACL xint3vec
SACL spiintvec
SACL wdtintvec
; SACL adcintvec
SACL cmp1intvec
SACL cmp2intvec
SACL cmp3intvec
SACL scmp1intvec
SACL scmp2intvec
SACL scmp3intvec
SACL tpint1vec
SACL tcint1vec
SACL tufint1vec
SACL tofint1vec
SACL tpint2vec
SACL tcint2vec
SACL tufint2vec
SACL tofint2vec
SACL tpint3vec
SACL tcint3vec
SACL tufint3vec
SACL tofint3vec
SACL capint1vec
SACL capint2vec
SACL capint3vec
SACL capint4vec
SACL pdpintvec
**********************************************
* Function to disable the watchdog timer
**********************************************
ldp #DP_PF1
splk #006Fh, WD_CNTL
splk #05555h, WD_KEY
splk #0AAAAh, WD_KEY
splk #006Fh, WD_CNTL ; clear WDFLAG, Disable WDT, set WDT
; for 1 second overflow.
; ici pas de WD ! ie disabled
***********************************************************
* Function to initialise the Event Manager
* GPTimer 1 => Full PWM
* Enable Timer 1==0 interrupt on INT2
* All other pins are IO
***********************************************************
;Set up SYSCLK and PLL for C24 EVM with 10MHz ;External Clk
; inutile car l'IMMC est déjà à 20 MHz
; Set up CLKOUT to be SYSCLK p6-6
; ldp #DP_PF1 ;deja mise pour le WD
;_____IRDA______
SPLK #4000h,SYSCR ; je veux le port IOPC1 dispo
LACC SYSSR
AND #069FFh
SACL SYSSR ; Clear reset variables :-)
;Set up zero wait states for external memory
;inutile aussi
; lacc #0004h
; sacl *
; out *,WSGR
;ADC Unit setting : mesure de Imes1 et Imes2 sur ADC 6 et 14 d'abord
ldp #0E0h
splk #1B6Ch,ADCNTL1 ; cONFIGURATION DE L'adcntl1
splk #0403h,ADCNTL2 ; cONFIGURATION DE L'adcntl2
; PREPARE TO CONV. adc 1&2
lacc ADCFIFO1 ; CLEAR FIRST AND SECOND LEVEL adc fifo 1 & 2
lacc ADCFIFO1
lacc ADCFIFO2
lacc ADCFIFO2
;Clear All EV Registers
zac
LDP #DP_EV
; sacl GPTCON
sacl T1CNT
sacl T1CMP
sacl T1PER
sacl T1CON
sacl T2CNT
sacl T2CMP
sacl T2PER
sacl T2CON
sacl T3CNT
sacl T3CMP
sacl T3PER
sacl T3CON
sacl COMCON
sacl ACTR
sacl SACTR
sacl DBTCON
sacl CMPR1
sacl CMPR2
sacl CMPR3
sacl SCMPR1
sacl SCMPR2
sacl SCMPR3
sacl CAPCON
sacl CAPFIFO
LACC FIFO1
LACC FIFO2
LACC FIFO3
; sacl FIFO1
; sacl FIFO2
; sacl FIFO3
; sacl FIFO4
;Initialise PWM ; No software dead-band
splk #666h,ACTR ;Bits 15-12 not used, no space vector
;PWM compare actions
;PWM6/PWM5 -Active Low/Active High
;PWM4/PWM3 -Active Low/Active High
;PWM2/PWM1 -Active Low/Active High
SPLK #002Ah,SACTR ; active high pour les simple PWM
splk #100,CMPR1
splk #200,CMPR2
splk #300,CMPR3
splk #100,SCMPR1 ; pour la 2eme etoile initialisation
splk #200,SCMPR2
splk #300,SCMPR3
splk #0307h,COMCON ;FIRST enable PWM operation
;Reload Full Compare when T1CNT=0
;Disable Space Vector
;Reload Full Compare Action when T1CNT=0
;Enable Full Compare Outputs
; Enable Simple Compare Outputs
; GPTimer 1 for simple PWM
; Reload SIMPLE Compare Action when T1CNT=0
;Full Compare Units in PWM Mode
splk #8307h,COMCON ;THEN enable Compare operation
; configuration du timer 1
splk #0100h,GPTCON ; GP Timer Control Register : lance ADC sur Timer1 period match !
splk #PWMPRD,T1PER ;Set T1 period
splk #0,T1CNT
splk #02800h,T1CON ;STOP TIMER on Emulation suspend
; splk #0A800h,T1CON ;Ignore Emulation suspend POUR EVITER LES PBS aux IGBT et à la MAS
;Cont Up/Down Mode
;x/1 prescalar
;Use own TENABLE
;Disable Timer,enable later
;Internal Clock Source
;Reload Compare Register when T1CNT=0
;Disable Timer Compare operation
; I/O setting p11-11
; PWM enable sur les 2 etoiles 1-6 et 7-9 ainsi que les CAP... et des IOP A0 et A3 non utilisés ici
LDP #00E1h
SPLK #00700h, OPCRA ; pas de ADCIN 0,1, 9, 8 et ni IOPB0 à IOPB2 mais :
; IOPA0 ààà IOPA3 et PWM 7-9 sur I/O Port
; refait plus loin pour SCI / IRDA
;_____IRDA______
; --->SPLK #0030h, OPCRB ; mettre les CAP1 et 2 /QEP... sur I/O Port
SPLK #00F00h, PADATDIR ;IOPA0, 1, 2 et 3 as output
LACC PADATDIR
***********************************************************
* Incremental encoder initialization
* Capture for Incremental encoder correction with Xint2
***********************************************************
ldp #DP_EV
splk #0000h,T3CNT ;configure counter register
splk #00FFh,T3PER ;configure period register
splk #0E2F0h,CAPCON ;T3 is selected as Time base for QEP
splk #9870h,T3CON ;configure for QEP and enable Timer T3
*** END encoder/capture initialization
****************************************************************
* Serial communication initialization pour échange IRDA -> HSDL 1001 -> HSDL 7001->DSP (directement sur le SCI RX)
****************************************************************
LDP #0E0h
SPLK #00010111b,SCI_CCNTL ;one stop bit, no parity, 8bits
;_____IRDA______
SPLK #0011h,SCI_CNTL1 ;SCI SW RESET=0 + configure : enable RX only, pas de TX, clk
; SPLK #0013h,SCI_CNTL1 ;SCI SW RESET=0 + configure : enable RX et TX , clk
SPLK #0000h,SCI_CNTL2 ;disable SCI interrupts
SPLK #0000h,SCI_HBAUD ;MSB |
SPLK #0082h,SCI_LBAUD ;LSB |9600 Baud for sysclk 10MHz
SPLK #0022h,SCI_PORT_C2 ;I/O setting, SCI RX et TX pins disponibles
; SPLK #0052h,SCI_PORT_C2 ;I/O setting, SCI RX pin disponible, TX en I/O Output 5V pour HDSL7001
; SPLK #0012h,SCI_PORT_C2 ;I/O setting, SCI RX pin disponible, TX en I/O Output 0V (marche pas)
SPLK #0031h,SCI_CNTL1 ;end initialization et l'active : SW RESET=1
****************************************************************
* IOPC1 et 2 en output 5V pour config HSDL 7001 en 9600 bds
****************************************************************
LDP #00E1h
;_____IRDA______
SPLK #0034h, OPCRB ; mettre les CAP1 et 2 /QEP et IOPC2 ... sur I/O Port
SPLK #00606h, PCDATDIR ;IOPC1 et 2 as output et 5V
LACC PCDATDIR
; XINTj en input ... pas d'interruptions... car pattes en l'air
LDP #0E0h
LACC XINT1_CNTL
AND #0FFFEh
SACL XINT1_CNTL
LACC XINT2_CNTL
AND #0FFFEh
SACL XINT2_CNTL
LACC XINT3_CNTL
AND #0FFFEh
SACL XINT3_CNTL
; determine config ControlMode en fonction de Ref1 : <200h mode 0 V/f
; entre 200h et 300h mode 1 Cntr Vect I
; >300h mode 2 Cntr Vect Wm
LDP #00E0h ; DP=224 pour la configuration de l'ADC
splk #195Bh,ADCNTL1 ; 5 et 13 en Immediate SOC
RPT #3
NOP
Cnvf BIT ADCNTL1,BIT7
BCND Cnvf,TC
; Lecture de la référence _Ref1 sur la broche 5
LACC ADCFIFO1,10
LDP #_Ref1
SACH _Ref1
LACC _Ref1
AND #03FFh ; on ajoute le masque
SACL _Ref1
SUB #200h
BCND TestCM1,GEQ
SPLK #0,ControlMode
B fintestCM
TestCM1
LACC _Ref1
SUB #300h
BCND SetCM2,GEQ
SPLK #1,ControlMode
B fintestCM
SetCM2
SPLK #2,ControlMode
fintestCM
; on remet la config ADC start par EV sur 6 et 14
ldp #0E0h
splk #1B6Ch,ADCNTL1 ; cONFIGURATION DE L'adcntl1
splk #0403h,ADCNTL2 ; cONFIGURATION DE L'adcntl2
; PREPARE TO CONV. adc 1&2
lacc ADCFIFO1 ; CLEAR FIRST AND SECOND LEVEL adc fifo 1 & 2
lacc ADCFIFO1
lacc ADCFIFO2
lacc ADCFIFO2
*********************************************************
* SPI COnfig et DAC variable init
*********************************************************
LDP #WhichDAC
SPLK #0001h,WhichDAC
CS_LCDoff
CS_DACoff
SPI_Config 1 ; SPI pour DAC et LCD idem :-)
LACC SPI_BUF ; clear le SPI_STATUS,BIT6 : donnée_envoyé
;----------------------------------------------------------------
; initialisation de l'écran LCD
;----------------------------------------------------------------
; SPI_Config 2 ; SPI pour LCD config identique
LDP #LCDlo
SPLK #000h, LCDlo
SPLK #0C0h, LCDhi
DispLCD 1 ; foncé
; observers
LDP #00E1h
LACC PADATDIR
OR #1 ; A0= +5V
SACL PADATDIR
Attends 2000
LDP #LCDlo
SPLK #080h, LCDlo
SPLK #000h, LCDhi
DispLCD 1 ; clear LCD, 1.53 ms
Attends 2000
; observers
LDP #00E1h
LACC PADATDIR
AND #0FFFEh ; A0= 0V
SACL PADATDIR
; LDP #LCDlo
; SPLK #050h, LCDlo ; display off, cursor On
; SPLK #000h, LCDhi
; DispLCD 1
; Attends 2000
LDP #LCDlo
SPLK #060h, LCDlo ; increment, 1.53 ms
SPLK #000h, LCDhi
DispLCD 1
Attends 2000
LDP #LCDlo
SPLK #030h, LCDlo ; display on, cursor Off
SPLK #000h, LCDhi
DispLCD 1
Attends 2000
; LDP #LCDlo
; SPLK #080h, LCDlo
; SPLK #020h, LCDhi
; DispLCD 2 ; Char "A"
; Attends 500
; LDP #LCDlo
; SPLK #040h, LCDlo
; SPLK #020h, LCDhi
; DispLCD 2 ; Char "B"
; Attends 500
LDP #LCDupdate
SPLK #1, LCDupdate ; doit MAJ la table sur l'Ecran LCD
SPLK #0, LCDline ; REW au début de la table
SPLK #0, LCDcol
****************************
* Enable Interrupts
****************************
;Clear EV IFR and IMR regs
LDP #DP_EV
SPLK #07FFh,IFRA
SPLK #00FFh,IFRB
SPLK #000Fh,IFRC
;NO Timer Int ! only ADCINT voir plus bas
SPLK #0000h,IMRA ;PDPINT is disabled, with 0201h is enabled
SPLK #0000h,IMRB
SPLK #0000h,IMRC
LACC IVRA
LACC IVRB
LACC IVRC
;Set IMR and clear any Flags
LDP #0h
LACC #0FFh
SACL IFR
LACC #0100000b ; INT6 ADC int
SACL IMR
LDP #DP_EV
LACC T1CON
OR #40h ;Enable Timer 1 qu'à la fin !!!
SACL T1CON
SPM 00 ;?? ça stabilise
CLRC INTM ;enable all interrupts, now we may
;serve interrupts
****************************
* END Enable Interrupts
****************************
***************
* Virtual Menu
***************
;routine de lecture du port serie SCI pour les infos PC->DSP
MENU
; clrc xf ; TI
LDP #0E0h
BIT SCI_RX_STAT,BIT6 ;is there any character available ?
BCND MENU,ntc ;if not repeat the cycle (polling)
LACC SCI_RX_BUF
AND #0ffh ;only 8 bits !!!
LDP #option ;if yes, get it and store it in option
SACL option ;now in option we have the option number
;of the virtual menu
SUB #031h ;is it option 1 ?
BCND notone,neq ;if not branch to notone
*****************************
* Option 1): Speed Ref
*****************************
navail11
ldp #0E0h
BIT SCI_RX_STAT,BIT6 ;is there any character available ?
BCND navail11,ntc ;if not repeat the cycle (polling)
LACC SCI_RX_BUF
AND #07Fh ;prend les 7 LSB
LDP #serialtmp
SACL serialtmp
navail12
ldp #0E0h
BIT SCI_RX_STAT,BIT6 ;is there any character available ?
BCND navail12,ntc ;if not repeat the cycle (polling)
LACC SCI_RX_BUF,7 ;prend les 8 MSB
LDP #serialtmp
AND #03F80h ;prend les 7 MSB du High
ADD serialtmp
SACL _SpeedRef
;--debugg
; /Wmbase -> SALC n_ref
B MENU ; back to the main loop
*** END Option 1): Speed Ref
notone
B MENU ; back to the main loop
**********************************************************
* fin main part
**********************************************************
***********************************************************
* ADCint ISR *
* synchronization of the control algorithm with the PWM *
* EOC interrupt au milieur du motif MLI *
***********************************************************
ADCINT
************************
* Context Saving
************************
; deja sauvé par le moniteur
; sauf le AR6 !!!
LARP AR1
SAR AR6,*+ ;context save
CLRC CNF ; B0 for data memory, car le monitor le touche !!!
SPM 0 ;PM=00, no shift after multiplication
LDP #Task
LACC Task
BZ TaskAffichage
;-------------------------------------------------------
;-- else: Task=1 : Cmde Vect , Regul I, Calc Vitesse ---
;-------------------------------------------------------
SPLK #0, Task ; prochaine Task 0
; observers
LDP #00E1h
LACC PADATDIR
OR #1 ; debut Task 1 => A0= +5V
SACL PADATDIR
; ****************************
; * ACQUISITION DE MESURES *
; ****************************
; Lecture de la mesure sur la broche 6
LDP #00E0h ; DP=224
LACC ADCFIFO1,10
LDP #_Imes1
SACH _Imes1
LACC _Imes1
AND #03FFh ; on ajoute le masque
SACL _Imes1
; Lecture de la mesure sur la broche 14
LDP #00E0h ; DP=224
LACC ADCFIFO1,10
LACC ADCFIFO2,10
LDP #_Imes2
SACH _Imes2
LACC _Imes2
AND #03FFh ; on ajoute le masque
SACL _Imes2
; Configuration de ADCNTL1 : on va l'obliger à lire les références en broche 5 et 13
; Suite à cette opération on va reconfigurer l'ADC comme précédemmen
LDP #00E0h ; DP=224 pour la configuration de l'ADC
LACC ADCFIFO2,10 ; pour effacer
SPLK #195Bh,ADCNTL1 ; Configuration de l'ADCNTL1 : broche 5 et conversion immédiate
; RPT #3
; NOP attente introduite par du calcul de Kcurrent*Imes
; mise en forme des courants (2.5V offset, Koefficien et LEM DC)
LDP #_Imes1
LACC _Imes1
AND #03FFh ; on ajoute le masque
SUB #0200h ; on enlève l'offset de 2.5 V (courant pos ou negatif..)
SACL _Imes1
SPM 3 ;PM=11, 6 right shift after multiplication
LT _Imes1
MPY Kcurrent
PAC
SFR
SFR
SACL ia ;PM=11, +2 sfr= 8 right shift
; SPM 0 tjrs en 3
; SUB #112 ;then we subtract a DC offset (that should be zero, but it isn't)
; SACL ia ;sampled current ia, 4.12 format
LACC _Imes2
AND #03FFh ; on ajoute le masque
SUB #0200h ; on enlève l'offset de 2.5 V (courant pos ou negatif..)
SACL _Imes2
; SPM 3 ;PM=11, 6 right shift after multiplication
LT _Imes2
MPY Kcurrent
PAC
SFR
SFR
SACL ib ;PM=11, +2 sfr= 8 right shift
SPM 0
; ADD #-80 ;then we subtract a DC offset (that should be zero, but it isn't)
; SACL ib ;sampled current ia, 4.12 format
; Attente de la fin de la conversion
LDP #00E0h ; DP=224 pour la configuration de l'ADC
Conv_End BIT ADCNTL1,BIT7
BCND Conv_End,TC
; Lecture de la référence _Ref1 sur la broche 5
LACC ADCFIFO1,10
LDP #_Ref1
SACH _Ref1
LACC _Ref1
AND #03FFh ; on ajoute le masque
SACL _Ref1
; Lecture de la référence _Ref2 sur la broche 13
LDP #00E0h ; DP=224
LACC ADCFIFO1,10
LACC ADCFIFO2,10
LDP #_Ref2
SACH _Ref2
LACC _Ref2
AND #03FFh ; on ajoute le masque
SACL _Ref2
LDP #00E0h ; DP=224
LACC ADCFIFO2,10
; Réinitialisation de l'ADC : on repasse sur les broches 6 et 14
LDP #00E0h
SPLK #1B6Ch,ADCNTL1 ; Configuration initiale de l'ADCNTL1
;----- FIN DE L'ACQUISITION --------------------
;----- END Current sampling - AD conversions -----
;-------------------------------------------------
; Clarke transformation
; (a,b) -> (alpha,beta)
; calcule aussi ic...
; Ialphas = ia
; Ibetas = (2 * ib + ia) / sqrt(3)
;-------------------------------------------------
LDP #ia
lacc ia
sacl Ialphas ;iSalfa 4.12 format
add ib
neg
sacl ic
lacc ib,1 ;iSbeta = (2 * ib + ia) / sqrt(3)
add ia
sacl tmp
lt tmp
mpy #SQRT3inv ;SQRT3inv = (1 / sqrt(3)) = 093dh
;4.12 format = 0.577350269
pac
sach Ibetas,4;iSbeta 4.12 format
;------- END Clarke transformation ---------------
;-------------------------------------------------
; Measured speed and control
;-------------------------------------------------
;--- encoder pulses reading
LDP #DP_EV
LACC T3CNT ;we read the encoder pulses
SPLK #0000h,T3CNT
LDP #encincr
SACL encincr
;--- END Encoder pulses reading
;-------------------------------------------------
; Calculate speed and update reference speed variables
;-------------------------------------------------
LACC speedstep ;are we in speed control loop
;(SPEEDSTEP times current control loop)
SUB #1
SACL speedstep
BCND nocalc,GT ;if we aren't, skip speed calculation
;-------------------------------------------------
; Speed calculation from encoder pulses
;-------------------------------------------------
SPM 3 ;PM=11, 6 right shift after multiplication
LT speedtmp;multiply encoder pulses by Kspeed
;(8.8 format constant)
;to have the value of speed
MPY #Kspeed
PAC
SFR
SFR ;PM=11, +2 sfr= 8 right shift
SACL n ; vitesse en pu ie 1000h=1.0=1500 tr/mn
; calcule la vitesse en tr/mn pour l'affichage LCD
LT speedtmp
MPY #Kspeed_tr_mn
PAC
SFR
SFR ;PM=11, +2 sfr= 8 right shift
SACL n_tr_mn
LACC #0 ;zero speedtmp for next calculation
SACL speedtmp
LACC #SPEEDSTEP ;restore speedstep to the value
;SPEEDSTEP
SACL speedstep ;for next speed control loop
SPM 0 ;PM=00, no shift after multiplication
;--- END Speed calculation from encoder pulses --
;-------------------------------------------------
; Speed regulator with integral component correction
;-------------------------------------------------
LACC ControlMode
SUB #2 ; Speed regul ON
BNZ nocalc ; evite
; convertit nref_tr_mn en nref
SPM 3 ;PM=11, 6 right shift after multiplication
LT nref_tr_mn
MPY #Kspeed_tr_mn_pu
PAC
SFR
SFR ;PM=11, +2 sfr= 8 right shift
SACL n_ref
SPM 0 ;PM=00, no shift after multiplication
; LACC n_ref
;Regulateur de Vitesse : (n_ref-n) -> Iqsref
LACC n_ref
SUB n
SACL epin ;epin=n_ref-n, 4.12 format
LACC xin,12
LT epin
MPY Kpin
APAC
SACH upi,4 ;upi=xin+epin*Kpin, 4.12 format
;here we start to saturate
BIT upi,0
BCND upimagzeros,NTC ;If value >0 we branch
LACC #Iqsrefmin ;negative saturation
SUB upi
BCND neg_sat,GT ;if upi<ISqrefmin then branch to saturate
LACC upi ;value of upi is valid
B limiters
neg_sat
LACC #Iqsrefmin ;set acc to -ve saturated value
B limiters
upimagzeros ;Value is positive
LACC #Iqsrefmax ;positive saturation
SUB upi
BCND pos_sat,LT ;if upi>ISqrefmax then branch to saturate
LACC upi ;value of upi valid
B limiters
pos_sat
LACC #Iqsrefmax ;set acc to +ve saturated value
limiters
SACL Iqsref ;Store the acc as reference value
SUB upi
SACL elpi ;elpi=iSqref-upi, 4.12 format
LT elpi ;if there is no saturation elpi=0
MPY Kcorn
PAC
LT epin
MPY Kin
APAC
ADD xin,12
SACH xin,4 ;xin=xin+epin*Kin+elpi*Kcorn, 4.12 format
;--- END Speed regulator with integral component correction --
nocalc ;branch here if we don't have to calculate the speed
LACC speedtmp;use the actual encoder increment to ;update the
;increments accumulator used to calculate the speed
ADD encincr
SACL speedtmp
;----- END Measured speed -----------------
;-------------------------------------------------
; sinTeta_cm, cosTeta_cm calculation
;-------------------------------------------------
LDP #Teta_cm
; j'essaie l'AR6 mais je le sauve d'abord...
LARP AR6
LT Teta_cm ;current model rotor flux position
MPYU SR8BIT ; OK now plus de .data
PAC
SACH Index
LACC Index
AND #0ffh
ADD #SinusRam
SACL tmp
LAR AR6,tmp
LACC *
SACL sinTeta_cm ;sine Teta_cm value, 4.12 format
LACC Index ;The same for Cos ...
;cos(teta)=sin(teta+90ø)
ADD #40h ;90ø = 40h elements of the table
AND #0ffh
ADD #SinusRam
SACL tmp
LAR AR6,tmp
LACC *
SACL cosTeta_cm ;cosine Teta_cm value, 4.12 format
;-- END sinTeta_cm, cosTeta_cm calculation -------
;-------------------------------------------------
; Park transformation
; (alpha,beta)->(d,q)
; Ids=Ialphas*cos(Teta_cm)+Ibetas*sin(Teta_cm)
; Iqs=-Ialphas*sin(Teta_cm)+Ibetas*cos(Teta_cm)
;-------------------------------------------------
lt Ibetas
mpy sinTeta_cm
lta Ialphas
mpy cosTeta_cm
mpya sinTeta_cm
sach Ids,4 ;Ids 4.12 format
lacc #0
lt Ibetas
mpys cosTeta_cm
apac
sach Iqs,4 ;Iqs 4.12 format
;-------- END Park transformation ----------------
;-------------------------------------------------
; Current Model
;-------------------------------------------------
;-------------------------------------------------
; V/f open loop control ou Vector
;-------------------------------------------------
; attribure les Ref potar en fonction du ControlMode
LACC ControlMode
BNZ tstRefContrlVect_I_Wm
; ICI V/f open loop control
; fs et K_V_fs en entrée sur les _Refx
; LACC _Ref2,2 ; multiplie par 4
; SACL fs ; variation de la frequence
LACC _Ref2,3 ; multiplie par 8
SUB #0FF8h ; (1000h-8) pour avoir -49.9 Hz -> +50Hz
SACL fs
; multiplie par 4 puis par VDC pour avoir le K
; ou Vdsref si l'on veut une commande indépendante
; cmde indépendante
SPM 00 ;?? ça stabilise
LT VDC
MPY _Ref1
PAC
SACH K_V_fs,6 ; decal 4 bits et *2 pour qu'au maximum, il ne vale que 0.923 pu =VDC :-)
; cmde dépendante
; lt VDC
; mpy _Ref1
; pac
; sach vSdref,6 ; decal 4 bits et *2
; V/f : calcule Vsdref=K_V_fs*fs en p.u.
LT fs
MPY K_V_fs
PAC
SACH Vdsref,4
SPLK #0, Vqsref
B FinControlMode
; !!! saute la partie controle vect (I et Wm)
tstRefContrlVect_I_Wm
; attribure les Ref potar en fonction du ControlMode
LACC ControlMode
SUB #2
BNZ tstRefControlModeI
; ControlMode *Wm*
LACC _Ref1
;-- debug positif only + echelle Wmref
SACL nref_tr_mn
LACC _Ref2
SACL Idsref
B ContrlVect
tstRefControlModeI
LACC ControlMode
SUB #1
BNZ ContrlVect
; ControlMode *I*
LACC _Ref1
SFL
SUB #400h
;-- debug positif only
SACL Iqsref
LACC _Ref2
SACL Idsref
ContrlVect
; ICI Vector control : regul I (et meme Wm)
; calculde fs=p*Wm+Wsl ou plutot : fs=n+Kt*(iSq/i_mr)
;--debug
;--debug
; LACC iSd
; SUB i_mr
; SACL tmp
; LT tmp
; MPY #Kr
; PAC
; SACH tmp,4
; LACC tmp
; ADD i_mr
; SACL i_mr ;i_mr=i_mr+Kr*(iSd-i_mr), 4.12 f
; BCND i_mrnotzero,NEQ
; LACC #0
; SACL tmp ;if i_mr=0 then tmp=iSq/i_mr=0
; B i_mrzero
i_mrnotzero
;--- division (iSq/i_mr)
; LACC i_mr
LACC Ids
BCND i_mrzero,EQ
SACL tmp1
LACC Iqs
ABS
SACL tmp
LACC tmp,12
RPT #15
SUBC tmp1
SACL tmp ;tmp=Iqs/i_mr ou plutot Iqs/Ids
LACC Iqs
BCND Iqspositif,GT
LACC tmp
NEG
SACL tmp ;tmp=Iqs/i_mr, 4.12 format
Iqspositif
i_mrzero
;--- END division
LT tmp
MPY #Kt
PAC
SACH tmp,4 ;slip frequency, 4.12 format
LACC tmp ;load tmp in low ACC
ADD n
SACL fs ;rotor flux speed, 4.12 format,
;fs=n+Kt*(iSq/i_mr)
;; current regul / field weakening
;noFieldWeakening
; lacc #0666h ;Idsref=0.4 pu ie 1A pour 2.5 A de Ibase
; sacl Idsref ;Idsref 4.12 f without Field Weakening
;
;-------------------------------------------------------
; q-axis current regulator with integral component * correction
; (Iqsref-Iqs)->(Vqsref)
;-------------------------------------------------------
lacc Iqsref
sub Iqs
sacl epiq ;epiq=Iqsref-Iqs, 4.12 format
lacc xiq,12
lt epiq
mpy Kpi
apac
sach upi,4 ;upi=xiq+epiq*Kpi, 4.12 format
bit upi,0
bcnd upimagzeroq,NTC
lacc #Vmin
sub upi
bcnd neg_satq,GT ;if upi<Vmin branch to saturate
lacc upi ;value of upi is valid
b limiterq
neg_satq
lacc #Vmin ;set ACC to neg saturation
b limiterq
upimagzeroq ;Value was positive
lacc #Vmax
sub upi
bcnd pos_satq,LT ;if upi>Vmax branch to saturate
lacc upi ;value of upi is valid
b limiterq
pos_satq
lacc #Vmax ;set ACC to pos saturation
limiterq
sacl Vqsref ;Save ACC as reference value
sub upi
sacl elpi ;elpi=vSqref-upi, 4.12 format
lt elpi
mpy Kcor ;change to dma
pac
lt epiq
mpy Ki ;change to dma
apac
add xiq,12
sach xiq,4 ;xiq=xiq+epiq*Ki+elpi*Kcor, 4.12 f
;------- END q-axis regulator with integral component correction
;-------------------------------------------------------
; d-axis current regulator with integral component
; correction
; (Idsref-Ids)->(Vdsref)
;-------------------------------------------------------
lacc Idsref
sub Ids
sacl epid ;epid=Idsref-Ids, 4.12 format
lacc xid,12
lt epid
mpy Kpi
apac
sach upi,4 ;upi=xid+epid*Kpi, 4.12 format
bit upi,0
bcnd upimagzerod,NTC
lacc #Vmin
sub upi
bcnd neg_satd,GT ;if upi<Vmin branch to saturate
lacc upi ;upi value valid
b limiterd
neg_satd
lacc #Vmin ;set acc to neg saturation
b limiterd
upimagzerod ;value was positive
lacc #Vmax
sub upi
bcnd pos_satd,LT ;if upi>Vmax branch to saturate
lacc upi ;upi value valid
b limiterd
pos_satd
lacc #Vmax ;set acc to pos saturation
limiterd
sacl Vdsref ;store ACC as reference value
sub upi
sacl elpi ;elpi=vSdref-upi, 4.12 format
lt elpi
mpy Kcor
pac
lt epid
mpy Ki
apac
add xid,12
sach xid,4 ;xid=xid+epid*Ki+elpi*Kcor, 4.12 f
;------- END d-axis regulator with integral component correction
;--debug
;--debug
FinControlMode
LDP #fs
;--- rotor flux position calculation ---
LACC fs
ABS
SACL tmp
LT tmp
MPY #K
PAC
SACH tetaincr,4
BIT fs,0
BCND fs_neg,TC
LACC tetaincr
ADDS Teta_cm
SACL Teta_cm
B fs_pos
fs_neg
LACC Teta_cm
SUBS tetaincr
SACL Teta_cm
;Teta_cm=Teta_cm+K*fs=Teta_cm+tetaincr
;(0;360)<->(0;65535)
fs_pos
; RPT #3
RPT #4 ; (5 shifts to the right =>0 et 800h pour toute l'echelle)
SFR
SACL Teta_cm1;(0;360)<->(0;2048), ds notre cas
;used only for the visualization
;-------- END Current Model ----------------------
;-------- Inverse Park transformation ------------------
; (d,q) -> (alpha,beta)
; Vbetasref = Vqsref * cos(Teta_cm)+ Vdsref * sin(Teta_cm)
; Valphasref =-Vqsref * sin(Teta_cm) + Vdsref * cos(Teta_cm)
;-------------------------------------------------------
LACC #0
LT Vdsref
MPY sinTeta_cm
LTA Vqsref
MPY cosTeta_cm
MPYA sinTeta_cm
SACH Vbetasref,4
LACC #0
LT Vdsref
MPYS cosTeta_cm
APAC
SACH Valphasref,4
;-------- END Inverse Park transformation ------------------
; MLI calculée Lotfi
;-------------------------------------------------------
;-------- Clarke ---------------------------------------
; Vref1 = Valphasref
; Vref2 = (-Valphasref + sqrt(3) * Vbetasref) / 2
; Vref3 = (-Valphasref - sqrt(3) * Vbetasref) / 2
;-------------------------------------------------------
LDP #Valphasref
LT Vbetasref
MPY #SQRT32
PAC
SUB Valphasref,11
SACH Vref2,4 ;4.12 format
PAC
NEG
SUB Valphasref,11
SACH Vref3,4 ;4.12 format
LACC Valphasref
SACL Vref1 ;4.12 format
;-------- END reference voltage (Clarke inverse)------------
; bras phase a
LT VDCinvT
MPY Vref1
PAC
SACH tmp,4 ;tmp=VDCinvT*Vref1, 4.12 format
LACC #PWMPRD
SUB tmp
SFR
SACL taon ;taon=0.5*(PWMPRD-tmp) =0.5*(PWMPRD-VDCinvT*Vref1)
; bras phase b
LT VDCinvT
MPY Vref2
PAC
SACH tmp,4 ;tmp=VDCinvT*Vref2, 4.12 format
LACC #PWMPRD
SUB tmp
SFR
SACL tbon ;tbon=0.5*(PWMPRD-tmp) =0.5*(PWMPRD-VDCinvT*Vref2)
; bras phase c
LT VDCinvT
MPY Vref3
PAC
SACH tmp,4 ;tmp=VDCinvT*Vref3, 4.12 format
LACC #PWMPRD
SUB tmp
SFR
SACL tcon ;tcon=0.5*(PWMPRD-tmp) =0.5*(PWMPRD-VDCinvT*Vref3)
*** END taon,tbon and tcon calculation
; recopie des taon ds les cmp registers
BLDD taon,#CMPR1
BLDD taon,#SCMPR1
BLDD tbon,#CMPR2
BLDD tbon,#SCMPR2
BLDD tcon,#CMPR3
BLDD tcon,#SCMPR3
***********************************************
* END Pulse Width Modulation
***********************************************
; observers
LDP #00E1h
LACC PADATDIR
AND #0FFFEh ; fin Task 1 => => A0= 0V
SACL PADATDIR
B FinISR_ADCINT
;--- END Task=1 : Cmde Vect , Regul I, Calc Vitesse --
;-------------------------------------------
;-------- Task=0 : Affichage LCD/DAC -------
;-------------------------------------------
TaskAffichage
LDP #Task
SPLK #1, Task ; prochaine Task 1
; observers
LDP #00E1h
LACC PADATDIR
OR #8 ; debut Task 0 => A3= +5V
SACL PADATDIR
; essai pour voir l'update du dac
LDP #i
LACC j
; ADD #1000
ADD #200
SACL j
SUB #1000h
BCND pasdeRAZj,LT
splk #0, j
pasdeRAZj
CALL DACout ; toutes les 200us , sort sur le DAC 1 voie, puis mux...
;debug C/C le LCD
; B finLCD
; comme elle prend du temps c'est soit afficher 1 char,soit un ordre à l'LCD
; affiche la LCD table si besoin
LDP #LCDupdate
LACC LCDupdate
SUB #1
BNZ testhome
; if (LCDupdate==1) => Affiche 1 char
CALL AfficheLCDTable
B finLCD
testhome
LACC LCDupdate
SUB #2
BNZ testMAJ
; if (LCDupdate==2) => remise à la 1er ligne du curseur
LDP #LCDupdate
SPLK #0h, LCDupdate ; Cursor homed start
SPLK #040h, LCDlo ; Cursor home
SPLK #000h, LCDhi
DispLCD 1
B finLCD
; si zero alors demande la MAJ des I1 et I2
testMAJ
LACC LCDupdate
BNZ finLCD
; if (LCDupdate==0 ou autre) => le home s'est effectué
LDP #LCDRafj
LACC LCDRafj
SUB #1
SACL LCDRafj
BNZ finLCD
; demande le rafraichissement toutes les (500+32+1)*200 us
SPLK #LCDRafrate, LCDRafj
SPLK #1h, LCDupdate ; Cursor must go home
; test si le type de contrôle a changé, auquel cas, change l'affichage
LACC ControlMode
SUB ControlModeOld
BZ finrecopieMaskLCD
MAR *,AR6
LACC ControlMode
SACL ControlModeOld
SUB #2
BNZ testControlModeI
; recopie la LCDtable *Wm* de la flash vers la la ExtDM(.data en run)
LAR AR6, #LCDtable
RPT #31
BLPD #LCDtable_srcWm, *+
B finrecopieMaskLCD
testControlModeI
LACC ControlMode
SUB #1
BNZ testControlModeV_f
; recopie la LCDtable *I* de la flash vers la la ExtDM(.data en run)
LAR AR6, #LCDtable
RPT #31
BLPD #LCDtable_srcI, *+
B finrecopieMaskLCD
testControlModeV_f
; qqesoit ControlMode !=2 et !=1 alors V_f
; recopie la LCDtable *V/f* de la flash vers la la ExtDM(.data en run)
LAR AR6, #LCDtable
RPT #31
BLPD #LCDtable_src, *+
finrecopieMaskLCD
;--debug C/C rafraichissement memoire vidéo : LCDTable
; B finLCD
MAR *,AR6 ; AR6 déjà sauvé sur la pile pointée par AR1
LACC ControlMode
SUB #2
BNZ InfotestCtrl_I
; rafraichissement memoire vidéo pour Controle Wm
; courant Idsref
LAR AR6, #WmIdsrefoffs
LT Idsref
MPY M250
PAC
SACH Var,4
ConvDec Var
; afficher +2.5 ie "+c.d"
LACC signe
SACL *+
LACC centaine
SACL *+
LACC #02Eh ; "."
SACL *+
LACC dizaine
SACL *+
; vitesse mecanique Wmref
LAR AR6, #Wmrefoffs
LACC nref_tr_mn
SACL Var
ConvDec Var
; afficher "+mcdu"
LACC signe
SACL *+
LACC millier
SACL *+
LACC centaine
SACL *+
LACC dizaine
SACL *+
LACC unite
SACL *+
; courant Iqs
LAR AR6, #Iqsoffs
LT Iqs
MPY M250
PAC
SACH Var,4
ConvDec Var
; afficher +2.5 ie "+c.d"
LACC signe
SACL *+
LACC centaine
SACL *+
LACC #02Eh ; "."
SACL *+
LACC dizaine
SACL *+
; vitesse mecanique Wm
LAR AR6, #Wmoffs
LACC n_tr_mn
SACL Var
ConvDec Var
; afficher "+mcdu"
LACC signe
SACL *+
LACC millier
SACL *+
LACC centaine
SACL *+
LACC dizaine
SACL *+
LACC unite
SACL *+
B finInfotestCtrl
InfotestCtrl_I
LACC ControlMode
SUB #1
BNZ InfotestCtrl_V_f
; rafraichissement memoire vidéo pour Controle I
; courant Iqsref
LAR AR6, #Iqsrefoffs
LT Iqsref
MPY M250
PAC
SACH Var,4
ConvDec Var
; afficher +2.5 ie "+c.d"
LACC signe
SACL *+
LACC centaine
SACL *+
LACC #02Eh ; "."
SACL *+
LACC dizaine
SACL *+
; courant Idsref
LAR AR6, #Idsrefoffs
LT Idsref
MPY M250
PAC
SACH Var,4
ConvDec Var
; afficher +2.5 ie "+c.d"
LACC signe
SACL *+
LACC centaine
SACL *+
LACC #02Eh ; "."
SACL *+
LACC dizaine
SACL *+
; courant Iqs
LAR AR6, #Iqsoffs
LT Iqs
MPY M250
PAC
SACH Var,4
ConvDec Var
; afficher +2.5 ie "+c.d"
LACC signe
SACL *+
LACC centaine
SACL *+
LACC #02Eh ; "."
SACL *+
LACC dizaine
SACL *+
; vitesse mecanique Wm
LAR AR6, #Wmoffs
LACC n_tr_mn
SACL Var
ConvDec Var
; afficher "+mcdu"
LACC signe
SACL *+
LACC millier
SACL *+
LACC centaine
SACL *+
LACC dizaine
SACL *+
LACC unite
SACL *+
B finInfotestCtrl
InfotestCtrl_V_f
; qqesoit ControlMode !=2 et !=1 alors V_f
; rafraichissement memoire vidéo pour Controle V_f
; Vdsref
LAR AR6, #Vdsrefoffs
LT Vdsref
MPY M300
PAC
SACH Var,4
ConvDec Var
; afficher +300 ie "+cdu"
LACC signe
SACL *+
LACC centaine
SACL *+
LACC dizaine
SACL *+
LACC unite
SACL *+
; frequence fs
LAR AR6, #fsoffs
; ConvHexa fs, 4
; ConvHexa fs, 8
; ConvHexa fs, 12
; ConvHexa fs, 16
; ----- afiche en 4 char decimaux :
LT fs
MPY M5000 ;neg ou positif
PAC
SACH Var,4
ConvDec Var
; afficher +50.0 ie "+mc.d"
LACC signe
SACL *+
LACC millier
SACL *+
LACC centaine
SACL *+
LACC #02Eh ; "."
SACL *+
LACC dizaine
SACL *+
; Constante K_V_f
LAR AR6, #Koffs
LACC K_V_fs
SACL Var
ConvDec Var
; afficher "+mcdu"
MAR *+ ; evite le signe +
LACC millier
SACL *+
LACC centaine
SACL *+
LACC dizaine
SACL *+
LACC unite
SACL *+
; vitesse mecanique Wm
LAR AR6, #Wmoffs
LACC n_tr_mn
SACL Var
ConvDec Var
; afficher "+mcdu"
LACC signe
SACL *+
LACC millier
SACL *+
LACC centaine
SACL *+
LACC dizaine
SACL *+
LACC unite
SACL *+
finInfotestCtrl
finLCD
; observers
LDP #00E1h
LACC PADATDIR
AND #0FFF7h ; fin Task 0 => A3= 0V
SACL PADATDIR
;--- END Task=0 : Affichage LCD/DAC -------
FinISR_ADCINT
*-- rajouté (ou modifié) par lotfi 06/03-----------
LDP #0E0h
LACC SYSIVR
ldp #0h ;
LACC IFR ;
sacl IFR ; clear interrupt flags (int1 par ADCint)
;---- fin rajout
;-------------------------------------------
; restore status registers à la manière IMMC monitor :-(
;-------------------------------------------
LARP AR1 ; ARP=1
MAR *- ; AR1--
LAR AR6,*- ;restore the one used for sin table access
ZALS *- ; restore ACCL
ADDH *- ; restore ACCH
LST #0, *- ; load ST0
LST #1, *- ; load ST1
CLRC INTM
RET
;---- END Context Restore and Return ------
;-------------------------------------------
;- routine d'envoi de donnees sur SPI pour DAC (elle envoie HI et lo)
;-------------------------------------------
DACout
; sort les references sur le SPI
; NewData : inutile car l'envoi est controlé uniquement ici
; LDP #Vdsref
; LACC Vref1
; ADD #01000h
; SFR
; SFR
; spyDAC
; LDP #Idsref
; LACC Idsref
; ADD #00800h
; SACL DAC1
LDP #n_ref
LACC n_ref
ADD #01000h
SFR
SACL DAC1
; LDP #n
; LACC n
; ADD #01000h
; SFR
; SFR
; SFR
; SFR
; spyDAC
; spyDAC
; LACC j
; LDP #ids
LACC n
ADD #01000h
SFR
SACL DAC2
; LDP #fs
; LACC fs
; ADD #01000h
; SFR
; SFR
; SACL DAC2
; LDP #sinTeta_cm
; LACC sinTeta_cm
; ADD #01000h
; SFR
; SACL DAC3
; SUB #01000h
; BCND neSatpas, LT
; SPLK #00FFFh, DAC3 ;à cause de 1000h (qd ça depasse d'un bit)
;neSatpas
;
; LDP #Teta_cm1
; LACC Teta_cm1
; SACL DAC4
;LDP #iqs
LACC Iqsref
ADD #00800h
SACL DAC3
;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;
;LDP #iqs
LACC Iqs
ADD #00800h
SACL DAC4
; roulement de tambour ehhh de DAC
LACC WhichDAC
ADD #1 ; la prochaine fois, sort le suivant
SACL WhichDAC
SUB #5
BCND NoRaz, LT
SPLK #1, WhichDAC
NoRaz
; debug
; SPLK #1, WhichDAC
; ici on arrive avec DAC1..4
LDP #WhichDAC
LACC WhichDAC
SUB #1
BNZ test2
SPLK #0010h, DAC_OR
LACC DAC1 ; Load data du DAC 1 into the ACC
SACL DataDAC
B fintest
test2 LACC WhichDAC
SUB #2
BNZ test3
SPLK #0050h, DAC_OR
LACC DAC2 ; Load data du DAC 2 into the ACC
SACL DataDAC
B fintest
test3 LACC WhichDAC
SUB #3
BNZ test4
SPLK #0090h, DAC_OR
LACC DAC3 ; Load data du DAC 3 into the ACC
SACL DataDAC
B fintest
test4 ;inutile de tester c'est un 4 !
SPLK #00D0h, DAC_OR
LACC DAC4 ; Load data du DAC 4 into the ACC
SACL DataDAC
fintest
; SPI_Config 1 ; SPI pour DAC inutile car la même
CS_DACon ; Active le DAC
; sort sur DAC numero DAC_OR
LDP #0E0h
LACC SPI_BUF ; clear le SPI_STATUS,BIT6 : donnée_envoyé
LACC SPI_PORT_C1 ; SPISTE à 0 to start the transfer
AND #0BFh
SACL SPI_PORT_C1
LDP #DataDAC
; version plus rapide
LACC DataDAC,8 ; Load data du DataDAC into the Hi ACC
SACH tmp
LACC tmp
AND #000Fh ; Set the DAC control bits (4-7) ou plutot D12-D15
OR DAC_OR ; DAC voie "DAC_OR", SPD=fast mode
LDP #0E0h
SACL SPI_DAT ; Transmit HI byte
att1 BIT SPI_STATUS,BIT6
BCND att1,NTC
LACC SPI_BUF ; clear le SPI_STATUS,BIT6 : donnée_envoyé
;low_byte
LDP #DataDAC
LACC DataDAC ; Read the current value of
AND #00FFh ; data, mask off the lower
LDP #0E0h
SACL SPI_DAT ; Transmit the LOW byte
; transfert fini ?
att2 BIT SPI_STATUS,BIT6
BCND att2,NTC
LACC SPI_BUF ; clear le SPI_STATUS,BIT6 : donnée_envoyé
LACC SPI_PORT_C1 ; en fait : set SPIPC1.6 à 1 => SPISTE pin à 1 ie latch the Data !
OR #040h
SACL SPI_PORT_C1 ; latch the data into the DAC
CS_DACoff ; Desactive le DAC
RET
;-- fin de la routine d'envoi de donnees sur SPI (pour DAC)
;-------------------------------------------
; Routine d'envoi de donnees sur SPI (pour LCD)
; met à jour la LCDtable sur l'écran
;-------------------------------------------
AfficheLCDTable
; SPI_Config 2 ; SPI pour LCD inutile car la même
LARP AR6 ; déjà sauvé sur la pile pointée par AR1
LDP #LCDcol
LACC LCDline,4 ; LCDline*16
AND #010h
ADD LCDcol
ADD #LCDtable
SACL tmp
LAR AR6,tmp
LACC *
SACL LCDChar
trChar
DispLCD 2 ; sortir le Char actuel
; Attends 500 inutile car routine ttes les 200 us !
LDP #LCDupdate
LACC LCDcol ; col suivante
ADD #1
SACL LCDcol
SUB #16
RETC LT
ZAC
SACL LCDcol
LACC LCDline ; ligne suivante
ADD #1
SACL LCDline
SUB #2
RETC LT
ZAC ; fini d'updater et RAZ au début du tableau
SACL LCDline
SACL LCDcol
LACC #2 ; demande de remise à la 1er ligne du curseur
SACL LCDupdate
RET
;--- fin de la routine d'envoi de donnees sur SPI (pour LCD)
;-------------------------------------------
; trap some phantom interrupt
Phantom2
B Phantom2
;-------------------------------------------
