picservo.asm

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

	LIST   P=PIC16F84, R=DEC
	#INCLUDE "p16f84.inc"

;------------------------------------------------------------------------------
; ASSEMBLE With MPASM. available for free from http://www.microchip.com
;------------------------------------------------------------------------------
;
; Servo Controler Version 1.1
;	28/10/2001
;	- Fixed Serial receive data problem where the PIC could
;	  miss a byte by assuming that the PC isn't in the middle of 
;	  sending a byte as it deactivates CTS.
;
; This program will control 8 servos connected to PORT B of the
; PIC microcontroler.
;
; The servos work by responding to a pulse presented to them 
; approximately every 20ms. The width of the pulse controls the 
; position of the servo.
;
; In general, a pulse width of around 1520us is used as a neutral
; (centre) position indication. The pulse width is then increased
; or decreased from there. Generally it the pulse range goes from 
; 1 to 2 ms for full deflection (90 degree movement of horn.
;
; So we need to arrange timing to control the width of the output
; pulse. To do this we will run the PIC at 4MHz, this means that
; our instructions are 1us long (clock/4). We will arrange a delay
; loop to wait for a given number of clock cycles.
;
; Because servo aren't that accurate, we will divide the 1 to 2ms
; pulse range into 256 values, which if we work on a loop of 
; four instruction cycles we can get a loop between 4 and 1024 ms
; (the loop is constructed to run at least once)
;
; We then need to offset this (roughly) 0 to 1ms pulse to ensure
; that the servo is centered. We will do this by having an "offset"
; adjustment for each servo. This offset will control the width of
; the initial part of the pulse from 4 to 1024ms. This means that
; we can centre the servo using the offset and then move the servo
; +/-45 degrees from there. With careful use of the offset and
; position, it should be possible to use almost the full travel of
; the servo. 
;
; We will also need to be able to turn on and off the output of
; each servo driver.
;
; PORT B is configured:
;
;	RB0		Servo Control 0
;	RB1		Servo Control 1
;	RB2		Servo Control 2
;	RB3		Servo Control 3
;	RB4		Servo Control 4
;	RB5		Servo Control 5
;	RB6		Servo Control 6
;	RB7		Servo Control 7
;
; PORT A is configured:
;
;	RA0		Serial TX (output)
;	RA1		Serial Request To Send (input)
;	RA2		Serial Clear To Send (output)
;	RA3		Serial RX (input)
;	RA4		LED Drive (0=on, 1=off. Used to indicate data transmission)
;
; Control Data
;
; To control the servos and outputs we need to send commands to the PIC.
; These commands will come from the serial port of a host computer running
; at 2400 baud, Hardware flow control (using CTS/RTS) with eight data bits,
; one stop bit and no parity. (2400 8-N-1).
;
; The commands take the format of one or two bytes sent sequentially 
; the first being the command to execute the second containing the data 
; for the command if needed.
;
; The command byte is split into two "nibbles". The upper 4 bits are used to
; determine the command, the lower 4 bits are used to select the output channel
;
;     +-----+-----+-----+-----+-----+-----+-----+-----+
; MSB |  7  |  6  |  5  |  4  |  3  |  2  |  1  |  0  | LSB
;     | 128 |  64 |  32 |  16 |  8  |  4  |  2  |  1  | Decimal Value
;     +-----+-----+-----+-----+-----+-----+-----+-----+
;     | Command               | Channel Select        |   
;     +-----+-----+-----+-----+-----+-----+-----+-----+
;
; Note that the value (in hex or decimal) is added to the channel number
; to generate the command byte.
; 
; The Commands are:
;
;	Hex		Decimal		Meaning
;	---------------------------------------------------------------------------
;	0x00	0			Reset Device. All outputs off, servos disabled and 
;						offset and position values set to 128 (mid range).
;						Data byte MUST BE Zero. The Channel Value MUST BE Zero.
;						Special, SHOULD be followed by another zero byte, see
;						text below.
;
;	0x10	16			Set Servo Output "Positon Value". Data byte is the
;						value (between 0 and 255). The Channel Value must be
;						set in the lower 4 bits between 0 and 7.
;
;	0x20	32			Set Servo Output "Offset Value". Data byte is the 
;						value (between 0 and 255). The Channel Value must be
;						set in the lower 4 bits between 0 and 7.
;
;	0x30	48			Enable Servo Output. This starts the PIC generating
;						servo control pulses on the given channel. The Channel 
;						Value must be set in the lower 4 bits between 0 and 7.
;						No Data byte.
;
;	0x40	64			Disable Servo Output. This stops the PIC generating
;						servo control pulses on the given channel. The Channel 
;						Value must be set in the lower 4 bits between 0 and 7.
;						No Data byte.
;
;
; Parser State Control.
;
; Even with the hardware flow control where the PIC uses the CTS line to inhibit
; the host from sending more data when it can't receive it, there is a chance 
; the host and PIC will get out of step with commands and data bytes. This could
; occur because of either a bug in the host software or power being lost to the 
; PIC. This may result in the PIC attempting to treat a data byte as a command or
; a command as a data byte. Therefore some scheme is needed to ensure that the
; state of both can be quickly resynchronised.
;
; This will be done in two ways, the reset command and the rejection of unknown
; commands.
;
; If the PIC receives a byte it believes to be a command but is doesn't
; recognise the command, it will discard it and start looking for another
; command byte. This means that for more values of the data bytes it will
; ignore these as commands. Unfortunately not all however. It is therefore
; possible for the PIC to execute incorrent commands until it comes accross
; one that doesn't make sense. This means that it could skip a normal reset
; reset command if it was one byte long by reading it as a data byte.
;
; The reset command is two bytes long (a command and a data byte) both being
; zeros. However to ensure that the PIC is reset EVERY TIME the resent command
; it sent, it should be sent as THREE sequential zero bytes.
;
; The reason is that if the PIC is out of step and waiting for a data byte, it 
; will comsume the first zero as a data byte, take the next one as the command
; and the last one as the data byte. But what if it isn't out of sync? The
; parser in the PIC will know that a zero command requires a zero data byte
; to be valid, therefore it will receive the first zero as the command, the 
; second as the data and execute the reset. It will then get another zero byte
; which is the command for reset and be waiting for the corresponding zero data
; byte. However the host will start sending other valid command at this time
; and the parser will not receive a zero data byte. It then knows that it must
; discard the reset command and use the new byte as a command.
;

;+=============================================================================
;| Program Beginning.
;+=============================================================================

; Set the configuration, PUT Enabled, WDT Disabled, XT Oscillator, No Code Protection
;
	__CONFIG _XT_OSC & _PWRTE_ON & _WDT_OFF & _CP_OFF    


;+-----------------------------------------------------------------------------
;| Declare Variables 
;+-----------------------------------------------------------------------------

; We will have up to 4 servos connected, each one needs to store the
; number 4us loops for the postion and the number of loops for the
; offset. The values are ordered in memory one after the other to allow 
; the use of the indirect addressing to call a routine that will process 
; all servos. Because we are using the indirect addressing to access
; all the detail for the servo, we will need an extra value to specify
; the mask to use to set and clear the correct servo output.
;
; NOTE: These are assumed to start at the Ram_Base (0Ch) by the rest of
; the program so don't put any variables before these.
;
	
; Declare the start of RAM that we can use
RamBase				EQU		0x0C

	CBLOCK RamBase
	Servo0_Mask				; Mask for Servo 0 output
	Servo0_Offset			; loop count for Servo 0 offset
	Servo0_Position			; loop count for Servo 0 position

	Servo1_Mask				; Mask for Servo 1 output
	Servo1_Offset			; loop count for Servo 1 offset
	Servo1_Position			; loop count for Servo 1 position

	Servo2_Mask				; Mask for Servo 2 output
	Servo2_Offset			; loop count for Servo 2 offset
	Servo2_Position			; loop count for Servo 2 position

	Servo3_Mask				; Mask for Servo 3 output
	Servo3_Offset			; loop count for Servo 3 offset
	Servo3_Position			; loop count for Servo 3 position

	Servo4_Mask			; Mask for Servo 4 output
	Servo4_Offset			; loop count for Servo 4 offset
	Servo4_Position			; loop count for Servo 4 position

	Servo5_Mask			; Mask for Servo 5 output
	Servo5_Offset			; loop count for Servo 5 offset
	Servo5_Position			; loop count for Servo 5 position

	Servo6_Mask			; Mask for Servo 6 output
	Servo6_Offset			; loop count for Servo 6 offset
	Servo6_Position			; loop count for Servo 6 position

	Servo7_Mask			; Mask for Servo 7 output
	Servo7_Offset			; loop count for Servo 7 offset
	Servo7_Position			; loop count for Servo 7 position

	Int_W_Save				; Interrupt W register Store
	Int_STATUS_Save			; Interrupt STATUS register Store

	Flags					; eight flags, see the Flag_* macros
	Enables					; eight servo enable flags

	LED_Drive_Count			; used to count the number of 20ms blocks to keep
							; the LED on to indicate a data byte was received.

	CurrentServoMask		; the current servo mask

	DataByte				; somewhere to store a incoming data byte for processing

	Serial_CurrentByte		; the current byte being received or transmitted
	Serial_LoopCount		; the current bit loop counter

	Parser_Command			; the stored command
	Parser_Data				; the stored data byte
	Parser_Temp	
	ENDC

;+-----------------------------------------------------------------------------
;| Declare MACROs
;+-----------------------------------------------------------------------------

RTCC_19msValue		EQU		107		; leaves 148 'ticks' at 1:128 before the
									; RTCC register clocks over and generates 
									; an interrupt. ~19ms

RTCC_1msValue		EQU		247		; leaves 8 'ticks' at 1:128 before the
									; RTCC register clocks over and generates
									; an interrupt. ~1mS

LED_Drive_Time		EQU		10		; leave the LED on for approx 200ms
#DEFINE LED_Drive			PORTA,4

#DEFINE Serial_TX			PORTA,0
#DEFINE Serial_RX			PORTA,3
#DEFINE Serial_CTS			PORTA,2
#DEFINE Serial_RTS			PORTA,1

Serial_BitDelay		EQU		103
Serial_HalfBitDelay	EQU		52

; Flag Access macros
#DEFINE Serial_ReceiveValid	Flags,0	; set if the current byte received was valid

#DEFINE Parser_WaitForData	Flags,1	; If set the parser is waiting for a data byte
									; for the current command

#DEFINE Flag_ServoPulseSafe Flags,2	; Set once about 2 bit times have expired since the
									; CTS line was deactivated in preperation for doing
									; the servo pulses.

#DEFINE Flag_WaitingForCTS	Flags,3	; The CTS line was deactivated and we are waiting for
									; about 2 bit times to make sure we're not going to miss
									; a data byte.

#DEFINE Serial_RestoreCTS	Flags,4	; The Serial Transmit routine needs to disable CTS when
									; transmitting so it needs to remember to restore it

; servo enable outputs. NOTE These MUST be sequential starting at bit 0 as 
; it is assumed in the parser when enabling and disabling servos
#DEFINE Flag_Servo0_Active	Enables,0
#DEFINE Flag_Servo1_Active	Enables,1
#DEFINE Flag_Servo2_Active	Enables,2
#DEFINE Flag_Servo3_Active	Enables,3
#DEFINE Flag_Servo4_Active	Enables,4
#DEFINE Flag_Servo5_Active	Enables,5
#DEFINE Flag_Servo6_Active	Enables,6
#DEFINE Flag_Servo7_Active	Enables,7

;+-----------------------------------------------------------------------------
;| Beginning of Program 
;+-----------------------------------------------------------------------------
	ORG			0
	GOTO		Main				; Jump to the main entry point

;+-----------------------------------------------------------------------------
;| Beginning of Interrupt Routine 
;| Note that we jump to the interrupt routine so that we can place all our 
;| 'lookup' table at the beginning of the program memory to ensure that when
;| we do an ADD to the PCL register we aren't going to clock over the address
;| and thus stuffing up our instruction pointer
;+-----------------------------------------------------------------------------
	ORG			4
	GOTO		Interrupt

;+-----------------------------------------------------------------------------
;| SUBROUTINE:	ParserChannelToBitMask
;| 
;| Take the channel number as a binary number in the W register and change it
;| to a bit mask so that bit 0 is set if W=0, bit 1 is set of W=1 etc.
;| This can then be used to alter output ports etc.
;| 
;| Valid range for W is 0 to 7
;+-----------------------------------------------------------------------------
ParserChannelToBitMask
	ANDLW	07h						; Ensure that there is a max of 7.
	ADDWF	PCL, f					; Jump into the commands below to return
									; the correct value
	DT		0x01, 0x02, 0x04, 0x08, 0x10, 0x20, 0x40, 0x80

;+-----------------------------------------------------------------------------
;| SUBROUTINE: GetPWROnMsg
;| 
;| return the character at offset in the W register for the Power On Message
;| returns 0 if at the end of the string
;+-----------------------------------------------------------------------------
GetPWROnMsg
	ADDWF	PCL, f
	DT		"PIC Servo Controller\r\n", 0

;+-----------------------------------------------------------------------------
;| SUBROUTINE: GetResetMsg
;| 
;| return the character at offset in the W register for the Power On Message
;| returns 0 if at the end of the string
;+-----------------------------------------------------------------------------
GetResetMsg
	ADDWF	PCL, f
	DT		"Reset\r\n", 0

;+-----------------------------------------------------------------------------
;| SUBROUTINE:	GetAddressOfServoData
;|
;| Returns the acutal memory address of the first data byte for the given servo
;| This is the Mask value, it is then followed by the offset and position.
;|
;| Call with W between 0 and 3. This routine will ensure the value is 
;| within that range.
;+-----------------------------------------------------------------------------
GetAddressOfServoData
	ANDLW	07h						; Ensure that there is a max of 7.
	ADDWF	PCL, f					; Jump into the commands below to return
									; the correct value
	DT		Servo0_Mask, Servo1_Mask, Servo2_Mask, Servo3_Mask, Servo4_Mask, Servo5_Mask, Servo6_Mask, Servo7_Mask



;+-----------------------------------------------------------------------------
;| Interrupt Routine.
;+-----------------------------------------------------------------------------
Interrupt
	; Save the state of the W and STATUS registers
	; Note this is the only way to do it properly
	MOVWF	Int_W_Save
	MOVF    STATUS, w
	MOVWF	Int_STATUS_Save

	; Process the interrupt. Note that the only interrupt generated is the 
	; timer overflow.
	;
	; when the timer expires from the 19ms delay, we have to disable the CTS line
	; so that the computer will not send us any more data, however we have to
	; continue listening for data for a little while so that we don't miss any if
	; it starts to send just as we deactivate the CTS line.
	;
	; Therefore we will wait for a further 2 bit times (about 1ms) before we signal
	; the main routine that it is safe to process the servo pulses.

	; Check if we have just expired the short 1ms delay
	BTFSS	Flag_WaitingForCTS
	GOTO	Int_LongDelayFinished

	; our short delay has just expired, we need to signal to the
	; main loop that it is now safe to proceed with the servo pulses
	; and disable any further timer interrupts (the main loop will
	; set them up again when it is ready)
	BCF		Flag_WaitingForCTS