Linux Droids Blog

Experiencias y Proyectors robóticos de Sphinx

Programando el 16F84A para controlar 12 servos 24 julio, 2010

Filed under: Programación — Sphinx @ 10:34
Tags: , , ,

ProgramaciónUtilizando el hardware del artículo anterior voy a mostrar el programa para que podamos controlar los 12 servos programando el PIC 16F84A.

Pues básicamente la rutina que he utilizado se la debemos a Jaap Havinga, y no requiere ninguna modificación para hacer funcionar el circuito que os mostraba en el articulo anterior, controlando 12 servos.

El control lo haremos desde el ordenador, por el puerto serie, y lo configuraremos a una velocidad de 34800 Baudios, con 8 bits de datos, 1 bit de stop, sin paridad y  sin control de flujo.

El circuito que hemos diseñado tiene un reloj de 20 Mhz, lo cual como se comenta da la posibilidad de trabajar a esos 34800 Baudios, y una resolución de 1 a 239. Sin embargo, si no contamos con un 16F84A a 20Mhz, sino con la version de 4 Mhz (y su correspondiente cristal de 4 Mhz), también es posible utilizar esta rutina, pero nos limitaremos a una comunicación a 9600 Baudios, y una resolución de 1 a 39 pasos por servo.

La resolución que nos da la rutina es de 240 pasos para manejar el servo sobre 90 grados de rotación.

Para controlar cada servo desde el PC, enviaremos 2 bytes via serie:

1 – el primero contendrá el valor hexadecimal 0xf0 + el numero de servo que queremos controlar, es decir:

0xf0 para el servo 1

0xf1 para el servo 2

0xfb para el servo 12

2 – El segundo byte contendrá el valor de la posición que le queremos dar al servo indicado. Es decir, de 1 a 239.

Ejemplo: Le enviamos los byte 0xf5, 0x3e. Con estos datos le indicamos que sitúe el servo 6 en la posición central.

El controlador mantendrá al servo en la posición indicada hasta que le enviemos un nuevo valor para ese servo.

Si además queremos que los servos se sitúen en una posición determinada cuando el PIC vuelva a ser energizado, solo tenemos que enviarle el comando 0xff  seguido del valor 0x00. Esto es de gran ayuda si queremos que nuestra aplicación arranque siempre con los servos en una misma posición.

Y aquí va la rutina (utilizad los botones que aparecen en la esquina superior derecha del recuadro con el código para descargarlo o copiarlo):

; *********************************************************************
; Servo controller
;
; Jaap Havinga 16 aug 1999
; Revision 1    25 april 2000
; Revision 2    1 aug 2000	; serial input can be defined positive or inverted
; Revision 3    24 oct 2000	; speed doubled to 38400 baud, and resolution doubled also.
; Revision 4    27 dec 2000	; increased number of servo's to 12. (PB0-7 plus PA0-3)
; Revision 5    13 mar 2001	; PWD values are stored into eeprom, and read out during initialization

;
; servo outputs: PB0-7, PA0-3
; serial input : PA4,  rs232 pin 3 via 33k resistor (rs 232 pin 5 = ground)
; 4MHz:
; baudrate: 9600, no parity, 8 databits, 1 or 2 stopbits, no control flow
; 20MHz:
; baudrate: 38400, no parity, 8 databits, 1 or 2 stopbits, no control flow
;
; First sent addressbyte (== servo number),
; then the databyte (== position of servo shaft)
; address = f0..f7 == PB0-7, f8..fB == PA0-3
; data    = 0..36 @4Mhz, or 1..239 @ 20MHz (RANGE_TIME/TIMER_PERIOD)
;
;; PWD values are stored in eeprom, and read out during initialization
;; command for storing PWD's is address 0xff followed by data 0x00
;;
; *********************************************************************
	list	p=16F84A
	include <p16f84a.inc>

; genereer config data
 __config	_HS_OSC & _PWRTE_ON
; __config	_RC_OSC

#define BANK_LOW	bcf	STATUS,RP0
#define BANK_HIGH	bsf	STATUS,RP0

 ; serial input can be defined positive (input direct to standard RS232)
;      (NORMAL_SERIAL defined) or inverted (NORMAL_SERIAL NOT defined)
;;#define NORMAL_SERIAL

ifdef NORMAL_SERIAL
#define SER_BITTEST_H	btfss
#define SER_BITTEST_L	btfsc
else
#define SER_BITTEST_H	btfsc
#define SER_BITTEST_L	btfss
endif

; timer runs at 1Mhz, for 4MHz PIC version, baudrate = 9600
; timer runs at 5Mhz, for 20MHz PIC version, baudrate = 38400
; define HIGH_SPEED if running on 20 Mhz
#define HIGH_SPEED

ifdef HIGH_SPEED
#define TIMER_PERIOD	D'26'		; is defined by baudrate, resolution and the max. number of instructions per interrupt.
#define TIMER_PERIOD_SCALE	5	; real_timer_period  = 	TIMER_PERIOD/TIMER_PERIOD_SCALE	 usec
#define BAUDRATE D'38400'
else
#define TIMER_PERIOD	D'26'		; is defined by baudrate, resolution and the max. number of instructions per interrupt.
#define TIMER_PERIOD_SCALE	1       ; real_timer_period = 	TIMER_PERIOD/TIMER_PERIOD_SCALE	 usec
#define BAUDRATE D'9600'
endif

; pulse width modulation for rc-servo motors
#define MIN_PULSE	D'800'; in usec
#define MAX_PULSE	D'2000'; in usec
#define PULSE_RANGE	(MAX_PULSE - MIN_PULSE)

#define INIT_PULSE_COUNT	(MIN_PULSE*TIMER_PERIOD_SCALE/TIMER_PERIOD)
#define END_OF_PERIOD_COUNT	(PULSE_RANGE*TIMER_PERIOD_SCALE/TIMER_PERIOD)

#define TIMER_CORRECTION_VALUE	D'6'

#define	TIMERCOUNT	 (0xff-TIMER_PERIOD+TIMER_CORRECTION_VALUE)
#define	PWD_REGISTERS		D'8'
#define	PWD_REGISTERS_MASK	0x80

; serial interface
; baudrate is 9600/38400
; bit-duration of serial is  1/9600 = 104 usec
; bit-duration of serial is  1/38400 = 26 usec
; oversampling of serial signal should be at least 2 times , higher sampling rate is more reliable
; 1 bit-duration is n timer ticks: n = baudrate / (real_timer_period)
; correct for 2 extra substages!
#define SER_BIT_TIMER		((D'1000000'*TIMER_PERIOD_SCALE)/(BAUDRATE*TIMER_PERIOD))
#define	SER_BIT_TICKS		SER_BIT_TIMER-D'2'

; The first delay after detection of the startbit is 1.5* de bit-duration
; correct for 2 extra substages!
#define	SER_FIRST_BIT_TICKS	(SER_BIT_TIMER * D'3') / D'2' - D'2'

#define SER_BYTE_SIZE		D'8'

#define SER_RX_BIT		0x04	; A4 is receive bit

; *************** used registers *********************

PWD_BASE		equ	0x0B
PWD0			equ	0x0C
PWD1			equ	0x0D
PWD2			equ	0x0E
PWD3			equ	0x0f
PWD4			equ	0x10
PWD5			equ	0x11
PWD6			equ	0x12
PWD7			equ	0x13
PWD8			equ	0x14
PWD9			equ	0x15
PWD10			equ	0x16
PWD11			equ	0x17
PWD12			equ	0x18
PWD13			equ	0x19
PWD14			equ	0x1a
PWD15			equ	0x1b

PWD_STARTUP_COUNTER	equ	0x21
EndOfPeriodCount	equ	0x22
InitPulseCount		equ	0x23
ExtendedPulseCountA	equ	0x24
ExtendedPulseCountB	equ	0x25
PWD_REGISTER		equ	0x26
PWD_REGISTER_MASK	equ	0x27
PWD_STATE		equ	0x28
;
SER_STATE		equ	0x29
SER_TICK_COUNT		equ	0x2a
SER_BIT_COUNT		equ	0x2b
SER_PORTA		equ	0x2c
SER_DATA		equ	0x2d

SER_SET_DATA_ADDR	equ	0x2e
SERAddress		equ	0x2f
SERData			equ	0x30

DecExtRdyCnt		equ	0x31
counter1		equ	0x32

#define PWD_STARTUP_COUNT	30

; *************** EEPROM initial data *********************
	org	0x2100

	DE D'152', D'197', D'123'	; left hip, knee ankle
	DE D'104', D'55', D'145'	; right hip, knee ankle
	DE D'138'			; hip
	DE 0,0,0,0,0,0,0,0,0		; future use

; *************** Start of program *********************

	org	0x00
reset
	goto	init

	org	0x04
irq
	movlw	TIMERCOUNT		; reload timer
	movwf	TMR0;

	; tmr0 has increased with 6 cycles after irq occured,
	; also the reloading of tmr0 will stop the timer for 2 cycles
	; So the timer should be corrected by 4+2 = 6 cycles

	bcf	INTCON,T0IF		; enable irq again

PWD_STATE_DISPATCH
	movfw	PWD_STATE
	movwf	PCL

PWD_STATE_WAIT_PERIOD_END
	decfsz	EndOfPeriodCount,F		; dec main counter
	goto	SERIAL_DISPATCH			; not zero, continue

;; this state is split up in sections
;; to assure processingtime is within timer-period

PWD_STATE_PERIOD_ENDED
	movlw	PWD_CONTINUE1			; set new state
	movwf	PWD_STATE

	CLRWDT					; Clear watchdog! <18 ms!

	movlw	END_OF_PERIOD_COUNT
	movwf	EndOfPeriodCount 		; yes, reset periodcounter + set outputs high

	movlw	INIT_PULSE_COUNT
	movwf	InitPulseCount 		; yes, reset initpulsecounter + set outputs high

	bcf	STATUS,C		; clear carry before shift
 	rrf	PWD_REGISTER_MASK,F	; shift register bit to right
	goto 	SERIAL_DISPATCH

PWD_CONTINUE1
	movlw	PWD_CONTINUE2		; set new state
	movwf	PWD_STATE

	decfsz	PWD_REGISTER,F		; and decrement register counter
;;;;;	goto 	PWD_CHECK_STARTUP
	goto 	SERIAL_DISPATCH

	movlw	PWD_REGISTERS		; carry, so real reset register counter
	movwf	PWD_REGISTER
	movlw	PWD_REGISTERS_MASK
	movwf	PWD_REGISTER_MASK
	goto 	SERIAL_DISPATCH

;;;;PWD_CHECK_STARTUP
;;;;	decfsz	PWD_STARTUP_COUNTER,F		;  decrement startup counter
;;;;	goto 	SERIAL_DISPATCH
;;;;	movlw	PWD_STARTUP_COUNT
;;;;	movwf	PWD_STARTUP_COUNTER
;;;;	goto 	SERIAL_DISPATCH

; this state is a continuation of PWD_CONTINUE1
PWD_CONTINUE2

	movlw	PWD_CONTINUE3		; set new state
	movwf	PWD_STATE

	movlw	PWD_BASE
	addwf	PWD_REGISTER,W
	movwf	FSR
	movfw	INDF			; load wanted pwd count
	movwf	ExtendedPulseCountB 	; set to ExtendedPulseCountB

	goto 	SERIAL_DISPATCH

; this state is a continuation of PWD_CONTINUE2
PWD_CONTINUE3

	movlw	PWD_STATE_WAIT_INIT_PULSE	; set new state
	movwf	PWD_STATE

	movlw	(PWD_BASE+8)
	addwf	PWD_REGISTER,W
	movwf	FSR
	movfw	INDF				; load wanted pwd count
	movwf	ExtendedPulseCountA 		; set to ExtendedPulseCountA

	movfw	PWD_REGISTER_MASK		; set bit of registers A and B to 1
	movwf	PORTA
	movwf	PORTB

	goto 	SERIAL_DISPATCH

PWD_STATE_WAIT_INIT_PULSE
	decfsz	InitPulseCount,f
	goto 	SERIAL_DISPATCH

	movlw	PWD_STATE_WAIT_EXT_PULSE
	movwf	PWD_STATE
	movlw	D'2'
	movwf	DecExtRdyCnt
	goto 	SERIAL_DISPATCH

PWD_STATE_WAIT_EXT_PULSE

	decfsz	EndOfPeriodCount,F		; dec main counter
	decfsz	ExtendedPulseCountA,f
	goto 	DecExtB
	clrf	PORTA
	decfsz	DecExtRdyCnt,f			; ports are zero, so we're ready: set next state
	goto	DecExtB
	goto	DecExtRdy
DecExtB
	decfsz	ExtendedPulseCountB,f
	goto 	SERIAL_DISPATCH

	clrf	PORTB

	decfsz	DecExtRdyCnt,f			; ports are zero, so we're ready: set next state
	goto	SERIAL_DISPATCH

DecExtRdy
	movlw	PWD_STATE_WAIT_PERIOD_END
	movwf	PWD_STATE

; *************** SERIAL IRQ routines ***************

SERIAL_DISPATCH
	movfw	SER_STATE
	movwf	PCL

SER_STATE_DETECT_START

	SER_BITTEST_L	PORTA,SER_RX_BIT		; current value 0?
	retfie 						; no, wait

	movlw	SER_STATE_DETECT_START1	 		; wait for 1
	movwf	SER_STATE

 	retfie

SER_STATE_DETECT_START1
	SER_BITTEST_H	PORTA,SER_RX_BIT		; current value 1?
	retfie 						; no, wait

	movlw	SER_STATE_RECIEVE	 		;  start detected, so set state to recieve
	movwf	SER_STATE

	movlw	SER_FIRST_BIT_TICKS
	movwf	SER_TICK_COUNT				; reset tickcount

	movlw	SER_BYTE_SIZE				; how many bits to expect
	movwf	SER_BIT_COUNT

	retfie

;; this state is divided into 3 substages in order to ensure timely return of irq routine
SER_STATE_RECIEVE

	decfsz	SER_TICK_COUNT,f
 	retfie					; nothing to do

	movfw	PORTA				; get data
	movwf	SER_PORTA			; store for later use
	movlw	SER_BIT_TICKS
	movwf	SER_TICK_COUNT			; reset tickcount

	movlw	SER_STATE_RECIEVE1
	movwf	SER_STATE
	retfie

SER_STATE_RECIEVE1
	bcf     STATUS,C				; clear carry
	SER_BITTEST_H	SER_PORTA,SER_RX_BIT		; test data input
	bsf     STATUS,C				; set carry if data = 0
	rrf	SER_DATA,F				; rotate carry further to address

	movlw	SER_STATE_RECIEVE2
	movwf	SER_STATE
	retfie

SER_STATE_RECIEVE2

	movlw	SER_STATE_RECIEVE
	movwf	SER_STATE
	decfsz	SER_BIT_COUNT,f				; all bits done?
	retfie
	;  All done, next will be stop bit, time to process data
	movlw	SER_STATE_VERWERK_DATA	 		;next state is process data
	btfss	SER_SET_DATA_ADDR,1
	movlw	SER_STATE_VERWERK_ADDRESS		; or process address
	movwf	SER_STATE
	retfie

SER_STATE_VERWERK_ADDRESS

	movlw	SER_STATE_DETECT_START	 		;  address will be processed, so set state to detect start again
	movwf	SER_STATE

  	movfw	SER_DATA 			;; copy data to address field
  	movwf	SERAddress
 	andlw	0xf0
	sublw	0xf0
	btfsc	STATUS,Z			;; if not zero, so if high nibble <> 0xf0 then not to data bit.
	bsf	SER_SET_DATA_ADDR,1		;; set next byte target to data
 	retfie

SER_STATE_VERWERK_DATA

	movlw	SER_STATE_DETECT_START	 		;  data will be processed, so set state to detect start again
	movwf	SER_STATE
	clrf	SER_SET_DATA_ADDR		;; set next byte target to address

	movfw	SERAddress
 	andlw	0x0f				;; valid address = 0,1,2,..,15
 	addlw	PWD0				;; add the register-address to it
 	movwf	FSR
	sublw	(PWD0+0x0f)			;; test if adress 15 is called
	btfsc	STATUS,Z
	goto	write_to_eeprom			;; yes write pwd data to eeprom...
	movfw	SER_DATA 			;; no store data
 	movwf	INDF
 	retfie

write_to_eeprom
	movfw	SER_DATA 			;; extra test: data should be 0
	btfss	STATUS,Z
	retfie
	call	eewrite_pwd			;; write it!
	retfie

; *************** Read eeprom **************
; read address in w
; data in w returned
RD_EEPROM
	movwf	EEADR			;; set address to read
	BANK_HIGH
	bsf	EECON1,RD		;; fetch it
	BANK_LOW
	movfw	EEDATA			;; and read data
	return

; *************** write eeprom **************
; write address in w
; data indirect in indf
; note  IRQ is NOT switched on again
; 	since callee is  IRQ routine
WR_EEPROM
	movwf	EEADR			;; set address to write
	movfw	INDF			;; get data to write
	movwf	EEDATA			;; set data to write
	CLRWDT				;; clear watchdog
	BANK_HIGH
	bcf	INTCON,GIE		;; IRQ off
	bsf	EECON1,WREN		;; set write enable
	movlw	0x55			;; special sequence
	movwf	EECON2
	movlw	0xaa
	movwf	EECON2
	bsf	EECON1,WR		;; set write flag
;;;;	bsf	INTCON,GIE		;; enable irq again
waitwr_complete
	btfsc	EECON1,WR		;; poll wr flag
	goto	waitwr_complete		;; and wait until wr is low

	bcf	EECON1,WREN		;; disable write
	BANK_LOW
	return

; +++++++++++++++++++++++++++++++++++++++++++++
;; retrieve pwd data from eeprom
eeread_pwd
	movlw 	D'15'		;; Nb of addresses to read
	movwf	counter1	;; is our counter+ source address
	movlw	PWD15		;; get start destination address
	movwf	FSR		;; put it indirect register
eeread_loop
	movfw	counter1	;; get source address
	call	RD_EEPROM	;; get data
	movwf	INDF		;; save data
	movfw	counter1	;; is end reached?
	btfsc	STATUS,Z
	return			;; yes return
	decf	counter1,f	;; no dec counter + dest address
	decf	FSR,f
	goto 	eeread_loop	;; and loop again

;;+++++++++++++++++++++++++++++++++++++++++++++++

; +++++++++++++++++++++++++++++++++++++++++++++
;; write current pwd data to eeprom
eewrite_pwd
	movlw 	D'15'		;; Nb of addresses to read
	movwf	counter1	;; is our counter+ destination address
	movlw	PWD15		;; get start source address
	movwf	FSR		;; put it indirect register

eewrite_loop
	movfw	counter1	;; get destination address
	call	WR_EEPROM	;; write data
	movfw	counter1	;; is end reached?
	btfsc	STATUS,Z
	return			;; yes return
	decf	counter1,f	;; no dec counter + dest address
	decf	FSR,f
	goto 	eewrite_loop	;; and loop again

;;+++++++++++++++++++++++++++++++++++++++++++++++

; *************** INIT part ****************
init

	BANK_LOW

; *************** init ports ***************
	clrf	PORTA	; set data A to 0
	clrf	PORTB

	BANK_HIGH

	clrf	TRISB	; set B  to output
	movlw	0x10
	movwf	TRISA	; set A4 to input

	BANK_LOW

; *************** SERIAL initialisation ***************

	movlw	SER_STATE_DETECT_START
	movwf	SER_STATE
	clrf	SER_SET_DATA_ADDR

; *************** PWD timer initialisation ***************

	call 	eeread_pwd

	movlw	PWD_STATE_WAIT_PERIOD_END
	movwf	PWD_STATE

	movlw	1
	movwf	EndOfPeriodCount
	movlw	PWD_REGISTERS
	movwf	PWD_REGISTER
	movlw	PWD_REGISTERS_MASK
	movwf	PWD_REGISTER_MASK

	movlw	PWD_STARTUP_COUNT
	movwf	PWD_STARTUP_COUNTER

	movlw	TIMERCOUNT 		;; initialise timer
	movwf	TMR0

	BANK_HIGH

; *************** 16F84 chip initialisation ***************
					; Set chip options
	movlw	B'11001000'		; PullUp disabled, rb0 rising edge irq, psa to wdt, rate = 0
	movwf	OPTION_REG

	BANK_LOW

	CLRWDT				; Clear watchdog! (every 18 msec !!)

	bcf	INTCON,T0IF		; enable irq again
	bsf	INTCON,GIE		; enable interrupts
	bsf	INTCON,T0IE 		; enable timer 0 irq

; *************** main function ***************

loop	nop
	nop
	nop
	nop
	nop
	nop
	nop
	nop
	nop
	nop
	nop
	nop
	nop
	nop
	nop
	nop
	nop
	nop
	nop
	nop
	nop
	nop
	goto	loop			; wait for interrupt

	END

Aquí va el enlace a la página con el código original.

Y a la página de Jaap Havinga con su artículo .

Anuncios
 

2 Responses to “Programando el 16F84A para controlar 12 servos”

  1. Juan Carlos Says:

    Hola, estoy intentando simular este código en Proteus 7.6 SP0 (Buil 8304). El byte del servo y la posición se la doy desde un Terminal Virtual, pero no me funciona. Le pongo pe.: 0xf0 0x78 ó 0xf00x78 ó 0xf0 [intro] 0x78. Me puedes echar un cable. Muchas gracias… 😉

    Me gusta

    • Sphinx Says:

      Hola Juan Carlos,
      Nunca he utilizado Proteus. ¿ Has mirado que la comunicación sea a 38400 baudios, con 8 bits de datos, 1 bit de stop, sin paridad y sin control de flujo ?
      ¿ Tienes forma en Proteus de hacer un debug del pin por el que entran esos datos ?
      A ver si alguna de estas pistas te hace avanzar.
      Un saludo.

      Me gusta


Responder

Introduce tus datos o haz clic en un icono para iniciar sesión:

Logo de WordPress.com

Estás comentando usando tu cuenta de WordPress.com. Cerrar sesión / Cambiar )

Imagen de Twitter

Estás comentando usando tu cuenta de Twitter. Cerrar sesión / Cambiar )

Foto de Facebook

Estás comentando usando tu cuenta de Facebook. Cerrar sesión / Cambiar )

Google+ photo

Estás comentando usando tu cuenta de Google+. Cerrar sesión / Cambiar )

Conectando a %s