forked from KolibriOS/kolibrios
329 lines
17 KiB
NASM
329 lines
17 KiB
NASM
|
;throttle_backemf.asm
|
||
|
|
|
||
|
|
.NOLIST
|
||
|
|
; ***************************************************************************************
|
||
|
|
; * PWM MODEL RAILROAD THROTTLE *
|
||
|
|
; * *
|
||
|
|
; * WRITTEN BY: PHILIP DEVRIES *
|
||
|
|
; * *
|
||
|
|
; * Copyright (C) 2003 Philip DeVries *
|
||
|
|
; * *
|
||
|
|
; * This program is free software; you can redistribute it and/or modify *
|
||
|
|
; * it under the terms of the GNU General Public License as published by *
|
||
|
|
; * the Free Software Foundation; either version 2 of the License, or *
|
||
|
|
; * (at your option) any later version. *
|
||
|
|
; * *
|
||
|
|
; * This program is distributed in the hope that it will be useful, *
|
||
|
|
; * but WITHOUT ANY WARRANTY; without even the implied warranty of *
|
||
|
|
; * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the *
|
||
|
|
; * GNU General Public License for more details. *
|
||
|
|
; * *
|
||
|
|
; * You should have received a copy of the GNU General Public License *
|
||
|
|
; * along with this program; if not, write to the Free Software *
|
||
|
|
; * Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA *
|
||
|
|
; * *
|
||
|
|
; ***************************************************************************************
|
||
|
|
.LIST
|
||
|
|
|
||
|
|
.ifdef BACKEMF_ENABLED
|
||
|
|
;********************************************************************************
|
||
|
|
;* BACKEMF_ADJUST *
|
||
|
|
;* Top level routine *
|
||
|
|
;* *
|
||
|
|
;* The throttle_set is compared against the back emf generated by the motor *
|
||
|
|
;* and adjusted to reduce the error *
|
||
|
|
;* *
|
||
|
|
;* Inputs: throttle_set Target speed *
|
||
|
|
;* Returns: throttle_set Adjusted target speed *
|
||
|
|
;* Changed: error_hi Adjusted throttle, upper 8 bits (local) *
|
||
|
|
;* error_lo Adjusted throttle, lower 8 bits (local) *
|
||
|
|
;* error_hi_prev Previous throttle, for filter (global) *
|
||
|
|
;* error_lo_prev Previous throttle, for filter (global) *
|
||
|
|
;* emf_hi Measured emf, upper 8 bits (local) *
|
||
|
|
;* emf_lo Measured emf, lower 8 bits (local) *
|
||
|
|
;* B_Temp,B_Temp1 *
|
||
|
|
;* Calls: ADC_SETUP_EMF *
|
||
|
|
;* div16u *
|
||
|
|
;* DIVIDE_16_SIMPLE *
|
||
|
|
;* Goto: none *
|
||
|
|
;********************************************************************************
|
||
|
|
|
||
|
|
HILOCAL1 error_lo ; assign local variables
|
||
|
|
HILOCAL2 error_hi
|
||
|
|
|
||
|
|
;*****************************************************************
|
||
|
|
;*Convert throttle setting into 2 byte 2's compl. *
|
||
|
|
;* *
|
||
|
|
;* This is a 7 bit number plus 1 more bits after the radix *
|
||
|
|
;* It is in (error_hi) -radix- (error_lo) *
|
||
|
|
;*****************************************************************
|
||
|
|
mov error_hi,throttle_set ; Put throttle into 16 bit form
|
||
|
|
clr error_lo
|
||
|
|
|
||
|
|
lsr error_hi ; Convert to 2's compliment
|
||
|
|
ror error_lo
|
||
|
|
|
||
|
|
;********************************************************************************
|
||
|
|
;* READ_BACKEMF *
|
||
|
|
;* Returns a 2 byte 2's compliment measurement of the motor backemf. *
|
||
|
|
;* *
|
||
|
|
;* 1. Add together 8 samples of the (8 bit) pwm value in the two byte *
|
||
|
|
;* emf_hi--emf_lo register. *
|
||
|
|
;* 2. Multiply by 16. *
|
||
|
|
;* 3. Result: Minimum value = 0x000 (decimal 0) *
|
||
|
|
;* Maximum value = 0x7F8 (decimal 2040) *
|
||
|
|
;* *
|
||
|
|
;* Time required: *
|
||
|
|
;* 1. 1st Sample: 125uS *
|
||
|
|
;* 2. next 7 Samples: 455uS *
|
||
|
|
;* 3. balance of Subroutine: 10's of uS *
|
||
|
|
;* TOTAL 580uS min *
|
||
|
|
;* *
|
||
|
|
;* Each cycle of the 25kHz PWM takes 40uS, therefore, this routine takes *
|
||
|
|
;* at least 14.5 cycles of the 25kHz pwm. *
|
||
|
|
;* *
|
||
|
|
;* Inputs: None *
|
||
|
|
;* Returns: emf_hi--emf_lo: 2 Byte 2's compl (but always positive) *
|
||
|
|
;* measure of motor backemf. *
|
||
|
|
;* Changed: B_Temp,B_Temp1 *
|
||
|
|
;* Calls: ADC_SETUP_EMF *
|
||
|
|
;********************************************************************************
|
||
|
|
LOWLOCAL1 emf_hi ; Names of local registers
|
||
|
|
LOWLOCAL2 emf_lo
|
||
|
|
|
||
|
|
;READ_BACKEMF:
|
||
|
|
rcall ADC_SETUP_EMF ; Setup ADC to measure back_emf.
|
||
|
|
clr emf_lo ; Clear the value of emf.
|
||
|
|
clr emf_hi
|
||
|
|
|
||
|
|
ldi B_Temp,8 ; Add 8 samples
|
||
|
|
WAIT_FOR_EMF_MEASURE: ; Wait for a measurement of the EMF
|
||
|
|
sbis ADCSR,ADIF
|
||
|
|
rjmp WAIT_FOR_EMF_MEASURE
|
||
|
|
|
||
|
|
in B_Temp1,ADCH ; Read the measurement
|
||
|
|
|
||
|
|
sbi ADCSR,4 ; Clear the interrupt
|
||
|
|
|
||
|
|
add emf_lo,B_Temp1 ; Add to low byte (no carry)
|
||
|
|
|
||
|
|
clr B_Temp1
|
||
|
|
adc emf_hi,B_Temp1 ; Add carry to high byte.
|
||
|
|
|
||
|
|
dec B_Temp
|
||
|
|
brne WAIT_FOR_EMF_MEASURE ; Measure for complete set
|
||
|
|
; Sum of 8 samples.
|
||
|
|
|
||
|
|
ldi B_Temp,4 ; Convert 11 bit number into a 15 bit
|
||
|
|
COMPUTE_EMF_AVERAGE: ; number (only 11 significant figures
|
||
|
|
lsl emf_lo ; though)
|
||
|
|
rol emf_hi
|
||
|
|
dec B_Temp
|
||
|
|
brne COMPUTE_EMF_AVERAGE
|
||
|
|
|
||
|
|
;*****************************************************************
|
||
|
|
;* Compute the error. That is, throttle = throttle - emf *
|
||
|
|
;* *
|
||
|
|
;* The result is a two byte number (signed two's compl) in *
|
||
|
|
;* error_hi -radix- error_lo *
|
||
|
|
;*****************************************************************
|
||
|
|
sub error_lo,emf_lo ; subtract low bytes (after radix)
|
||
|
|
sbc error_hi,emf_hi ; subtract high bytes (before radix)
|
||
|
|
|
||
|
|
.ifdef BACKEMF_SCALE_ENABLED
|
||
|
|
;*****************************************************************
|
||
|
|
;* Error multiplier (complex) *
|
||
|
|
;* *
|
||
|
|
;* Error gain is equal to: *
|
||
|
|
;* *
|
||
|
|
;* Error err_scale err_mult *
|
||
|
|
;* ------------------------ * 2 * 2 *
|
||
|
|
;* err_scale *
|
||
|
|
;* 2 + throttle_set *
|
||
|
|
;* *
|
||
|
|
;* The maximum gain when throttle_set = 0 is 2^err_mult *
|
||
|
|
;* is cut in half when throttle_set = 2^err_scale *
|
||
|
|
;* *
|
||
|
|
;* Result is signed two's compliment in *
|
||
|
|
;* error_hi--error_lo -radix- *
|
||
|
|
;*****************************************************************
|
||
|
|
cbr Flags_1,F_negative_err ; Assume error is positive
|
||
|
|
|
||
|
|
sbrs error_hi,7 ; Test algebraic sign
|
||
|
|
rjmp POSITIVE_ERR
|
||
|
|
|
||
|
|
sbr Flags_1,F_negative_err ; If error is negative, set flag.
|
||
|
|
com error_lo ; Convert to positive
|
||
|
|
com error_hi
|
||
|
|
subi error_lo,0xFF
|
||
|
|
sbci error_hi,0xFF
|
||
|
|
|
||
|
|
POSITIVE_ERR:
|
||
|
|
|
||
|
|
B_TEMPLOCAL _bemf_lo_byte
|
||
|
|
B_TEMPLOCAL1 _bemf_hi_byte
|
||
|
|
|
||
|
|
mov _bemf_lo_byte,throttle_set ; Divisor = throttle_set+2^err_scale
|
||
|
|
clr _bemf_hi_byte
|
||
|
|
ldi B_Temp2,EXP2(err_scale)
|
||
|
|
add _bemf_lo_byte,B_Temp2
|
||
|
|
adc _bemf_hi_byte,_bemf_hi_byte
|
||
|
|
|
||
|
|
; mov dd16uL,error_lo ; Dividend = error (same register)
|
||
|
|
; mov dd16uH,error_hi
|
||
|
|
|
||
|
|
rcall div16u ; Divide error by (throttle+offset)
|
||
|
|
; (almost 4 pwm cycles)
|
||
|
|
; adds 3 to Cycle_count
|
||
|
|
|
||
|
|
; mov error_lo,dres16uL ; Same register
|
||
|
|
; mov error_hi,dres16uH
|
||
|
|
|
||
|
|
sbrs Flags_1,BF_negative_err ; Check sign flag
|
||
|
|
rjmp POSITIVE_ERR_1
|
||
|
|
|
||
|
|
com error_lo ; Convert back to negative
|
||
|
|
com error_hi ; if necessary
|
||
|
|
subi error_lo,0xFF
|
||
|
|
sbci error_hi,0xFF
|
||
|
|
|
||
|
|
POSITIVE_ERR_1: ; Scale for maximum
|
||
|
|
ldi _bemf_lo_byte, 7 - error_mult - err_scale
|
||
|
|
rcall DIVIDE_16_SIMPLE
|
||
|
|
|
||
|
|
.else ;case BACKEMF_SCALE_ENABLED is NOT enabled
|
||
|
|
;*****************************************************************
|
||
|
|
;* Error multiplier (simple) *
|
||
|
|
;* *
|
||
|
|
;* The error multiplier setting (error_mult) can range *
|
||
|
|
;* from -8 to +7, and the actual error multiplier is *
|
||
|
|
;* 2^(error_mult), which therefore ranges from 1/256 to 128. *
|
||
|
|
;* *
|
||
|
|
;* Step 1. Multiply by 2^8. *
|
||
|
|
;* Equivalent to moving the radix point to after *
|
||
|
|
;* error_lo. THIS STEP REQUIRES NO CODE *
|
||
|
|
;* *
|
||
|
|
;* Step 2. Divide by 2^(error_mult - 8) *
|
||
|
|
;* *
|
||
|
|
;* Result is signed two's compliment in *
|
||
|
|
;* error_hi--error_lo -radix- *
|
||
|
|
;*****************************************************************
|
||
|
|
|
||
|
|
ldi _bemf_lo_byte, 7 - error_mult
|
||
|
|
rcall DIVIDE_16_SIMPLE
|
||
|
|
|
||
|
|
.endif ;BACKEMF_SCALE_ENABLED
|
||
|
|
|
||
|
|
|
||
|
|
COMPUTE_NEW_PWM:
|
||
|
|
;*****************************************************************
|
||
|
|
;* Add in the original throttle *
|
||
|
|
;*****************************************************************
|
||
|
|
add error_lo,throttle_set
|
||
|
|
clr B_Temp
|
||
|
|
adc error_hi,B_Temp
|
||
|
|
|
||
|
|
;*****************************************************************
|
||
|
|
;* Clamp to between 0 and +255 *
|
||
|
|
;*****************************************************************
|
||
|
|
brmi SET_ZERO_PWM ; If result is NEGATIVE, set to zero.
|
||
|
|
|
||
|
|
cpi error_hi,0x00 ; If hi byte is zero, result is ok.
|
||
|
|
breq LOWPASS
|
||
|
|
ldi error_lo,0xFF ; otherwise, clamp
|
||
|
|
rjmp LOWPASS
|
||
|
|
|
||
|
|
SET_ZERO_PWM:
|
||
|
|
clr error_lo
|
||
|
|
|
||
|
|
LOWPASS:
|
||
|
|
|
||
|
|
.ifdef LOWPASS_ENABLED
|
||
|
|
;*****************************************************************
|
||
|
|
;* A transversal low pass filter *
|
||
|
|
;* Lowpass on the emf-adjusted pwm *
|
||
|
|
;* *
|
||
|
|
;* gain input "emf_lowpass_gain", range = 0 to 8 *
|
||
|
|
;* *
|
||
|
|
;* The actual filter time constant "tau" is equal to *
|
||
|
|
;* tau = 2^emf_lowpass_gain * sample_interval *
|
||
|
|
;* *
|
||
|
|
;* The sample interval is nominally 10mS, so the time *
|
||
|
|
;* constant values are: *
|
||
|
|
;* 0 1 2 3 4 5 6 7 8 *
|
||
|
|
;* 10mS,20mS,40mS,80mS,160mS,320mS,640mS,1.28S,2.56S *
|
||
|
|
;* *
|
||
|
|
;* The current sample is added to an attenuated sum of previous *
|
||
|
|
;* samples as follows: *
|
||
|
|
;* *
|
||
|
|
;* Adjusted Value = 1/(2^gain) x *
|
||
|
|
;* ( 1x sample number (i) *
|
||
|
|
;* + gain x sample number (i-1) *
|
||
|
|
;* + gain^2 x sample number (i-2) *
|
||
|
|
;* + gain^3 x sample number (i-3) *
|
||
|
|
;* + .... *
|
||
|
|
;* ) *
|
||
|
|
;* Where: *
|
||
|
|
;* Gain values: gain = (2^emf_lowpass_gain - 1) / 2^n *
|
||
|
|
;* 0,1/2,3/4,7/8,15/16 ... 255/256 *
|
||
|
|
;* *
|
||
|
|
;* Algorithm: *
|
||
|
|
;* *
|
||
|
|
;* -Input (current sample) in error_lo (error_hi=0) *
|
||
|
|
;* 0x00FF max *
|
||
|
|
;* *
|
||
|
|
;* -Input (scaled sum of previous samples) in *
|
||
|
|
;* error_hi_prev--error_lo_prev. *
|
||
|
|
;* 0x00FF * (2^emf_lowpass_gain - 1 ) max *
|
||
|
|
;* *
|
||
|
|
;* 1. The error_hi_prev--error_lo_prev is added to *
|
||
|
|
;* error_hi--error_lo *
|
||
|
|
;* 0x00FF * (2^emf_lowpass_gain) max *
|
||
|
|
;* *
|
||
|
|
;* 2. This value is also stored in *
|
||
|
|
;* error_hi_prev--error_lo_prev *
|
||
|
|
;* *
|
||
|
|
;* 3. The value (error_hi--error_lo) is divided by *
|
||
|
|
;* 2^emf_lowpass_gain (resulting in lowpass value, *
|
||
|
|
;* max 0x00FF) *
|
||
|
|
;* *
|
||
|
|
;* 4. The value (error_hi--error_lo) is subtracted from *
|
||
|
|
;* error_hi_prev--error_lo_prev. This is the new *
|
||
|
|
;* stored value. *
|
||
|
|
;*****************************************************************
|
||
|
|
clr error_hi
|
||
|
|
|
||
|
|
;****
|
||
|
|
;* 1. Add in cumulative previous error
|
||
|
|
;****
|
||
|
|
add error_lo,error_lo_prev ; Add in scaled previous samples
|
||
|
|
adc error_hi,error_hi_prev ;
|
||
|
|
|
||
|
|
;****
|
||
|
|
;* 2. Store
|
||
|
|
;****
|
||
|
|
mov error_lo_prev,error_lo ; Store new value
|
||
|
|
mov error_hi_prev,error_hi ; Store new value
|
||
|
|
|
||
|
|
;****
|
||
|
|
;* 3. Divide new value
|
||
|
|
;****
|
||
|
|
ldi _bemf_lo_byte,emf_lowpass_gain
|
||
|
|
rcall DIVIDE_16_SIMPLE
|
||
|
|
|
||
|
|
;****
|
||
|
|
;* 4. New value in error_prev
|
||
|
|
;****
|
||
|
|
ADJUST_STORED:
|
||
|
|
sub error_lo_prev,error_lo
|
||
|
|
sbc error_hi_prev,error_hi
|
||
|
|
|
||
|
|
.endif ;LOWPASS_FILTER
|
||
|
|
|
||
|
|
mov throttle_set,error_lo
|
||
|
|
subi Cycle_count,256-15 ; Normal arrival here occurs after 3.5 + 14.5 +
|
||
|
|
; pwm cycles. Add 15 counts here, also 3 added in
|
||
|
|
; div16u
|
||
|
|
|
||
|
|
.endif BACKEMF_ENABLED
|