soft_svpwm.asm

f2407:电机控制

		MAR		*-					;sector=3, t1=-Z, t2=X
									;STACK:t2/u_alfa/ *p/address/AR0/-vref1/-t1/Y/sector_srx 
									;ARP:AR0, AR0:-t1, AR2:t2, AR3:PWMPRD		
		LACC	*-,AR2				;ARP:AR2, AR0:-vref1, AR2:t2, AR3:PWMPRD
		NEG
		ADD		*,AR0	   			;t1+t2            
		SACL	*+,AR3				;STACK:t2/u_alfa/ *p/address/AR0/t1+t2/-t1/Y/sector_srx 
									;ARP:AR0, AR0:-t1, AR2:t2, AR3:PWMPRD
		SUB		*,AR0             		
		BCND	softsvpwm6,LEQ                                          
		MAR		*-,AR3				;ARP:AR3, AR0:t1+t2, AR2:t2, AR3:PWMPRD 
        LACC	*,15,AR0	 	  	;SATURATION
        							;ARP:AR0 
        RPT		#15
        SUBC	*					;CALCULATE PWMPRD/t1+t2, Q15
        SACL	* 				    ;STACK:t2/u_alfa/ *p/address/AR0/(PWMPRD/t1+t2)/-t1/Y/sector_srx 
									;ARP:AR0, AR0:(PWMPRD/t1+t2), AR2:t2, AR3:PWMPRD        
        LT		*+               	;(PWMPRD/t1+t2)-->TREG
        							;ARP:AR0, AR0:-t1, AR2:t2, AR3:PWMPRD
        MPY		*					;AR0:-t1  					
		SPH		*,AR2               ;t1=t1*T_pwm/(t1+t2)
							        ;STACK:t2/u_alfa/ *p/address/AR0/(PWMPRD/t1+t2)/-t1/Y/sector_srx 
									;ARP:AR2, AR0:-t1, AR2:t2, AR3:PWMPRD
        MPY		*					
        SPH		*,AR0              	;t2=t2*T_pwm/(t1+t2)
        							;STACK:t2/u_alfa/ *p/address/AR0/(PWMPRD/t1+t2)/-t1/Y/sector_srx 
									;ARP:AR0, AR0:-t1, AR2:t2, AR3:PWMPRD 
softsvpwm6
		LACC	*,15,AR2  			;-t1-->ACC
									;ARP:AR2  
		SUB		*,15,AR3			;-t1-t2
		ADD		*+,15				;taon=(WMPRD-t1-t2)/2
									;ARP:AR3, AR0:-t1, AR2:t2, AR3:va				
		SACH	*+,AR0				;va=taon 
		SUB		*,16,AR3			;tbon=taon-(-t1) 
									;ARP:AR3, AR3:vb
		SACH	*+,AR2				;vb=tbon
									;ARP:AR2
		ADD		*,16,AR3			;tcon=tbon+t2
									;ARP:AR3, AR3:vc
		SACH	*,AR1				;vc=tcon
		B		softsvpwm_end

sector_sr4
		MAR		*-					;sector=4, t1=-X, t2=Z
									;STACK:-t1/u_alfa/ *p/address/AR0/-vref1/t2/Y/sector_srx 
									;ARP:AR0, AR0:t2, AR2:-t1, AR3:PWMPRD
		LACC	*-,AR2				;ARP:AR2, AR0:-vref1, AR2:-t1, ARP:PWMPRD
		SUB		*,AR0		        ;t2-(-t1)
		SACL	*+,AR3				;STACK:-t1/u_alfa/ *p/address/AR0/t1+t2/t2/Y/sector_srx
									;ARP;AR3, AR0:t2, AR2:-t1, AR3:PWMPRD
		SUB		*,AR0				;ARP:AR0
		BCND	softsvpwm7,LEQ		
		MAR		*-,AR3				;ARP:AR3, AR0:t1+t2, AR2:-t1, AR3:PWMPRD 
        LACC	*,15,AR0	 	  	;SATURATION
        							;ARP:AR0 
        RPT		#15
        SUBC	*					;CALCULATE PWMPRD/t1+t2, Q15
        SACL	* 				    ;STACK:-t1/u_alfa/ *p/address/AR0/(PWMPRD/t1+t2)/t2/Y/sector_srx 
									;ARP:AR0, AR0:(PWMPRD/t1+t2), AR2:-t1, AR3:PWMPRD        
        LT		*+               	;(PWMPRD/t1+t2)-->TREG
        							;ARP:AR0, AR0:t2, AR2:-t1, AR3:PWMPRD
        MPY		*					;AR0:t2  					
		SPH		*,AR2               ;t2=t2*T_pwm/(t1+t2)
							        ;STACK:-t1/u_alfa/ *p/address/AR0/(PWMPRD/t1+t2)/t2/Y/sector_srx 
									;ARP:AR2, AR0:t2, AR2:-t1, AR3:PWMPRD
        MPY		*					
        SPH		*,AR0              	;t1=t1*T_pwm/(t1+t2)
        							;STACK:-t1/u_alfa/ *p/address/AR0/(PWMPRD/t1+t2)/t2/Y/sector_srx 
									;ARP:AR0, AR0:t2, AR2:-t1, AR3:PWMPRD
softsvpwm7
		LACC	*,15,AR2
		SUB		*,15,AR3			;t2-(-t1)
		SUB		*+,15				
		NEG							;taon=(PWMPRD-t2-t1)/2
									;ARP:AR3, AR0:t2, AR2:-t1, AR3:va
		ADRK	#2					;AR3:vc
		SACH	*-,AR2				;vc=taon
									;ARP:AR2, AR2:-t1, AR3:vb
		SUB		*,16,AR3            ;tbon=taon-(-t1)
									;ARP:AR3, AR3:vb
		SACH	*-,AR0              ;vb=tbon
									;ARP:AR0, AR0:t2, AR3:va
		ADD		*,16,AR3			;tcon=tbon+t2
		SACH	*,AR1				;va=tcon
									;ARP:AR1
		B		softsvpwm_end
		
sector_sr5
		LACC	*+,AR2				;sector=5, t1=X, t2=-Y
									;STACK:t1/u_alfa/ *p/address/AR0/-vref1/Z/-t2/sector_srx 
		SUB		*,AR0
		NEG							;t1+t2					
		SACL	*-,AR3				;STACK:t1/u_alfa/ *p/address/AR0/-vref1/Z/-t2/t1+t2
									;ARP:AR3, AR0:-t2, AR1:t1, AR3:PWMPRD
		SUB		*,AR0
		BCND	softsvpwm8,LEQ		
		MAR		*+,AR3				;ARP:AR3, AR0:t1+t2, AR2:t1, AR3:PWMPRD 
        LACC	*,15,AR0	 	  	;SATURATION
        							;ARP:AR0 
        RPT		#15
        SUBC	*					;CALCULATE PWMPRD/t1+t2, Q15
        SACL	* 				    ;STACK:t1/u_alfa/ *p/address/AR0/-vref1/Z/-t2/(PWMPRD/t1+t2) 
									;ARP:AR0, AR0:(PWMPRD/t1+t2), AR2:t1, AR3:PWMPRD        
        LT		*-               	;(PWMPRD/t1+t2)-->TREG
        							;ARP:AR0, AR0:-t2, AR2:t1, AR3:PWMPRD
        MPY		*					;AR0:-t2  					
		SPH		*,AR2               ;t2=t2*T_pwm/(t1+t2)
							        ;STACK:t1/u_alfa/ *p/address/AR0/-vref1/Z/-t2/(PWMPRD/t1+t2) 
									;ARP:AR2, AR0:-t2, AR2:t1, AR3:PWMPRD
        MPY		*					
        SPH		*,AR0              	;t1=t1*T_pwm/(t1+t2)
        							;STACK:t1/u_alfa/ *p/address/AR0/-vref1/Z/-t2/(PWMPRD/t1+t2) 
									;ARP:AR0, AR0:-t2, AR2:t1, AR3:PWMPRD
softsvpwm8
		LACC	*,15,AR2
		SUB		*,15,AR3			;-t2-t1
		ADD		*+,15				;taon=(PWMPRD-t1-t2)/2
									;ARP:AR3, AR0:-t2, AR2:t1, AR3:va
		ADRK	#1   				;AR3:vb
		SACH	*+,AR2				;vb=taon
									;ARP:AR2, AR3:vc
		ADD		*,16,AR3			;tbon=taon+t1
									;ARP:AR3, AR3:vc
		SACH	*-					;vc=tbon                
									;AR3:vb
		MAR		*-,AR0				;ARP:AR0, AR0:-t2, AR3:va
		SUB		*,16,AR3  			;tcon=tbon-(-t2)
									;ARP:AR3
		SACH	*,AR1				;va=tcon
									;ARP:AR1
		B		softsvpwm_end

sector_sr6
		LACC	*-					;sector=5, t1=-Y, t2=-Z
									;STACK:X/u_alfa/ *p/address/AR0/-vref1/-t2/-t1/sector_srx	
		                            ;ARP:AR0, AR0:-t2, AR2:X, AR3:PWMPRD
		ADD		*-		            ;-t1-t2  
									;ARP:AR0, AR0:-vref1, AR2:X, AR3:PWMPRD
		NEG							;t1+t2
		SACL	*+,AR3              ;STACK:X/u_alfa/ *p/address/AR0/t1+t2/-t2/-t1/sector_srx
									;ARP:AR0, AR0:-t2, AR3:PWMPRD
		SUB		*,AR0		        ;t1+t2-PWMPRD
		BCND	softsvpwm9,LEQ
		MAR		*-,AR3				;ARP:AR3, AR0:t1+t2, AR3:PWMPRD 
        LACC	*,15,AR0	 	  	;SATURATION
        							;ARP:AR0 
        RPT		#15
        SUBC	*					;CALCULATE PWMPRD/t1+t2, Q15
        SACL	* 				    ;STACK:X/u_alfa/ *p/address/AR0/(PWMPRD/t1+t2)/-t2/-t1/sector_srx 
									;ARP:AR0, AR0:(PWMPRD/t1+t2), AR3:PWMPRD        
        LT		*+               	;(PWMPRD/t1+t2)-->TREG
        							;ARP:AR0, AR0:-t2, AR3:PWMPRD
        MPY		*					;AR0:-t2  					
		SPH		*+		            ;t2=t2*T_pwm/(t1+t2)
							        ;STACK:X/u_alfa/ *p/address/AR0/(PWMPRD/t1+t2)/-t2/-t1/sector_srx 
									;ARP:AR0, AR0:-t1, AR3:PWMPRD
        MPY		*					
        SPH		*-	              	;t1=t1*T_pwm/(t1+t2)
        							;STACK:X/u_alfa/ *p/address/AR0/(PWMPRD/t1+t2)/-t2/-t1/sector_srx 
									;ARP:AR0, AR0:-t2, AR3:PWMPRD
softsvpwm9
		LACC	*+,15				
		ADD		*,15,AR3			;-t1-t2
		ADD		*+,15				;taon=(PWMPRD-t1-t2)/2
									;ARP:AR3, AR0:-t1, AR3:va
		ADRK	#2                  ;ARP:AR3, AR3:vc
		SACH	*-,AR0				;vc=taon
									;ARP:AR0, AR0:-t1, AR3:vb        
		SUB		*-,16,AR3			;tbon=taon-(-t1) 
									;ARP:AR3, AR0:-t2, AR3:vb
		SBRK	#1
		SACH	*+,AR0				;va=tbon
									;ARP:AR0, AR0:-t2, AR3:vb
		SUB		*,16,AR3			;tcon=tbon-(-t2)
		SACH	*,AR1				;vb=tcon
									;ARP:AR1
softsvpwm_end
		SPM		0					;PREG SHIFT MODE DISABLE
		SBRK	#_soft_svpwm_frame+1
		LAR		AR0,*-
		PSHD	*
		RET
		
		
sectable:
sect0	.word	mute				;error, reset
sect1	.word	sector_sr1         	;sector2
sect2	.word	sector_sr2         	;sector6
sect3	.word	sector_sr3         	;sector1
sect4	.word	sector_sr4         	;sector4
sect5	.word	sector_sr5         	;sector3
sect6	.word	sector_sr6         	;sector5
sect7	.word	mute               	;error, reset
				
		.end