WebSVN – Kolibri OS – Blame – /contrib/toolchain/avra/examples/throttle_backemf.asm

Rev	Author	Line No.	Line
6400	punk_joker	1	;throttle_backemf.asm
		2
		3	.NOLIST
		4	; ***************************************************************************************
		5	; * PWM MODEL RAILROAD THROTTLE *
		6	; * *
		7	; * WRITTEN BY: PHILIP DEVRIES *
		8	; * *
		9	; * Copyright (C) 2003 Philip DeVries *
		10	; * *
		11	; * This program is free software; you can redistribute it and/or modify *
		12	; * it under the terms of the GNU General Public License as published by *
		13	; * the Free Software Foundation; either version 2 of the License, or *
		14	; * (at your option) any later version. *
		15	; * *
		16	; * This program is distributed in the hope that it will be useful, *
		17	; * but WITHOUT ANY WARRANTY; without even the implied warranty of *
		18	; * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the *
		19	; * GNU General Public License for more details. *
		20	; * *
		21	; * You should have received a copy of the GNU General Public License *
		22	; * along with this program; if not, write to the Free Software *
		23	; * Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA *
		24	; * *
		25	; ***************************************************************************************
		26	.LIST
		27
		28	.ifdef BACKEMF_ENABLED
		29	;********************************************************************************
		30	;* BACKEMF_ADJUST *
		31	;* Top level routine *
		32	;* *
		33	;* The throttle_set is compared against the back emf generated by the motor *
		34	;* and adjusted to reduce the error *
		35	;* *
		36	;* Inputs: throttle_set Target speed *
		37	;* Returns: throttle_set Adjusted target speed *
		38	;* Changed: error_hi Adjusted throttle, upper 8 bits (local) *
		39	;* error_lo Adjusted throttle, lower 8 bits (local) *
		40	;* error_hi_prev Previous throttle, for filter (global) *
		41	;* error_lo_prev Previous throttle, for filter (global) *
		42	;* emf_hi Measured emf, upper 8 bits (local) *
		43	;* emf_lo Measured emf, lower 8 bits (local) *
		44	;* B_Temp,B_Temp1 *
		45	;* Calls: ADC_SETUP_EMF *
		46	;* div16u *
		47	;* DIVIDE_16_SIMPLE *
		48	;* Goto: none *
		49	;********************************************************************************
		50
		51	HILOCAL1 error_lo ; assign local variables
		52	HILOCAL2 error_hi
		53
		54	;*****************************************************************
		55	;Convert throttle setting into 2 byte 2's compl.
		56	;* *
		57	;* This is a 7 bit number plus 1 more bits after the radix *
		58	;* It is in (error_hi) -radix- (error_lo) *
		59	;*****************************************************************
		60	mov error_hi,throttle_set ; Put throttle into 16 bit form
		61	clr error_lo
		62
		63	lsr error_hi ; Convert to 2's compliment
		64	ror error_lo
		65
		66	;********************************************************************************
		67	;* READ_BACKEMF *
		68	;* Returns a 2 byte 2's compliment measurement of the motor backemf. *
		69	;* *
		70	;* 1. Add together 8 samples of the (8 bit) pwm value in the two byte *
		71	;* emf_hi--emf_lo register. *
		72	;* 2. Multiply by 16. *
		73	;* 3. Result: Minimum value = 0x000 (decimal 0) *
		74	;* Maximum value = 0x7F8 (decimal 2040) *
		75	;* *
		76	;* Time required: *
		77	;* 1. 1st Sample: 125uS *
		78	;* 2. next 7 Samples: 455uS *
		79	;* 3. balance of Subroutine: 10's of uS *
		80	;* TOTAL 580uS min *
		81	;* *
		82	;* Each cycle of the 25kHz PWM takes 40uS, therefore, this routine takes *
		83	;* at least 14.5 cycles of the 25kHz pwm. *
		84	;* *
		85	;* Inputs: None *
		86	;* Returns: emf_hi--emf_lo: 2 Byte 2's compl (but always positive) *
		87	;* measure of motor backemf. *
		88	;* Changed: B_Temp,B_Temp1 *
		89	;* Calls: ADC_SETUP_EMF *
		90	;********************************************************************************
		91	LOWLOCAL1 emf_hi ; Names of local registers
		92	LOWLOCAL2 emf_lo
		93
		94	;READ_BACKEMF:
		95	rcall ADC_SETUP_EMF ; Setup ADC to measure back_emf.
		96	clr emf_lo ; Clear the value of emf.
		97	clr emf_hi
		98
		99	ldi B_Temp,8 ; Add 8 samples
		100	WAIT_FOR_EMF_MEASURE: ; Wait for a measurement of the EMF
		101	sbis ADCSR,ADIF
		102	rjmp WAIT_FOR_EMF_MEASURE
		103
		104	in B_Temp1,ADCH ; Read the measurement
		105
		106	sbi ADCSR,4 ; Clear the interrupt
		107
		108	add emf_lo,B_Temp1 ; Add to low byte (no carry)
		109
		110	clr B_Temp1
		111	adc emf_hi,B_Temp1 ; Add carry to high byte.
		112
		113	dec B_Temp
		114	brne WAIT_FOR_EMF_MEASURE ; Measure for complete set
		115	; Sum of 8 samples.
		116
		117	ldi B_Temp,4 ; Convert 11 bit number into a 15 bit
		118	COMPUTE_EMF_AVERAGE: ; number (only 11 significant figures
		119	lsl emf_lo ; though)
		120	rol emf_hi
		121	dec B_Temp
		122	brne COMPUTE_EMF_AVERAGE
		123
		124	;*****************************************************************
		125	;* Compute the error. That is, throttle = throttle - emf *
		126	;* *
		127	;* The result is a two byte number (signed two's compl) in *
		128	;* error_hi -radix- error_lo *
		129	;*****************************************************************
		130	sub error_lo,emf_lo ; subtract low bytes (after radix)
		131	sbc error_hi,emf_hi ; subtract high bytes (before radix)
		132
		133	.ifdef BACKEMF_SCALE_ENABLED
		134	;*****************************************************************
		135	;* Error multiplier (complex) *
		136	;* *
		137	;* Error gain is equal to: *
		138	;* *
		139	;* Error err_scale err_mult *
		140	;* ------------------------ * 2 * 2 *
		141	;* err_scale *
		142	;* 2 + throttle_set *
		143	;* *
		144	;* The maximum gain when throttle_set = 0 is 2^err_mult *
		145	;* is cut in half when throttle_set = 2^err_scale *
		146	;* *
		147	;* Result is signed two's compliment in *
		148	;* error_hi--error_lo -radix- *
		149	;*****************************************************************
		150	cbr Flags_1,F_negative_err ; Assume error is positive
		151
		152	sbrs error_hi,7 ; Test algebraic sign
		153	rjmp POSITIVE_ERR
		154
		155	sbr Flags_1,F_negative_err ; If error is negative, set flag.
		156	com error_lo ; Convert to positive
		157	com error_hi
		158	subi error_lo,0xFF
		159	sbci error_hi,0xFF
		160
		161	POSITIVE_ERR:
		162
		163	B_TEMPLOCAL _bemf_lo_byte
		164	B_TEMPLOCAL1 _bemf_hi_byte
		165
		166	mov _bemf_lo_byte,throttle_set ; Divisor = throttle_set+2^err_scale
		167	clr _bemf_hi_byte
		168	ldi B_Temp2,EXP2(err_scale)
		169	add _bemf_lo_byte,B_Temp2
		170	adc _bemf_hi_byte,_bemf_hi_byte
		171
		172	; mov dd16uL,error_lo ; Dividend = error (same register)
		173	; mov dd16uH,error_hi
		174
		175	rcall div16u ; Divide error by (throttle+offset)
		176	; (almost 4 pwm cycles)
		177	; adds 3 to Cycle_count
		178
		179	; mov error_lo,dres16uL ; Same register
		180	; mov error_hi,dres16uH
		181
		182	sbrs Flags_1,BF_negative_err ; Check sign flag
		183	rjmp POSITIVE_ERR_1
		184
		185	com error_lo ; Convert back to negative
		186	com error_hi ; if necessary
		187	subi error_lo,0xFF
		188	sbci error_hi,0xFF
		189
		190	POSITIVE_ERR_1: ; Scale for maximum
		191	ldi _bemf_lo_byte, 7 - error_mult - err_scale
		192	rcall DIVIDE_16_SIMPLE
		193
		194	.else ;case BACKEMF_SCALE_ENABLED is NOT enabled
		195	;*****************************************************************
		196	;* Error multiplier (simple) *
		197	;* *
		198	;* The error multiplier setting (error_mult) can range *
		199	;* from -8 to +7, and the actual error multiplier is *
		200	;* 2^(error_mult), which therefore ranges from 1/256 to 128. *
		201	;* *
		202	;* Step 1. Multiply by 2^8. *
		203	;* Equivalent to moving the radix point to after *
		204	;* error_lo. THIS STEP REQUIRES NO CODE *
		205	;* *
		206	;* Step 2. Divide by 2^(error_mult - 8) *
		207	;* *
		208	;* Result is signed two's compliment in *
		209	;* error_hi--error_lo -radix- *
		210	;*****************************************************************
		211
		212	ldi _bemf_lo_byte, 7 - error_mult
		213	rcall DIVIDE_16_SIMPLE
		214
		215	.endif ;BACKEMF_SCALE_ENABLED
		216
		217
		218	COMPUTE_NEW_PWM:
		219	;*****************************************************************
		220	;* Add in the original throttle *
		221	;*****************************************************************
		222	add error_lo,throttle_set
		223	clr B_Temp
		224	adc error_hi,B_Temp
		225
		226	;*****************************************************************
		227	;* Clamp to between 0 and +255 *
		228	;*****************************************************************
		229	brmi SET_ZERO_PWM ; If result is NEGATIVE, set to zero.
		230
		231	cpi error_hi,0x00 ; If hi byte is zero, result is ok.
		232	breq LOWPASS
		233	ldi error_lo,0xFF ; otherwise, clamp
		234	rjmp LOWPASS
		235
		236	SET_ZERO_PWM:
		237	clr error_lo
		238
		239	LOWPASS:
		240
		241	.ifdef LOWPASS_ENABLED
		242	;*****************************************************************
		243	;* A transversal low pass filter *
		244	;* Lowpass on the emf-adjusted pwm *
		245	;* *
		246	;* gain input "emf_lowpass_gain", range = 0 to 8 *
		247	;* *
		248	;* The actual filter time constant "tau" is equal to *
		249	;* tau = 2^emf_lowpass_gain * sample_interval *
		250	;* *
		251	;* The sample interval is nominally 10mS, so the time *
		252	;* constant values are: *
		253	;* 0 1 2 3 4 5 6 7 8 *
		254	;* 10mS,20mS,40mS,80mS,160mS,320mS,640mS,1.28S,2.56S *
		255	;* *
		256	;* The current sample is added to an attenuated sum of previous *
		257	;* samples as follows: *
		258	;* *
		259	;* Adjusted Value = 1/(2^gain) x *
		260	;* ( 1x sample number (i) *
		261	;* + gain x sample number (i-1) *
		262	;* + gain^2 x sample number (i-2) *
		263	;* + gain^3 x sample number (i-3) *
		264	;* + .... *
		265	;* ) *
		266	;* Where: *
		267	;* Gain values: gain = (2^emf_lowpass_gain - 1) / 2^n *
		268	;* 0,1/2,3/4,7/8,15/16 ... 255/256 *
		269	;* *
		270	;* Algorithm: *
		271	;* *
		272	;* -Input (current sample) in error_lo (error_hi=0) *
		273	;* 0x00FF max *
		274	;* *
		275	;* -Input (scaled sum of previous samples) in *
		276	;* error_hi_prev--error_lo_prev. *
		277	;* 0x00FF * (2^emf_lowpass_gain - 1 ) max *
		278	;* *
		279	;* 1. The error_hi_prev--error_lo_prev is added to *
		280	;* error_hi--error_lo *
		281	;* 0x00FF * (2^emf_lowpass_gain) max *
		282	;* *
		283	;* 2. This value is also stored in *
		284	;* error_hi_prev--error_lo_prev *
		285	;* *
		286	;* 3. The value (error_hi--error_lo) is divided by *
		287	;* 2^emf_lowpass_gain (resulting in lowpass value, *
		288	;* max 0x00FF) *
		289	;* *
		290	;* 4. The value (error_hi--error_lo) is subtracted from *
		291	;* error_hi_prev--error_lo_prev. This is the new *
		292	;* stored value. *
		293	;*****************************************************************
		294	clr error_hi
		295
		296	;****
		297	;* 1. Add in cumulative previous error
		298	;****
		299	add error_lo,error_lo_prev ; Add in scaled previous samples
		300	adc error_hi,error_hi_prev ;
		301
		302	;****
		303	;* 2. Store
		304	;****
		305	mov error_lo_prev,error_lo ; Store new value
		306	mov error_hi_prev,error_hi ; Store new value
		307
		308	;****
		309	;* 3. Divide new value
		310	;****
		311	ldi _bemf_lo_byte,emf_lowpass_gain
		312	rcall DIVIDE_16_SIMPLE
		313
		314	;****
		315	;* 4. New value in error_prev
		316	;****
		317	ADJUST_STORED:
		318	sub error_lo_prev,error_lo
		319	sbc error_hi_prev,error_hi
		320
		321	.endif ;LOWPASS_FILTER
		322
		323	mov throttle_set,error_lo
		324	subi Cycle_count,256-15 ; Normal arrival here occurs after 3.5 + 14.5 +
		325	; pwm cycles. Add 15 counts here, also 3 added in
		326	; div16u
		327
		328	.endif BACKEMF_ENABLED

Subversion Repositories Kolibri OS

(root)/contrib/toolchain/avra/examples/throttle_backemf.asm – Rev 6400