motordrv.asm

VCP201_CODE is a FPGA source code.

;---------------------------------------------------
; motordrv
; - motor controlling both in protocol 1.0 & 2.0
; - call the syn_data to work for 49 & 4a command
; sonycmd0 - small motor
; sonycmd1 - large motor
; variables used : temp_buf1
;---------------------------------------------------
motordrv:
	btfsc	f_vib
	goto	both_motor_off		; motor OFF
	
        btfss   f_nmode         	; protocol 2.0 ?
        goto    motor_protocol_1       	; no

        call    syn_data		; synchronous mode checking

	bsf	STATUS,RP0
        btfss   sm_cmd,0        	; 01 bit set ?
        goto    s_motor_off
        bcf	STATUS,RP0
        bsf     s_motor         	; ON small motor
        goto    l_motor_set

s_motor_off:
	bcf	STATUS,RP0
        bcf     s_motor
        
l_motor_set:
	bsf	STATUS,RP0
        movf    lm_cmd,W
        bcf	STATUS,RP0
        movwf	pwmduty

        return

motor_protocol_1:		; ps protocol 1.0
	bsf	STATUS,RP0
        btfsc   sonycmd0,7
        goto    motor_act
        btfss   sonycmd0,6
        goto    motor_act
				; in motor setting mode
	btfsc	sonycmd1,0	; ON ?
	goto	motor_command	
	bcf	STATUS,RP0
	bcf	f_old_vib_on	; OFF
	goto	motor_act

motor_command:
	bcf	STATUS,RP0	
	bsf	f_old_vib_on
	bsf	f_old_vib_count

motor_act:			; motor driving 
	bcf	STATUS,RP0
	btfss	f_old_vib_on
	goto	motor_old_off
	bsf	s_motor		; ON
	goto	end_motor_protocol_1

both_motor_off:
motor_old_off:
	bcf	s_motor		; OFF	
end_motor_protocol_1:
	clrf	pwmduty		; OFF large motor
        return


;------------------------------------------------------------------
; syn_data
; 1. If synchronous mode is ON:
;    chk if last command is 4a : Yes - send motor buffers out
;		     	         No - nothing to do
; 2. If synchronous mode is OFF:
;    chk if last command is 49 : Yes - send 49's motor command out.
;				 No - nothing to do
;
; variables affected : sm_buffer, lm_buffer, lm_cmd, sm_cmd
;------------------------------------------------------------------
syn_data:
        btfss   f_synchronous
        goto    syn_is_OFF
        movf    last_command,W
        xorlw   0x4a           ; 4a?
        btfss   STATUS,Z
        goto	end_syn_data    ; not 4a & synchronous is ON
syn_command_is_4a:
        bsf     STATUS,RP0
        movf    lm_buffer,W
        movwf   lm_cmd

        movf    sm_buffer,W
        movwf   sm_cmd
	bcf	STATUS,RP0
	        
        goto	end_syn_data

syn_is_OFF:
	movf	last_command,W
	xorlw	0x49		; 49 ?
	btfss	STATUS,Z
	goto	end_syn_data	; No	
	
	btfss	f_data1_is_0	; set actuator 0 ?
	goto	syn_49_actuator_not_0
	bsf	STATUS,RP0	; yes
	movf	sm_buffer,W
	movwf	sm_cmd
	bcf	STATUS,RP0
	goto	end_syn_data
	
syn_49_actuator_not_0:
	btfss	f_data1_is_1	; set actuator 1 ?
	goto	end_syn_data
	bsf	STATUS,RP0	; yes
	movf	lm_buffer,W
	movwf	lm_cmd
	bcf	STATUS,RP0
		
end_syn_data:
        return