;	Motor Control Chip Firmware
;	Designed for PIC16C620 chip
;	See design write-up for algorithm notes and pinouts
;
	LEFT_FWD EQU 0x20
	LEFT_REV EQU 0x10
	LEFT_MASK EQU 0x30
	RIGHT_FWD EQU 0x80
	RIGHT_REV EQU 0x40
	RIGHT_MASK EQU 0xC0
	LEFT_MOTOR EQU 0x01
	RIGHT_MOTOR EQU 0x02
	SWITCH_TIME EQU 0x01
	TMR0_START EQU 0xE6

	TIME_COUNT EQU 0x20
	W_SAVE EQU 0x21
	MOTOR_STATE EQU 0x22
	TEMP EQU 0x23
	DESIRED_STATE EQU 0x24
	LAST_COMMAND EQU 0x25
	RELAY_MASK EQU 0x26

	bcf RP0			;  Select Bank 0
	goto init		;  Skip past the interrupt vector

INTERRUPT
	org 0x0004
	bcf INTCON, T0IF	;  Clear the Timer 0 interrupt
	bcf INTCON, T0IE	;  Disable the Timer 0 interrupts
	movwf W_SAVE		;  Save the value of the W register
	movf TIME_COUNT		;  If TIME_COUNT is zero
	btfsc STATUS,Z		;      leave relays alone
	goto EXIT_INTERRUPT
	decf TIME_COUNT		;  Decrement the TIME_COUNT variable
	btfss STATUS,Z		;  If not zero, leave relay coils energized
	goto EXIT_INTERRUPT
	movf PORTB,W		;  Get the current state of the PORTB register
	andlw 0x0F		;  Clear the relay coil lines
	movwf PORTB		;  Send the relay state to the pins
	movf DESIRED_STATE,W	;  Get the desired motor condition
	andwf RELAY_MASK, W	;  Mask off the unused relays
	iorwf MOTOR_STATE	;  The relay state should match desired (assumes was "unknown" 00)
	movf DESIRED_STATE,W	;  Get the desired motor condition
	andlw 0x0F		;  Mask off the direction bits
	iorwf MOTOR_STATE	;  The motor state will match the desired state once output (assumes was "off" 00)
	movwf PORTA		;  Send the motor commands to the port
EXIT_INTERRUPT
	movf W_SAVE,W		;  Restore the value of the W register
	retfie			;  Return from the interrupt

	org 0x0020
init	movlw 0x07		;  Turn off the comparitors
	movwf CMCON

	bsf RP0			;  Select Bank 1
	movlw 0xC8		;  Set the prescaler to TMR0=Fosc/2
	movwf OPTION		;  Store the configuration into the option register

	movlw 0x00
	tris 5			;  Set the A port to all outputs except for the interrupt line
	movlw 0x0F
	tris 6			;  Set the B port to inputs for the control lines, outputs for the relay coil lines
	bcf RP0			;  Select Bank 0

	movlw 0x00		;
	movwf PORTA		;  Turn off both motors
	movwf TMR0		;  Clear Timer 0
	movwf LAST_COMMAND	;  Set the last command to "stop"
	movwf DESIRED_STATE	;  Set the desired state to all off
	movwf RELAY_MASK	;  The relays really don't matter for a "stop"

	movlw 0xA0		;
	movwf INTCON		;  Allow only TMR0 interrupts

	movlw LEFT_FWD | RIGHT_FWD	;  Set relays to initial state
	movwf MOTOR_STATE	;  Set the initial motor state
	movwf PORTB		;  Send the relay state to the pins
	movlw TMR0_START	;  Initialize the timer to count
	movwf TMR0		;    25 instructions (@6200 Hz = 4msec)
	movlw SWITCH_TIME	;  Set the timer to turn off the relay
	movwf TIME_COUNT	;    coils after the required switch time
INIT_WAIT
	movf TIME_COUNT		;  If TIME_COUNT is not zero
	btfss STATUS,Z		;      branch back because
	goto INIT_WAIT		;      the relays haven't settled

MAIN_LOOP
	movf PORTB,W		;  Get the current command
	andlw 0x0E		;  Mask off all but the command bits
	movwf TEMP		;  Put the bits into a register for manipulation
	rrf TEMP, W		;  Rotate the command to be 0 based
	movwf TEMP		;  Save the command for later
	btfss STATUS, Z		;  If 0 (stop), turn off the motors
	goto CHECK_COMMAND	;
	movwf PORTA		;  Turn off both motors
	bcf MOTOR_STATE, 0	;  Remember right motor off
	bcf MOTOR_STATE, 1	;  Remember left motor off
	movwf LAST_COMMAND	;  Set the last command to "stop"
	goto MAIN_LOOP		;  Branch back and do it again!
CHECK_COMMAND
	xorwf LAST_COMMAND,W	;  Check for changed command
	btfsc STATUS, Z		;  If the command hasn't changed go back
	goto MAIN_LOOP		;
	movf TEMP,W		;  Retrieve the saved motor command
	movwf LAST_COMMAND	;  Set the last command from the motor command
	call GET_MOTOR_CONTROL  ;  Get the motor setting for the command
	movwf DESIRED_STATE	;  Save the motor control for later
	movlw 0x00		;  Clear relay mask
	btfsc DESIRED_STATE, 0  ;  If the left motor is on
	movlw LEFT_MASK		;    add the left mask to the relay mask
	btfsc DESIRED_STATE, 1  ;  If the right motor is on
	iorlw RIGHT_MASK	;    add the right mask to the relay mask
	movwf RELAY_MASK	;  Store the relay mask bits
	andwf DESIRED_STATE,W   ;  Get the relay bits that matter
	xorwf MOTOR_STATE,W	;  Compare the current relay state with the desired state
	andwf RELAY_MASK,W	;  Mask off the motor on/off bits
	movwf TEMP		;  Save the relay bit differences for later
	btfss STATUS, Z		;  If only the motor on/offs changed, things are easy
	goto DIR_CHANGED
	movf DESIRED_STATE,W	;  Get the motor control bits
	andlw 0x0F;		;  Mask off the relay bits
	iorwf MOTOR_STATE	;  Once output, the motor state should match desired
	movwf PORTA		;  Send the motor commands to the port
	goto MAIN_LOOP		;  Branch back and do it again!
DIR_CHANGED
	movlw 0x00		;  Turn off both motors while the direction changes
	movwf PORTA		;  Send the motor commands to the port
	movf TEMP,W		;  Get the relay bits that change
	xorlw 0xFF		;  Invert to get the mask for relay bits that don't change
	andwf MOTOR_STATE,W	;  Get the relay states that don't need to change
	andlw 0xFC		;  Set the motor status bits to "off"
	movwf MOTOR_STATE	;  Set the affected relay states to "unknown" and motors to "off"
	movf TEMP,W		;  Get the bits that changed for the relays
	andwf DESIRED_STATE,W	;  Use the changed bits to mask off the relay commands
	movwf PORTB		;  Start the relay energization
	movlw TMR0_START	;  Initialize the timer to count
	movwf TMR0		;    25 instructions (@6200 Hz = 4msec)
	movlw SWITCH_TIME	;  Set the timer to turn off the relay
	movwf TIME_COUNT	;    coils after the required switch time
	bcf INTCON, T0IF	;  Clear the Timer 0 interrupt
	bsf INTCON, T0IE	;  Enable the Timer 0 interrupts
	goto MAIN_LOOP		;  Branch back and do it again!

	org 0x01F0
GET_MOTOR_CONTROL
	clrf PCLATH		;  Set the page to 1
	incf PCLATH		;  Set the high byte for the goto address
	addlw low(MOT_0)	;  Add the offset to the command in W
	movwf PCL		;  Go to the address
MOT_0	retlw 0x00				;  Motor 0 commands
	retlw LEFT_FWD + LEFT_MOTOR		;  Motor 1 commands
	retlw RIGHT_FWD + RIGHT_MOTOR		;  Motor 2 commands
	retlw LEFT_FWD+RIGHT_FWD+LEFT_MOTOR+RIGHT_MOTOR	;  Motor 3 commands
	retlw LEFT_REV + LEFT_MOTOR		;  Motor 4 commands
	retlw LEFT_REV+RIGHT_FWD+LEFT_MOTOR+RIGHT_MOTOR	;  Motor 5 commands
	retlw LEFT_FWD+RIGHT_REV+LEFT_MOTOR+RIGHT_MOTOR	;  Motor 6 commands
	retlw LEFT_REV+RIGHT_REV+LEFT_MOTOR+RIGHT_MOTOR	;  Motor 7 commands