;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