picservo.asm

Code ASM for PIC16F628. Use for CNC/Servo Motor. With simulatio in Proteus (7.0)

	BSF		Flag_ServoPulseSafe
	BCF		INTCON, T0IF
	BCF		INTCON, T0IE		; mask the timer interrupt
	GOTO	Int_Finish
	
	; Our long delay has just expired. Disable CTS and setup for the short delay
Int_LongDelayFinished
	BSF		Serial_CTS
	BSF		Flag_WaitingForCTS
	MOVLW	RTCC_1msValue
	MOVWF	TMR0
	BCF		INTCON, T0IF
	BSF		INTCON, T0IE		; unmask the timer interrupt

Int_Finish
	; Restore the STATUS and W registers
	; Note that this is the only way to do it properly
	MOVF	Int_STATUS_Save, w
	MOVWF	STATUS
	SWAPF	Int_W_Save, f
	SWAPF	Int_W_Save, w

	RETFIE							; return from interrupt and reset GIE 



;+-----------------------------------------------------------------------------
;| SUBROUTINE:	DelayWByFour
;|
;| Wait the number of 4 instruction cycles given in the W register. Note that
;| a value of zero will actually result in a delay of 256*4 instructions 
;| because the loop decrements W each iteration as the first instruction, 
;| so the zero will rollover to 255 and the loop will continue until it hits
;| zero again.
;| 
;| This version of the Delay add no extra cycles to the loop.
;| 
;| The numbers in brackets indicate the number of instructions in a normal
;| loop execution. However there are fixed costs to calling this routine which
;| need to be accounted for if accurate timing is to be generated.
;|
;| Each time the instruction pointer is adjusted (by a GOTO, CALL, RETURN etc)
;| the CPU stalls for an extra cycle resulting in an instruction taking 2 cycles.
;| Note that when the bit test results in a skip, the GOTO is effectivly treated
;| as a NOP so the loop consists of only 3 instructions before the return so 
;| we need to add an extra NOP to bring the total in the iteration up to 4
;|
;| Therefore the following are the extras that need to be accounted for:
;|		Call Entering:		2
;|		Call Return:		2
;|		---------------------
;|		Total:				4
;|
;| Meaning that one less loop is actually required to get the correct timing.
;|
;| Some timing examples, the number of cycles that "CALL DelayWByFour" takes:
;|		W		cycles
;|		--------------
;|		0		4+256*4	= 1028
;|		1		4+1*4	= 8
;|		2		4+2*4	= 12
;|		3		4+3*4	= 16
;|		...
;|		255		4+255*4	= 1024
;|
;+-----------------------------------------------------------------------------
DelayWByFour
	ADDLW	0FFh					; (1) Subtract one from the W register. 
	BTFSS	STATUS, Z				; (1) If we are at zero, then exit the loop
	GOTO	DelayWByFour			; (2) loop
	NOP								; (1) adjust final loop to be 4 instructions
	RETURN	


;+-----------------------------------------------------------------------------
;| SUBROUTINE:	DoServoControlPulse
;| 
;| Send a control pulse for a servo if it is active. This routine relies on the
;| caller setting the FSR (indirect addressing register) to the servo mask
;| for the correct server. It will then read the information it needs from it,
;| increment the FSR and access the servo's offset and position loop counts.
;|
;| Call with W between 0 and 3 indicating which servo to work with.
;+-----------------------------------------------------------------------------
DoServoControlPulse
	; convert the servo number in W to an offset for the Mask byte data for
	; the servo
	CALL	GetAddressOfServoData
	MOVWF	FSR

	; Get the Output Bit setting mask
	MOVF	INDF, w				; get the mask byte
	MOVWF	CurrentServoMask

	; Modify the FSR to point to the next byte of data which is the
	; offset loop counter, this is the number of 4 cycle
	; loops to preform.
	INCF	FSR, f

	; Set the control bit high to begin the control pulse. Note that from
	; now until it is set to low is part of the timimg pulse so instructions
	; are critical. The number in brackets is the number of EXTRA cycles each 
	; instruction uses which are not directly related to the pulse time and
	; need to be adjusted for This is easily done with the Offset value. 
	; Note that W still contains the output mask generated above.
	IORWF	PORTB, f			; bit is set, any other are left unchanged
								; pulse time starts at the end of this instruction

	; Offset Part of the Pulse 
	MOVF	INDF, w				; (1) Load the offset count
	ADDLW	1					; (1) Incremnt the count - 255->0, 0->1 because of 
								; calling convention of DelayWByFour
	CALL	DelayWByFour		; (8+) Delay 

	; Position Part of the Pulse
	INCF	FSR, f				; (1) Access the next value which is the 
								; position loop count

	MOVF	INDF, w				; (1) Load the variable loop count
	ADDLW	1					; (1) Incremnt the count - 255->0, 0->1 because of 
								; calling convention of DelayWByFour
	CALL	DelayWByFour		; (8+) Delay
	

	; Turn off the output bit to end the pulse
	MOVF	CurrentServoMask, w	; (1) Get the mask
	XORWF	PORTB, f			; (1) Turn of the pulse
		
	; and return from the call.
	RETURN


;+-----------------------------------------------------------------------------
;| SUBROUTINE: DisableInterrupts
;| 
;| Disable interrupts. This is not just as simple as BCF GIE because of the way
;| that interrupts are triggered it is possible for an interrupt to occur 
;| during the execution of the BCF GIE instruction which causes the PIC to start
;| the interrupt handler which then returns with RETFIE which sets the GIE flag
;| again!
;+-----------------------------------------------------------------------------
DisableInterrupts
	BCF		INTCON, GIE			; disable the interrupt
	BTFSC	INTCON, GIE			; check if GIE is still disabled
	GOTO	DisableInterrupts	; it isn't, so try again
	RETURN


;+-----------------------------------------------------------------------------
;| Main Program, Setup and Constantly Loop
;+-----------------------------------------------------------------------------
Main
	BCF		STATUS, RP0				; Set the lower RAM bank as the default.
	CALL	DisableInterrupts		; disable interrupts until were setup
	

	; Setup the PIC's ports, PortB is all outputs, PortA is the Serial interface
	CLRF	PORTA					; Clear the port outputs
	CLRF	PORTB				

	BSF		STATUS, RP0				; Set the Upper RAM bank while we init the ports
	MOVLW	0Ah						; All outputs except RA3 and RA1
	MOVWF	TRISA					; Set the port directional flags	
				
	MOVLW	00h						; Port B All outputs
	MOVWF	TRISB					; Set the port directional flags
	
	BCF		STATUS, RP0				; Set the lower RAM bank as the default.

	; Turn off the LED
	BSF		LED_Drive

	; initialise the serial port
	CALL	InitSerialPort

	; Transmit the power on welcome message (interrupts already disabled)
	CLRF	DataByte
PwrOnMsgLoop
	MOVF	DataByte, w
	CALL	GetPWROnMsg
	ANDLW	0ffh					; check for end of message
	BTFSC	STATUS, Z
	GOTO	EndPwrOnMsgLoop			; was 0, exit
	CALL	TransmitDataByte
	INCF	DataByte, f
	GOTO	PwrOnMsgLoop
EndPwrOnMsgLoop

	; Initialise the Servo values to resonable defaults and set their
	; mask values
	MOVLW	01h
	MOVWF	Servo0_Mask
	MOVLW	02h
	MOVWF	Servo1_Mask
	MOVLW	04h
	MOVWF	Servo2_Mask
	MOVLW	08h
	MOVWF	Servo3_Mask
	MOVLW	10h
	MOVWF	Servo4_Mask
	MOVLW	20h
	MOVWF	Servo5_Mask
	MOVLW	40h
	MOVWF	Servo6_Mask
	MOVLW	80h
	MOVWF	Servo7_Mask
	CLRF	Flags				; initialise all flags
	CALL	ResetState			; initialise all outputs and state

	; Setup the Real time counter so it runs on the instruction
	; clock and triggers and interrupt every 20ms or so. This
	; then calls the interrupt routine which serives all the servo
	; outputs. This is then enabled or disabled by the main loop when
	; it is bit banging the serial input.

	; Assign the prescaler to the timer and to 1:128 scale. This
	; also enables the PORTB pullups.
	BSF		STATUS, RP0				; Set the Upper RAM bank while we init the ports
	MOVLW	86h						; binary: 0000 0110
	MOVWF	OPTION_REG				; Set the OPTION register
	BCF		STATUS, RP0				; Set the lower RAM bank as the default.

	; Setup the RTCC for the 19ms interrupts
	MOVLW	RTCC_19msValue
	MOVWF	TMR0
	BCF		INTCON, T0IF
	
	; Enable the Timer 0 Interrupt and disable all others
	CLRF	INTCON				; mask out all interrupts
	BSF		INTCON, T0IE		; unmask the timer interrupt

	; Enable interrupts
	BSF		INTCON, GIE

	; Set the CTS signal active so the host can send us data
	BCF		Serial_CTS	

	; Wait for the start bit (a high to low transition on Serial_RX)
	; or for the servo pulse safe flag to be activated.
StartBitLoop
	BTFSS	Serial_RX
	GOTO	StartReceiveData

	; Test for servo pulse safe flag. If set, start the servo pulses, otherwise
	; go back and test for a start bit again.
	BTFSS	Flag_ServoPulseSafe
	GOTO	StartBitLoop
	GOTO	ProcessServos

StartReceiveData
	; possible start bit, receive a byte
	CALL	DisableInterrupts
	CALL	ReceiveDataByte
	MOVWF	DataByte
	BSF		INTCON, GIE

	; check if it is valid. If it is send it straight back
	BTFSS	Serial_ReceiveValid
	GOTO	StartBitLoop		; invalid, continue looking for a start bit
	
	; light the LED
	MOVLW	LED_Drive_Time
	MOVWF	LED_Drive_Count
	BCF		LED_Drive

	MOVF	DataByte, w
	CALL	ParseCommand

	GOTO	StartBitLoop

ProcessServos
	; Process each of the servos and send the control pulse if needed.
	; We move the address of the Servo's Control data into the FSR and call
	; the DoServoControlPulse subroutine.
	BCF		Flag_ServoPulseSafe

	; Check that Servo 0 is enabled, skip if it isn't
	BTFSS	Flag_Servo0_Active
	GOTO	DoServo1
	MOVLW	0						; Servo 0
	CALL	DoServoControlPulse

DoServo1
	BTFSS	Flag_Servo1_Active
	GOTO	DoServo2
	MOVLW	1						; Servo 1
	CALL	DoServoControlPulse
	
DoServo2
	BTFSS	Flag_Servo2_Active
	GOTO	DoServo3
	MOVLW	2						; Servo 2
	CALL	DoServoControlPulse

DoServo3
	BTFSS	Flag_Servo3_Active
	GOTO	DoServo4
	MOVLW	3						; Servo 3
	CALL	DoServoControlPulse

DoServo4
	BTFSS	Flag_Servo4_Active
	GOTO	DoServo5
	MOVLW	4						; Servo 4
	CALL	DoServoControlPulse

DoServo5
	BTFSS	Flag_Servo5_Active
	GOTO	DoServo6
	MOVLW	5						; Servo 5
	CALL	DoServoControlPulse

DoServo6
	BTFSS	Flag_Servo6_Active
	GOTO	DoServo7
	MOVLW	6						; Servo 6
	CALL	DoServoControlPulse

DoServo7
	BTFSS	Flag_Servo7_Active
	GOTO	DoLED
	MOVLW	7						; Servo 7
	CALL	DoServoControlPulse

DoLED
	; If the LED Drive counter is not zero, check if it has timed out so needs disabling
	MOVF	LED_Drive_Count, f
	BTFSC	STATUS, Z
	GOTO	ResetTimer

	; Decrement the LED counter and if it hits zero, turn off the LED
	DECFSZ	LED_Drive_Count, f
	GOTO	ResetTimer				; not zero, contine with next bit

	; Turn off LED
	BSF		LED_Drive

ResetTimer
	; Clear the interrupt flag so we don't interrupt again as soon as we exit
	; and reset the RTCC register to wait for another 19ms
	MOVLW	RTCC_19msValue
	MOVWF	TMR0
	BCF		INTCON, T0IF
	BSF		INTCON, T0IE		; unmask the timer interrupt

	; Reenable the CTS flag so the computer can send us more data
	BCF		Serial_CTS

	; return to looking for a start bit
	GOTO StartBitLoop

;+-----------------------------------------------------------------------------
;| SUBROUTINE: ResetState
;| 
;| Set all the outputs to the reset state. That is:
;|	1. All digital output Off
;|	2. All Servo outputs disabled
;|	3. All Servo Offsets to 128 (midrange)
;|	3. All Servo Positions to 128 (midrange)
;+-----------------------------------------------------------------------------
ResetState
	; Clear all the servo enables (all servos off)
	CLRF		Enables

	MOVLW		128
	MOVWF		Servo0_Offset
	MOVWF		Servo1_Offset
	MOVWF		Servo2_Offset
	MOVWF		Servo3_Offset
	MOVWF		Servo4_Offset
	MOVWF		Servo5_Offset
	MOVWF		Servo6_Offset
	MOVWF		Servo7_Offset
	MOVWF		Servo0_Position
	MOVWF		Servo1_Position
	MOVWF		Servo2_Position
	MOVWF		Servo3_Position
	MOVWF		Servo4_Position
	MOVWF		Servo5_Position
	MOVWF		Servo6_Position
	MOVWF		Servo7_Position

	; Turn everything off
	CLRF	PORTB

	; Transmit the power on welcome message
	CALL	DisableInterrupts