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

Rev	Author	Line No.	Line
6400	punk_joker	1	; ***************************************************************************************
		2	; * PWM MODEL RAILROAD THROTTLE *
		3	; * *
		4	; * "throttle.asm" *
		5	; * *
		6	; * WRITTEN BY: PHILIP DEVRIES *
		7	; * *
		8	; * Copyright (C) 2003 Philip DeVries *
		9	; * *
		10	; * This program is free software; you can redistribute it and/or modify *
		11	; * it under the terms of the GNU General Public License as published by *
		12	; * the Free Software Foundation; either version 2 of the License, or *
		13	; * (at your option) any later version. *
		14	; * *
		15	; * This program is distributed in the hope that it will be useful, *
		16	; * but WITHOUT ANY WARRANTY; without even the implied warranty of *
		17	; * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the *
		18	; * GNU General Public License for more details. *
		19	; * *
		20	; * You should have received a copy of the GNU General Public License *
		21	; * along with this program; if not, write to the Free Software *
		22	; * Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA *
		23	; * *
		24	; ***************************************************************************************
		25	; ***************************************************************************************
		26	; * Fixed Version for avra >= 1.2.2 because .DSEG cannot be used, if device has no SRAM *
		27	; * B.Arenfeld, 10.05.2007 *
		28	; ***************************************************************************************
		29	; ***************************************************************************************
		30	; For Attiny 15
		31	; * *
		32	; For compilation with: Avra 0.70 or later
		33	; * *
		34	; * Atmel avrasm32.exe will not work becaue of the *
		35	; * use of preprocessor directives #ifdef, #ifndef *
		36	; * , and #endif, which Atmel doesn't support! *
		37	; * *
		38	; Compiling requires the following files:
		39	; * "tn15def.inc" Labels and identifiers for tiny15 *
		40	; * "throttle_dev_set.inc" Tiny 15 device settings *
		41	; * "throttle_op_set.inc" Operation settings (THIS IS THE ONE TO EDIT THE *
		42	; * COMPLIE-TIME OPTIONS ON THE WAY THE THROTTLE *
		43	; * PERFORMS *
		44	; * *
		45	; Depending on the compile time options, the following files are also required:
		46	; * "throttle_divide.asm" Two division routines. (one from atmel) *
		47	; * "throttle_set_lowpass.asm" Lowpass filter on throttle handle *
		48	; * "throttle_momentum.asm" Compute speed according to momentum *
		49	; * "throttle_momentum_lowpass.asm" Lowpass filter on momentum handle *
		50	; * "throttle_backemf.asm" Adjust pwm based on motor speed *
		51	; * "throttle_pulse.asm" Provide pulse assist at low motor speeds *
		52	; * "throttle_multiply.asm" atmel multiplication routine *
		53	; * *
		54	; Subroutine Categories and Stack
		55	; * *
		56	; * The Tiny 15 has a three level stack which handles return addresses for *
		57	; * subroutine calls and for interrupt service routines. The categories below *
		58	; * ensure that the stack does not overflow. The four (4) categories are: *
		59	; * *
		60	; * -- Top Level Routines: These routines are never called, and never *
		61	; * return ("ret") and so the stack is, "empty" *
		62	; * These routines may call any subroutine. *
		63	; * Register Variables: Temp, Temp1, ... *
		64	; * *
		65	; * -- First Level Subroutines: Stack has 1 entry. These routines may call *
		66	; * Second Level Subroutines only. *
		67	; * Register Variables: A_Temp, A_Temp1, ... *
		68	; * *
		69	; * -- Second Level Subroutines: Stack has 2 entries. These routines may NOT *
		70	; * call any subroutines. *
		71	; * Register Variables: B_Temp, B_Temp1, ... *
		72	; * *
		73	; * -- Interrupt Service These occur assynchronously, and therefor may *
		74	; * occur during Second Level Subroutines. If so, *
		75	; * the stack has 3 entries and IS FULL. These routines *
		76	; * may NOT call any subroutines. *
		77	; * Register Variables: I_Temp, I_Temp1, ... *
		78	; * *
		79	; * Register variables are reserved for each level of routine. Each level may freely *
		80	; * use the register variables for it's own level. Some sharing of variables may *
		81	; * occur subject to these guidelines: *
		82	; * -- No category execpt Interrupt Service may use Interrupt Service variables. This *
		83	; * is because ISRs occur asynchronously. *
		84	; * -- A category may NOT use variables reserved for higher level categories. *
		85	; * -- A category may use lower level routine variables as long as their use does not *
		86	; * span any subroutine calls. *
		87	; * *
		88	; Other settings
		89	; * There is no way to put these settings into this file, but these must also be *
		90	; * done: *
		91	; * *
		92	; * BODLEVEL: 0 4.0V *
		93	; * BOODEN: 0 ENABLED (brown out detection) *
		94	; * SPIEN: 0 ENABLED (in circuit programming) *
		95	; * RSTDISBL 0 DISABLED (reset on PB5) *
		96	; * CKSEL 11 (very quickly rising power) *
		97	; * LB1 1 (LB1 & LB2: No lock) *
		98	; * LB2 1 *
		99	; * *
		100	; * Calibration byte into flash byte address as specified in *
		101	; * osccal_location. *
		102	; * *
		103	; * Notes regarding these settings: *
		104	; * --Brown out detection. The datasheet warns against using the EEPROM without *
		105	; * brownout detection because of the possibility of errant execution at very low *
		106	; * voltage levels. *
		107	; * *
		108	; **************************************************************************************
		109
		110
		111
		112	;*****************************************************************************************
		113	;*****************************************************************************************
		114	;* Included files *
		115	;*****************************************************************************************
		116	;*****************************************************************************************
		117	.INCLUDE "tn15def.inc" ; Labels and identifiers for tiny15
		118	.INCLUDE "throttle_op_set.inc" ; Operation settings
		119	.INCLUDE "throttle_dev_set.inc" ; Tiny 15 device settings
		120
		121	;*****************************************************************************************
		122	;*****************************************************************************************
		123	;* DATA TABLE *
		124	;*****************************************************************************************
		125	;*****************************************************************************************
		126	;.CSEG
		127	;.ORG 0x01E0 ; Program .ORG 0x01E0 actually means byte
		128	; location 0x03C0.
		129
		130	;*****************************************************************************************
		131	;*****************************************************************************************
		132	;* Data: reserved for OSCCAL byte *
		133	;*****************************************************************************************
		134	;*****************************************************************************************
		135
		136	; Fix : The Tiny15 has NO SRAM. Use of .DSEG is invalid !
		137	;.DSEG
		138	;
		139	;.SET osccal_location = 0x3FF ;Place in the last byte of program memory.
		140	; ;High byte of program memory 1FF
		141	;
		142	;.ORG osccal_location ;reserve this byte for oscillator
		143	;.BYTE 1 ;calibration value
		144
		145
		146	; Fixed Version for avra >= 1.2.2
		147	.CSEG
		148	.EQU osccal_location = 0x3FF ; The last flash byte is used for the calibration
		149	; value and is replaced by the programmer
		150
		151	; Now reserve the last word in flash memory. If you are sure, that the program doesn't use
		152	; the last flash word, you can disable the following lines. If not, it's better to enable
		153	; them to check for overlapping code segments.
		154
		155	.ORG 0x01FF ; Last word in flash memory
		156	.DB 0xff,0xff ; Fill with dummy values. Only last byte is used
		157	; but flash is organized in words.
		158	; 0xff is the value of unprogrammed flash
		159
		160
		161	;*****************************************************************************************
		162	;*****************************************************************************************
		163	;* Reset and Interrupt Vectors *
		164	;*****************************************************************************************
		165	;*****************************************************************************************
		166	.CSEG
		167	.ORG 0x000
		168	rjmp ST_RESET
		169
		170	.ifdef OVERLOAD_ENABLED
		171	rjmp ST_PWM_LEVEL_OFF ; INT0 interrupt handler
		172	.else
		173	reti ; Not used.
		174	.endif ;OVERLOAD_ENABLED
		175
		176	reti ; Not used. rjmp PIN_CHANGE
		177	reti ; Not used. rjmp TIM1_CMP
		178	reti ; Not used. rjmp TIM1_OVF
		179	reti ; Not used. rjmp TIM0_OVF
		180	reti ; Not used. rjmp EE_RDY
		181	reti ; Not used. rjmp ANA_COMP
		182	reti ; Not used. rjmp ADC
		183
		184
		185	;*****************************************************************************************
		186	;*****************************************************************************************
		187	;* Top level routines. The basic program is a state machine, with states all being *
		188	;* top level routines. These routines are never used as subroutines *
		189	;* and therefore can call any subroutine. *
		190	;*****************************************************************************************
		191	;*****************************************************************************************
		192
		193	;********************************************************************************
		194	;* ST_RESET *
		195	;* This is power on reset. The reset vector points here. *
		196	;* *
		197	;* Inputs: none *
		198	;* Returns: none *
		199	;* Changed: B_Temp *
		200	;* Calls: *
		201	;* Goto: ST_MOTOR_OFF *
		202	;********************************************************************************
		203	ST_RESET:
		204	cli ; Disable interrupts
		205
		206	ldi B_Temp,(dir_out_port_bit \| pwm_port_bit \| dir_in_port_bit \| momentum_port_bit)
		207	out DDRB,B_Temp ; Assign output port directions.
		208	; Inclusion in the above list makes the
		209	; port an output port
		210
		211	ldi B_Temp,0x00 ; A "1" makes output logic level high
		212	out PORTB,B_Temp ; A "1" assigns pullups on inputs
		213	; Therefore all outputs are at logic low, and
		214	; all inputs do not have a pullup assigned
		215
		216	ldi B_Temp,acsr_val ; Disable comparator and interrupt
		217	out ACSR,B_Temp ; Using port for PWM
		218	; (comparator defaults to powered up)
		219
		220	ldi B_Temp,0b01000010 ; Disable pullups.
		221	out MCUCR,B_Temp ; Set sleep mode (moot)
		222	; INT0 interrupt on falling edge
		223
		224	ldi ZL,low(osccal_location) ; r30
		225	ldi ZH,high(osccal_location) ; r31
		226	lpm
		227	out OSCCAL,Implicit ; Place calibration byte
		228
		229	ldi B_Temp,0b00001010 ; Enable watchdog
		230	out WDTCR,B_Temp ; timout 64mS (nom)
		231
		232	ldi B_Temp1,pwm_period ; Set pwm oscillator period
		233	out OCR1B,B_Temp1
		234
		235	ldi B_Temp1,tccr1_enable_t1 ; Turn on the PWM oscillator
		236	out TCCR1,B_Temp1
		237
		238	ldi Flags_1,(0b00000000 \| F_stop) ; Set emergency stop flag so that
		239	; throttle doesn't start on powerup
		240	.ifdef TRADITIONAL_ENABLED
		241	.ifdef MOMENTUM_LOWPASS_ENABLED
		242	clr momentum_lo_prev ; MOMENTUM LOWPASS
		243	clr momentum_hi_prev ; Clear the history
		244	.endif ;MOMENTUM_LOWPASS_ENABLED
		245	.endif ;TRADITIONAL_ENABLED
		246
		247	; rjmp ST_EMERGENCY_STOP ; *EXIT STATE*
		248
		249	;********************************************************************************
		250	;* ST_EMERGENCY_STOP *
		251	;* *
		252	;* Reset to "off" state. *
		253	;* Clear global variables associated with momentum and lowpass filters. *
		254	;* *
		255	;* Inputs: none *
		256	;* Returns: none *
		257	;* Changed: Global variables cleared *
		258	;* Calls: None *
		259	;* Goto: ST_PWM_LEVEL_OFF If throttle is zero *
		260	;********************************************************************************
		261	ST_EMERGENCY_STOP:
		262
		263	.ifdef BACKEMF_ENABLED
		264	.ifdef LOWPASS_ENABLED ; BACKEMF LOWPASS
		265	clr error_hi_prev ; Clear the history
		266	clr error_lo_prev
		267	.endif ;LOWPASS_ENABLED
		268	.endif ;BACKEMF_ENABLED
		269
		270	.ifdef MOMENTUM_ENABLED ; MOMENTUM
		271	clr speed_lo_prev ; Clear the history
		272	clr speed_hi_prev
		273	.endif ;MOMENTUM_ENABLED
		274
		275	.ifdef TRADITIONAL_ENABLED
		276	.ifdef WALKAROUND_ENABLED
		277	clr throttle_hold ; Clear the history
		278	.endif ;WALKAROUND_ENABLED
		279
		280	.ifdef THROTTLE_LOWPASS_ENABLED
		281	clr throttle_lo_prev ; THROTTLE LOWPASS
		282	clr throttle_hi_prev ; Clear the history
		283	.endif ;THROTTLE_LOWPASS_ENABLED
		284
		285	.endif ;TRADITIONAL_ENABLED
		286
		287	; rjmp ST_PWM_LEVEL_OFF ; *EXIT STATE*
		288
		289
		290	;********************************************************************************
		291	;* ST_PWM_LEVEL_OFF *
		292	;* ST_MEASUREMENT_SETTLE *
		293	;* 1. If entered at ST_PWM_LEVEL_OFF turn pwm off *
		294	;* 2. Set the ADC ports to input *
		295	;* 3. Pause to let ADC inputs (including back-emf) settle. *
		296	;* 4. Read the throttle controller. *
		297	;* 5. Set LED ports and overload ports (also ADC inputs) to output *
		298	;* 6. If throttle_set is not zero, or if motor is still running by momentum *
		299	;* continue running motor (jump to ST_SET_NEW_PWM) *
		300	;* 7. If throttle set is zero and motor is not running, then set the direction *
		301	;* relay and test backemf input to determine backemf mode. *
		302	;* 8. Turn of motor (jump to ST_PWM_LEVEL_OFF) *
		303	;* *
		304	;* Inputs: none *
		305	;* Returns: none *
		306	;* Changed: B_Temp, B_Temp1 *
		307	;* Calls: READ_THROTTLE *
		308	;* Goto: ST_PWM_LEVEL_OFF If throttle is zero *
		309	;* ST_SET_NEW_PWM After delay *
		310	;********************************************************************************
		311	ST_PWM_LEVEL_OFF:
		312	clr B_Temp ; Set PWM duty = 0.
		313	rcall SET_PWM_DUTY ; i.e. turn off the power
		314
		315	ST_MEASUREMENT_SETTLE:
		316	;********************************************
		317	;* Set all measurement ports for input and pause.
		318	;* During the pause:
		319	;* 1. inductive current in the locomotive falls to zero, and
		320	;* the backemf voltage appears on the backemf port
		321	;* 2. the momentum, direction, and throttle voltages stabilize
		322	;********************************************
		323	.ifdef TRADITIONAL_ENABLED
		324	.ifdef LEDS_ENABLED
		325	cbi DDRB,momentum_port ; Make input port (pullup must be disabled)
		326	cbi DDRB,dir_in_port ; Make input port (pullup must be disabled)
		327	.endif ;LEDS_ENABLED
		328
		329	.ifdef OVERLOAD_ENABLED
		330	in B_Temp,GIMSK ; disable INT0 interrupt
		331	andi B_Temp,0b10111111
		332	out GIMSK,B_Temp
		333
		334	cbi DDRB,throttle_port ; Make input port (pullup must be disabled)
		335	.endif ;OVERLOAD_ENABLED
		336	.endif ;TRADITIONAL_ENABLED
		337
		338	sei ; Enable interrupts
		339	wdr ; Reset watchdog timer
		340
		341	ldi B_Temp1,pwm_full_count ; Pause for inputs to settle
		342	rcall COUNT_PWM_CYCLES
		343	clr Cycle_count
		344
		345	;********************************************
		346	;* Read the input ports and make some
		347	;* mode decisions based on those inputs.
		348	;********************************************
		349
		350	rcall READ_THROTTLE ; Find throttle handle position in throttle_set
		351
		352	.ifdef TRADITIONAL_ENABLED
		353	.ifdef MOMENTUM_ENABLED
		354	.ifdef MOMENTUM_LOWPASS_ENABLED
		355	.include "throttle_momentum_lowpass.asm"
		356	.endif;MOMENTUM_LOWPASS_ENABLED
		357	.endif; MOMENTUM_ENABLED
		358	.endif ;TRADITIONAL_ENABLED
		359
		360	.ifdef DIRECTION_ENABLED ; Check Stop, and Adjust Direction
		361	CHECKING_STOP:
		362	sbrs Flags_1,BF_stop ; Check stop flag is set
		363	rjmp DONE_CHECKING_STOP
		364
		365	cpi throttle_set,0x00 ; If throttle handle is at zero
		366	brne ST_EMERGENCY_STOP ; reset the emergency stop flag
		367	cbr Flags_1,F_stop ; reset emergency stop flag.
		368	rjmp ST_EMERGENCY_STOP ; ALWAYS STOP
		369	DONE_CHECKING_STOP:
		370
		371	CHECKING_DIRECTION:
		372
		373	.ifdef MOMENTUM_ENABLED
		374	mov B_Temp,speed_hi_prev ; Don't set direction unless the actual
		375	cpi B_Temp,direction_threshold ; speed is less than direction_threshold
		376	brsh DONE_CHECKING_DIRECTION
		377	.else
		378	cpi throttle_set,0x00 ; Don't set direction unless the throttle
		379	brne DONE_CHECKING_DIRECTION ; handle is at zero
		380	.endif ;MOMENTUM_ENABLED
		381
		382	sbic PORTB,dir_out_port ; Find port direction
		383	rjmp PORT_REVERSE
		384	;rjmp PORT_FORWARD
		385
		386	PORT_FORWARD:
		387	sbrs Flags_1,BF_reverse ; If port says forward
		388	rjmp DONE_CHECKING_DIRECTION
		389	sbi PORTB,dir_out_port ; But flag says reverse, then reverse
		390	rjmp ST_EMERGENCY_STOP
		391
		392	PORT_REVERSE:
		393	sbrc Flags_1,BF_reverse ; If port says reverse
		394	rjmp DONE_CHECKING_DIRECTION
		395	cbi PORTB,dir_out_port ; But flag says foreward, then forward
		396	rjmp ST_EMERGENCY_STOP
		397
		398	DONE_CHECKING_DIRECTION:
		399	.endif ;DIRECTION_ENABLED
		400
		401	.ifdef TRADITIONAL_ENABLED
		402	.ifdef THROTTLE_LOWPASS_ENABLED
		403	.include "throttle_set_lowpass.asm"
		404	.endif ;THROTTLE_LOWPASS_ENABLED
		405	.endif ;TRADITIONAL_ENABLED
		406
		407	cpi throttle_set,0x00 ; Run the pwm unless the throttle
		408	brne ST_SET_NEW_PWM ; is zero
		409
		410	.ifdef MOMENTUM_ENABLED
		411	mov B_Temp,speed_hi_prev ; In momentum mode, run the pwm unless
		412	cpi B_Temp,0x00 ; the actual throttle setting reaches zero
		413	brne ST_SET_NEW_PWM
		414	.endif ;MOMENTUM_ENABLED
		415
		416	;********************************************
		417	;* Only arrive here if the throttle is set for 0 speed
		418	;* and the locomotive is actually stopped (momentum)
		419	;********************************************
		420
		421	.ifdef BACKEMF_ENABLED
		422	;********************************************
		423	;* The backemf measurement should be at or near zero,
		424	;* since the locomotive is stopped. If it isn't,
		425	;* do not use backemf speed control.
		426	;********************************************
		427	sbr Flags_1,F_use_backemf ; Default to use backemf
		428
		429	rcall ADC_SETUP_EMF ; 4 lines read the backemf
		430	WAIT_FOR_VALID:
		431	sbis ADCSR,ADIF
		432	rjmp WAIT_FOR_VALID
		433
		434	in B_Temp,ADCH ; Read the measurement
		435
		436
		437	cpi B_Temp,0x40 ; Test measurement
		438	brlo END_CHECK_BACKEMF_MODE ; If small, use backemf adjustment.
		439
		440	cbr Flags_1,F_use_backemf ; Otherwise, don't use backemf
		441	END_CHECK_BACKEMF_MODE:
		442	.endif ;BACKEMF_ENABLED
		443
		444	.ifdef TRADITIONAL_ENABLED
		445	.ifdef LOCO_LIGHT_ENABLED
		446	ldi throttle_set,light_pwm
		447	rjmp STABLE_PWM_SET
		448	.else
		449	rjmp ST_PWM_LEVEL_OFF
		450	.endif ;LOCO_LIGHT_ON
		451	.else
		452	rjmp ST_PWM_LEVEL_OFF
		453	.endif ;TRADITIONAL_ENABLED
		454
		455
		456	;********************************************************************************
		457	;* ST_SET_NEW_PWM *
		458	;* Compute the pwm setting based upon momentum, backemf, and throttle setting *
		459	;* Inputs: throttle_set *
		460	;* Returns: none *
		461	;* Changed: throttle_set, other variables in included files *
		462	;* Calls: various in included files *
		463	;* Goto: ST_PWM_LEVEL_ON *
		464	;********************************************************************************
		465	ST_SET_NEW_PWM:
		466
		467	.ifdef TRADITIONAL_ENABLED
		468	.ifdef LEDS_ENABLED
		469	cbi PORTB,dir_in_port ; logic low out (turn off LED)
		470	sbi DDRB,dir_in_port ; Assign output to drive led (output is low)
		471
		472	cbi PORTB,momentum_port ; logic low out (turn off LED)
		473	sbi DDRB,momentum_port ; Assign output to drive led (output is low)
		474	.endif ;LEDS_ENABLED
		475
		476	.ifdef OVERLOAD_ENABLED
		477	;********************************************
		478	;* The thottle port is driven to logic high. If this port gets pulled
		479	;* low (overload), this triggers the INT0 interrupt, which will shut off
		480	;* the pwm.
		481	;********************************************
		482	sbi PORTB,throttle_port ; Logic hi out.
		483	sbi DDRB,throttle_port ; Make output port.
		484	.endif ;OVERLOAD_ENABLED
		485	.endif ;TRADITIONAL_ENABLED
		486
		487	.ifdef MOMENTUM_ENABLED
		488	.include "throttle_momentum.asm" ; momentum adjustment
		489	.endif ;MOMENTUM_ENABLED
		490
		491	.ifdef TRADITIONAL_ENABLED
		492	.ifdef WALKAROUND_ENABLED
		493	mov throttle_hold,throttle_set
		494	.endif ;WALKAROUND_ENABLED
		495	.endif ;TRADITIONAL_ENABLED
		496
		497	.ifdef BACKEMF_ENABLED ;********************************************
		498	; Adjust throttle_set according to
		499	; measured backemf.
		500	;********************************************
		501
		502	sbrs Flags_1,BF_use_backemf ; If the flag is set, use backemf
		503	rjmp DONT_BACKEMF ; Otherwise, don't
		504
		505	.include "throttle_backemf.asm"
		506	; If using backemf, don't use throttle_scale
		507	rjmp ST_PWM_LEVEL_ON ; *EXIT STATE*
		508
		509	DONT_BACKEMF:
		510	.endif ;BACKEMF
		511
		512
		513	;*****************************************************************
		514	;* Scale the throttle_set between 0 and pwm_period *
		515	;* multiply pwm_period and throttle_set and divide by 256 *
		516	;* read answer from hi byte of return. *
		517	;*****************************************************************
		518
		519
		520	HILOCAL1 _main_scale_multiplicand
		521	B_TEMPLOCAL _main_scale_multiplier
		522	B_TEMPLOCAL1 _main_scale_result_hi
		523
		524	ldi _main_scale_multiplicand,pwm_period - pwm_min
		525	mov _main_scale_multiplier,throttle_set
		526	rcall mpy8u ; multiply
		527	mov throttle_set,_main_scale_result_hi ; read result
		528
		529	; rjmp ST_PWM_LEVEL_ON ; *EXIT STATE*
		530
		531
		532	;********************************************************************************
		533	;* ST_PWM_LEVEL_ON *
		534	;* 1. Enable overload testing *
		535	;* 2. Produce pulse if required *
		536	;* 3. Run pwm at throttle_set *
		537	;* 4. Wait for a while *
		538	;* *
		539	;* Inputs: throttle_set *
		540	;* Returns: none *
		541	;* Changed: B_Temp, B_Temp1, various *
		542	;* Calls: SET_PWM_DUTY *
		543	;* COUNT_PWM_CYCLES *
		544	;* Goto: ST_PWM_LEVEL_OFF After PWM goes to off state *
		545	;********************************************************************************
		546	ST_PWM_LEVEL_ON:
		547
		548	.ifdef TRADITIONAL_ENABLED
		549	.ifdef OVERLOAD_ENABLED
		550	ldi B_Temp,0b01000000 ; clear INT0 interrupt
		551	out GIFR,B_Temp
		552
		553	in B_Temp,GIMSK ; enable INT0 interrupt
		554	ori B_Temp,0b01000000
		555	out GIMSK,B_Temp
		556	.endif ;OVERLOAD_ENABLED
		557	.endif ;TRADITIONAL_ENABLED
		558
		559	cpi throttle_set,light_pwm ; never run pwm lower than light_pwm level
		560	brsh DONE_CHECKING_MINIMUM
		561	ldi throttle_set,light_pwm
		562	rjmp STABLE_PWM_SET
		563
		564	DONE_CHECKING_MINIMUM:
		565
		566	.ifdef PULSE_ENABLED ; Produce pulses during output
		567	.ifdef BACKEMF_ENABLED
		568	sbrc Flags_1,BF_use_backemf ; If the flag is set to use backemf
		569	rjmp STABLE_PWM_SET ; don't pulse
		570	.endif ;BACKEMF_ENABLED
		571
		572	; Pass in: throttle_set
		573	.include "throttle_pulse.asm"
		574	.endif ;PULSE_ENABLED
		575
		576
		577	STABLE_PWM_SET:
		578	mov B_Temp,throttle_set ; Stabilize at throttle_set
		579	rcall SET_PWM_DUTY
		580
		581	ldi B_Temp1,pwm_full_count-pwm_settle_count
		582	rcall COUNT_PWM_CYCLES ; Wait for end of interval
		583
		584	.ifdef BACKEMF_ENABLED
		585	sbrc Flags_1,BF_use_backemf ; If the flag is set to use backemf
		586	rjmp ST_PWM_LEVEL_OFF ; *EXIT STATE*
		587	.endif ;BACKEMF_ENABLED
		588	rjmp ST_MEASUREMENT_SETTLE ; *EXIT STATE*
		589
		590	;*****************************************************************************************
		591	;*****************************************************************************************
		592	;* First Level Subroutines. *
		593	;* These routines include the routines which are called by other code and also call *
		594	;* Second Level Subroutines. *
		595	;*****************************************************************************************
		596	;*****************************************************************************************
		597
		598	;********************************************************************************
		599	;* READ_THROTTLE *
		600	;* First Level Subroutine *
		601	;* *
		602	;* Read the throttle controls, which are: *
		603	;* Momentum level (analog): Returned in "momentum_set" *
		604	;* Returns and 8 bit number, with '0' meaning minimum momentum. *
		605	;* *
		606	;* Direction, brake, and stop switch. *
		607	;* Returns value in flags: F_brake, F_reverse, and F_stop *
		608	;* *
		609	;* Throttle setting (analog: Returned in "throttle_set" *
		610	;* Returns an 8 bit number (0x00 to 0xFF; 0 to 255), *
		611	;* where '0' means "motor off" and 0xFF (255) means full speed. *
		612	;* *
		613	;* If a speed table is implemented, it will be in this routine *
		614	;* *
		615	;* Just now, this value comes from the analog input and is converted by the *
		616	;* ADC. The raw 8 bit number is returned. *
		617	;* *
		618	;* Inputs: None *
		619	;* Returns: Momentum setting in "momentum_set" *
		620	;* Switch positions in F_brake, F_reverse, and F_stop *
		621	;* Throttle setting in "throttle_set" *
		622	;* Changed: Cycle_count incremented by up to 5 *
		623	;* Calls: ADC_SETUP_MOMENTUM *
		624	;* ADC_SETUP_DIRECTION *
		625	;* ADC_SETUP_THROTTLE *
		626	;********************************************************************************
		627	READ_THROTTLE:
		628	.ifdef TRADITIONAL_ENABLED
		629	.ifdef DIRECTION_ENABLED
		630	;********************************************
		631	;* Measure the direction, brake, and stop switches and
		632	;* set the flags appropriately
		633	;********************************************
		634	rcall ADC_SETUP_DIRECTION ; Setup to read
		635	WAIT_FOR_VALID_DIRECTION:
		636	sbis ADCSR,ADIF ; Check for ADC completion
		637	rjmp WAIT_FOR_VALID_DIRECTION
		638
		639	in B_Temp,ADCH ; Read value
		640
		641	.ifdef WALKAROUND_ENABLED
		642	cpi B_Temp,0x90 ; Above this threshold
		643	; deactivates handheld controller
		644	; brsh HOLD_THROTTLE
		645
		646	brlo HOLD_THROTTLE_NOT
		647	rjmp HOLD_THROTTLE
		648	HOLD_THROTTLE_NOT:
		649	.endif ;WALKAROUND_ENABLED
		650
		651	cpi B_Temp,0x1B ; Below this threshold (0.53V) sets 'stop' flag
		652	brsh TEST_BRAKE_LEVEL ; Typical stop voltage is 0.30V
		653
		654	.ifdef SWITCH_LOWPASS_ENABLED
		655	sbrs Flags_2,BF_stop_count ; If the stop count flag is not set, then
		656	clr Flags_2 ; set the counter to zero
		657
		658	cbr Flags_2,F_stop_count ; clear the stop count flag
		659	inc Flags_2 ; increment the counter
		660
		661	cpi Flags_2,stop_count_max ; compare the count to the maximum
		662	sbr Flags_2,F_stop_count ; set the stop count flag
		663
		664	brlo END_READ_DIRECTION ; if the count is lower, don't change status flag
		665
		666	dec Flags_2,F_stop_count ; decrement stop count flag
		667	.endif;SWITCH_LOWPASS_ENABLED
		668
		669	sbr Flags_1,F_stop
		670	rjmp END_READ_DIRECTION
		671
		672	TEST_BRAKE_LEVEL:
		673	cpi B_Temp,0x37 ; Below this threshold (1.07V) sets 'brake' flag
		674	brsh TEST_REVERSE_LEVEL ; Typical brake voltage 0.87V
		675
		676	.ifdef SWITCH_LOWPASS_ENABLED
		677	sbrs Flags_2,BF_brake_count ; If the brake count flag is not set, then
		678	clr Flags_2 ; set the counter to zero
		679
		680	cbr Flags_2,F_brake_count ; clear the break count flag
		681	inc Flags_2 ; increment the counter
		682
		683	cpi Flags_2,brake_count_max ; compare the count to the maximum
		684	sbr Flags_2,F_brake_count ; set the break count flag
		685
		686	brlo END_READ_DIRECTION ; if the count is lower, don't change status flag
		687
		688	dec Flags_2,F_brake_count ; decrement break count flag
		689	.endif;SWITCH_LOWPASS_ENABLED
		690
		691	sbr Flags_1,F_brake
		692	rjmp END_READ_DIRECTION
		693
		694	TEST_REVERSE_LEVEL:
		695	cpi B_Temp,0x53 ; Below this threshold (1.62V) sets 'reverse' flag
		696	brsh TEST_FOREWARD_LEVEL ; Typical reverse level 1.40V
		697
		698	.ifdef SWITCH_LOWPASS_ENABLED
		699	sbrs Flags_2,BF_reverse_count ; If the reverse count flag is not set, then
		700	clr Flags_2 ; set the counter to zero
		701
		702	cbr Flags_2,F_reverse_count ; clear the reverse count flag
		703	inc Flags_2 ; increment the counter
		704
		705	cpi Flags_2,reverse_count_max ; compare the count to the maximum
		706	sbr Flags_2,F_reverse_count ; set the reverse count flag
		707
		708	brlo END_READ_DIRECTION ; if the count is lower, don't change status flag
		709
		710	dec Flags_2,F_reverse_count ; decrement reverse count flag
		711	.endif;SWITCH_LOWPASS_ENABLED
		712
		713	cbr Flags_1,F_brake ; Clear brake flag
		714	sbr Flags_1,F_reverse ; Set brake flag
		715	rjmp END_READ_DIRECTION
		716
		717	TEST_FOREWARD_LEVEL: ; Typical "nothing" 1.95V
		718	;no test required
		719
		720	.ifdef SWITCH_LOWPASS_ENABLED
		721	sbrs Flags_2,BF_foreward_count ; If the foreward count flag is not set, then
		722	clr Flags_2 ; set the counter to zero
		723
		724	cbr Flags_2,F_foreward_count ; clear the foreward count flag
		725	inc Flags_2 ; increment the counter
		726
		727	cpi Flags_2,foreward_count_max ; compare the count to the maximum
		728	sbr Flags_2,F_foreward_count ; set the foreward count flag
		729
		730	brlo END_READ_DIRECTION ; if the count is lower, don't change status flag
		731
		732	dec Flags_2,F_foreward_count ; decrement forward count flag
		733	.endif;SWITCH_LOWPASS_ENABLED
		734
		735	cbr Flags_1,F_brake ; Clear brake flag
		736	cbr Flags_1,F_reverse ; Clear reverse flag (i.e., foreward)
		737
		738	END_READ_DIRECTION:
		739	.endif ;DIRECTION_ENABLED
		740
		741	.ifdef MOMENTUM_ENABLED
		742	;********************************************
		743	;* Measure and adjust the momentum input
		744	;********************************************
		745	rcall ADC_SETUP_MOMENTUM ; Setup to read
		746	WAIT_FOR_VALID_MOMENTUM:
		747	sbis ADCSR,ADIF ; Wait for ADC completion
		748	rjmp WAIT_FOR_VALID_MOMENTUM
		749
		750	in momentum_set,ADCH ; Read value
		751
		752	.ifdef WALKAROUND_ENABLED
		753	ldi B_Temp,0x90
		754	cp momentum_set,B_Temp ; Above this threshold
		755	; deactivates handheld controller
		756	brsh HOLD_THROTTLE
		757	.endif ;WALKAROUND_ENABLED
		758
		759	ldi B_Temp,0x40
		760	sub momentum_set,B_Temp ; Subtract offset (1/4 of 0xFF)
		761
		762	brsh END_READ_MOMENTUM
		763	sub momentum_set,momentum_set ; If smaller than offset, make zero
		764
		765	END_READ_MOMENTUM:
		766	.endif ;MOMENTUM_ENABLED
		767
		768	;********************************************
		769	;* Read the throttle level
		770	;********************************************
		771	rcall ADC_SETUP_THROTTLE
		772	WAIT_FOR_VALID_THROTTLE:
		773	sbis ADCSR,ADIF ; Check for ADC completion
		774	rjmp WAIT_FOR_VALID_THROTTLE
		775
		776	in throttle_set,ADCH ; Read throttle value
		777
		778	subi throttle_set,0x08 ; Subtract offset (force zero)
		779	brcc DONE_READ_THROTTLE ; If new throttle is negative,
		780
		781	clr throttle_set ; make throttle zero.
		782	DONE_READ_THROTTLE:
		783
		784	subi Cycle_count,256-3 ; Normal arrival here occurs after 3 adc
		785	ret ; conversions, which take 195uS, or 4.875
		786	; pwm cycles
		787	.ifdef WALKAROUND_ENABLED
		788	HOLD_THROTTLE: ; Normal arrival here occurs after 1 adc
		789	; conversion, which takes 65uS, or 1.625
		790	; pwm cycles
		791
		792	cbr Flags_1,F_brake ; Clear brake flag
		793	mov throttle_set,throttle_hold ; Use previous value.
		794
		795	.ifdef SWITCH_LOWPASS_ENABLED
		796	clr Flags_2
		797	.endif;SWITCH_LOWPASS_ENABLED
		798
		799	.ifdef MOMENTUM_ENABLED
		800	ldi B_Temp,0x40
		801	mov momentum_set,B_Temp ; 'long' momentum
		802	.endif ;MOMENTUM_ENABLED
		803	ret
		804	.endif ;WALKAROUND_ENABLED
		805
		806	.else ;NOT TRADITIONAL_THROTTLE
		807	sbr Flags_1,F_stop
		808	ret
		809	.endif ;TRADITIONAL_THROTTLE
		810
		811
		812	;********************************************************************************
		813	;* COUNT_PWM_CYCLES *
		814	;* First evel Subroutine *
		815	;* *
		816	;* Increment Cycle_count timer each PWM cycle. *
		817	;* Return when Cycle_count = B_Temp1 *
		818	;* *
		819	;* Inputs: B_Temp1 Exit when count reaches this number *
		820	;* Returns: None *
		821	;* Changed: B_Temp,Cycle_count *
		822	;* Calls: None *
		823	;********************************************************************************
		824	COUNT_PWM_CYCLES:
		825	in B_Temp,TIFR ; Wait for pwm timer to reset
		826	sbrs B_Temp,OCF1A
		827	rjmp COUNT_PWM_CYCLES
		828
		829	ldi B_Temp,0b01000000 ; reset interrupt flag
		830	out TIFR,B_Temp
		831
		832	inc Cycle_count ; increment counter and repeat
		833	cp Cycle_count,B_Temp1
		834	brne COUNT_PWM_CYCLES
		835	ret
		836
		837	;*****************************************************************************************
		838	;*****************************************************************************************
		839	;* Second Level Subroutines. *
		840	;* These routines make no further subroutine calls. *
		841	;*****************************************************************************************
		842	;*****************************************************************************************
		843
		844	.include "throttle_divide.asm"
		845
		846	.include "throttle_multiply.asm"
		847
		848	;********************************************************************************
		849	;* SET_PWM_DUTY *
		850	;* Second Level Subroutine *
		851	;* *
		852	;* Inputs: B_Temp PWM on count *
		853	;* Returns: None *
		854	;* Changed: None *
		855	;* Calls: Not allowed *
		856	;********************************************************************************
		857	SET_PWM_DUTY:
		858
		859	out OCR1A,B_Temp ; Set the PWM equal to the input B_Temp
		860	ret
		861
		862	;********************************************************************************
		863	;* ADC_SETUP_DIRECTION *
		864	;* ADC_SETUP_MOMENTUM *
		865	;* ADC_SETUP_THROTTLE *
		866	;* ADC_SETUP_BACK_EMF *
		867	;* Second Level Subroutine *
		868	;* *
		869	;* The ADC is switched off, and restarted on the selected port. *
		870	;* *
		871	;* Inputs: None *
		872	;* Returns: None *
		873	;* Changed: Various B_Temp variables *
		874	;* Calls: Not allowed *
		875	;********************************************************************************
		876	.ifdef DIRECTION_ENABLED
		877	ADC_SETUP_DIRECTION:
		878	ldi B_Temp,admux_direction ; Setup MUX for direction/brake measurement
		879	rjmp ADC_SETUP
		880	.endif ;DIRECTION_ENABLED
		881
		882	.ifdef MOMENTUM_ENABLED
		883	ADC_SETUP_MOMENTUM:
		884	ldi B_Temp,admux_momentum ; Setup MUX for momentum set measurement
		885	rjmp ADC_SETUP
		886	.endif ;MOMENUTM_ENABLED
		887
		888	.ifdef BACKEMF_ENABLED
		889	ADC_SETUP_EMF:
		890	ldi B_Temp,admux_emf ; Setup MUX for back_emf measurement
		891	rjmp ADC_SETUP
		892	.endif ;BACKEMF_ENABLED
		893
		894	.ifdef TRADITIONAL_ENABLED
		895	ADC_SETUP_THROTTLE:
		896	ldi B_Temp,admux_throttle ; Setup MUX for analog measure
		897	; rjmp ADC_SETUP ; of throttle.
		898	.endif ;TRADITIONAL_ENABLED
		899
		900	ADC_SETUP:
		901	ldi B_Temp1,adcsr_off ; Turn off the ADC
		902	out ADCSR,B_Temp1 ;
		903	out ADMUX,B_Temp ; Setup MUX as per entry point
		904	ldi B_Temp,adcsr_enable ; enable ADC, disable interrupt, clear
		905	out ADCSR,B_Temp ; interrupt flag, free-running.
		906
		907	ret
		908
		909
		910	;*****************************************************************************************
		911	;*****************************************************************************************
		912	;* Interrupt Service routines. *
		913	;* These routines can occur assynchronously. Therfore, they might occur during a second *
		914	;* level routine. Therefore THEY MAY NOT CALL ANY SUBROUTINES. *
		915	;*****************************************************************************************
		916	;*****************************************************************************************

Subversion Repositories Kolibri OS

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