sub.asm

基本完成迷控制算法

;================================
;	User's subroutine	|
;================================
const_init:
	mov	a,#5
	mov	intwk00,a
	mov	a,#8
	mov	motorstep,a
	ret
;.data
;motorflag	ds	1
;	forward_f	equ	motorflag.0
;	backward_f	equ	motorflag.1
;	leftward_f	equ	motorflag.2
;	rightward_f	equ	motorflag.3
;	upward_f	equ	motorflag.4
;	downward_f	equ	motorflag.5
;ledflag	ds	1
;	leftled_f	equ	ledflag.0
;	rightled_f	equ	ledflag.1
;forback_pwm	ds	2
;leftright_pwm	ds	2
;updown_pwm	ds	2
;motorstep	ds	1
;Noforbackval		equ	#00000000b	;	
;forwardval		equ	#00001111b	;1:1
;fastforwardval		equ	#00111111b	;3:1
;backwardval		equ	#00001111b	;1:1
;slowbackwardval	equ	#00000011b	;1:3
;Noleftrightval		equ	#00000000b	;
;leftrightval		equ	#00000111b	;3:5
;Noupdownval		equ	#00000000b	;
;updownval		equ	#00000111b	;3:5
update_information:				;Note: call at @@main
	bts0	RF_receive_finish
	jmp	update_information10
	bts0	@Sys200msIRQ
	jmp	update_information80
	jmp	update_information90
update_information10:
	bclr	RF_receive_finish
	mov_	@Sys200msC,@Sys200msStep 	;reset the steps of 200msC
        b0bclr  @Sys200msIRQ
update_information50:
	mov	a,RF_receiveRam
	and	a,#00000111b
	cja	a,#4,update_information80
	@JMP_A	5
	jmp	No_forback_process	;a=0
	jmp	forward_process		;a=1
	jmp	fastforward_process	;a=2
	jmp	slowbackward_process	;a=3
	jmp	backward_process	;a=4
No_forback_process:
	bclr	forward_f
	bclr	backward_f
	mov	a,Noforbackval
	mov	forback_pwm,a
	Stop_Music		;//////////////
	jmp	update_information60
forward_process:
	bset	forward_f
	bclr	backward_f
	mov	a,forwardval
	mov	forback_pwm,a
	Play_Music2		;///////////
	jmp	update_information60
fastforward_process:	
	bset	forward_f
	bclr	backward_f
	mov	a,fastforwardval
	mov	forback_pwm,a
	Play_Music3		;/////////////
	jmp	update_information60
slowbackward_process:	
	bclr	forward_f
	bset	backward_f
	mov	a,slowbackwardval
	mov	forback_pwm,a
	Play_Music4		;////////////
	jmp	update_information60
backward_process:
	bclr	forward_f
	bset	backward_f
	mov	a,backwardval
	mov	forback_pwm,a
	Play_Music4		;////////////
;	jmp	update_information60
update_information60:	
	mov	a,RF_receiveRam
	mov	wk00,a
	rrcm	wk00
	rrcm	wk00
	rrcm	wk00
	mov	a,wk00
	and	a,#00000111b      
	cja	a,#4,update_information80
	@JMP_A	5
	jmp	NOledupdown_process	;a=0
	jmp	leftward_process	;a=1      
	jmp	rightward_process	;a=2
	jmp	upward_process		;a=3
	jmp	downward_process	;a=4
NOledupdown_process:	
	bclr	leftled_f
	bclr	rightled_f
	bclr	leftward_f
	bclr	rightward_f
	bclr	upward_f
	bclr	downward_f
	mov	a,Noleftrightval
	mov	leftright_pwm,a
	mov	a,Noupdownval
	mov	updown_pwm,a
	jmp	update_information90
leftward_process:
	bset	leftled_f
	bclr	rightled_f
	bset	leftward_f
	bclr	rightward_f
	bclr	upward_f
	bclr	downward_f
	mov	a,leftrightval
	mov	leftright_pwm,a
	mov	a,Noupdownval
	mov	updown_pwm,a
	jmp	update_information90	
rightward_process:
	bclr	leftled_f
	bset	rightled_f
	bclr	leftward_f
	bset	rightward_f
	bclr	upward_f
	bclr	downward_f
	mov	a,leftrightval
	mov	leftright_pwm,a
	mov	a,Noupdownval
	mov	updown_pwm,a	
	jmp	update_information90
upward_process:	
	bclr	leftled_f
	bclr	rightled_f
	bclr	leftward_f
	bclr	rightward_f
	bset	upward_f
	bclr	downward_f
	mov	a,Noleftrightval
	mov	leftright_pwm,a
	mov	a,updownval
	mov	updown_pwm,a	
	jmp	update_information90
downward_process:
	bclr	leftled_f
	bclr	rightled_f
	bclr	leftward_f
	bclr	rightward_f
	bclr	upward_f
	bset	downward_f
	mov	a,Noleftrightval
	mov	leftright_pwm,a
	mov	a,updownval
	mov	updown_pwm,a	
	jmp	update_information90
update_information80:
	bclr	@Sys200msIRQ
	clr	RF_receiveRam
	bset	RF_receive_finish
;	jmp	update_information90
update_information90:
	ret
;---------------------------------------
;Motor control process		;Note: call at 1ms's process
Motor_process:
	decms	motorstep
	jmp	@f
	mov	a,#8
	mov	motorstep,a	;reset
	mov_	forback_pwm+1,forback_pwm
	mov_	leftright_pwm+1,leftright_pwm
	mov_	updown_pwm+1,updown_pwm
@@:	
	mov	a,motorflag
	and	a,#00111111b
	cmprs	a,#0
	jmp	$+2
	jmp	Motor_process80
	bts0	forward_f
	jmp	Motor_process10
		Motor_process10_ret:
	bts0	backward_f
	jmp	Motor_process20
		Motor_process20_ret:
	bts0	leftward_f
	jmp	Motor_process30
		Motor_process30_ret:
	bts0	rightward_f
	jmp	Motor_process40
		Motor_process40_ret:
	bts0	upward_f
	jmp	Motor_process50
		Motor_process50_ret:
	bts0	downward_f
	jmp	Motor_process60
		Motor_process60_ret:
	jmp	Motor_process90
Motor_process10:	;forward
	rrcm	forback_pwm+1
	jc	@f
	stop_forbackward_run
	jmp	Motor_process10_ret
    @@:	
	forward_run
	jmp	Motor_process10_ret
Motor_process20:	;backward
	rrcm	forback_pwm+1
	jc	@f
	stop_forbackward_run
	jmp	Motor_process20_ret
    @@:	
	backward_run
	jmp	Motor_process20_ret
Motor_process30:	;leftward
	rrcm	leftright_pwm+1
	jc	@f
	stop_leftrigntward_run
	jmp	Motor_process30_ret
    @@:
    	leftward_run	
	jmp	Motor_process30_ret
Motor_process40:	;rightward
	rrcm	leftright_pwm+1
	jc	@f
	stop_leftrigntward_run
	jmp	Motor_process40_ret
    @@:
    	rightward_run
	jmp	Motor_process40_ret
Motor_process50:	;upward
	rrcm	updown_pwm+1
	jc	@f
	stop_updownward_run
	jmp	Motor_process50_ret
    @@:
	upward_run	
	jmp	Motor_process50_ret
Motor_process60:	;upward
	rrcm	updown_pwm+1
	jc	@f
	stop_updownward_run
	jmp	Motor_process60_ret
    @@:
	downward_run	
	jmp	Motor_process60_ret
Motor_process80:
	stop_forbackward_run
	stop_leftrigntward_run
	stop_updownward_run
;	jmp	Motor_process90	
Motor_process90:
;	ret
	jmp	Motor_process_ret
;------------------------------------------
;Led control process:
Led_process:			;Note: call at 1s's process
	bts0	leftled_f
	jmp	Led_process10
	Close_leftled
	jmp	Led_process20
Led_process10:
	notb	O_left_Led
	b0bset	Om_left_Led
;	jmp	Led_process20
Led_process20:
	bts0	rightled_f
	jmp	Led_process30
	close_rightled
	jmp	Led_process90
Led_process30:
	notb	O_right_Led
	b0bset	Om_right_Led
	jmp	Led_process90
Led_process90:
	ret
;===========================================
;	System subroutine(user select)	   |
;===========================================
;System Error
@@System_Err:
       	jmp	@@System_Err	; wait for watchdog reset the system       	
;=====================================================