Utilizando 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 .
–
Linux Droids Blog 
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 gustaMe gusta
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 gustaMe gusta