; ControlServos.asm ; ; (c) Ivan Ricondo, 2008 ; Distribuido bajo licencia Creative Commons Attribution-Noncommercial-Share Alike 3.

0 Unported ; Resumen de la licencia se puede encontrar en http://creativecommons.org/licenses/bync-sa/3.0/deed.es ; License in English: http://creativecommons.org/licenses/by-nc-sa/3.0/ ; ; Version 1.0 alfa ; Primera version (todavia no funciona por que no esta acabada) ; ; ; Descripcion: ; ; La gestion de los servos se hace mediante interrupciones. Dado que queremos conseguir que ; cada 20ms se le envie una seal a cada servo, lo que se va a hacer es dividir esos 20ms entre ; 8 ciclos de 2,5ms. En cada ciclo de 2'5ms se gestionaran 4 servos. ; ; La forma de gestionar los servos se basara en gestionar el Timer1 del PIC (es un timer de 16bits). ; Para la gestion del grupo de 4 servos lo que se hara es ordenar los tiempos de cada pulso de menor ; a mayor. ; ; Por ejemplo si tuviesemos: Ser0=0,7ms Ser1=2,3ms Ser2=0,7ms Ser3=1,9ms ; Trataramos primero Ser0, luego Ser2, luego Ser3 y por ltimo Ser1 ; ; Lo primero que se hara es poner a 1 cada uno de servos uno detras de otro. Para ello generaremos ; 4 interrupciones, separadas por 0,05ms (50us) en la que iremos poniendo cada servo a 1 en orden. ; Una vez que acabemos con estas 4 primeras interrupciones, comenzaran las interrupciones de poner ; a 0 cada uno de los servos en el mismo orden. ; ; En nuestro ejemplo lo que se haria es: ; El primero (Ser0) se pondra a cero despus de Ser0-0,150ms=0,7-0,15=0,45ms (tiempo entre 3 y 4) ; El segundo (Ser2) despues de Ser2-Ser0+0,05=0,7-0,7+0,05=0,05 (tiempo entre 4 y 5) ; El tercero (Ser3) despues de ser3-Ser2+0,05=1,9-0,7+0,05=0,85 (tiempo entre 5 y 6) ; El cuarto (Ser1) despues de ser1-Ser3+0,05=2,3-1,9+0,05=0,45 (tiempo entre 6 y 7) ; Aqui se habrian puesto todos a 0 otra vez. Esperariamos 2,5-0,05*3-Ser1=2,35-2,3=0,05 (tiempo entre ; 7 y 0) y empezariamos otro ciclo con otros cuatro servos. ; ; De forma grafica seraa segun el siguiente grafico: ; ; ; Int 0 1 2 3 4 5 6 7 0 ; | | | | | | | | | ; v v v v v v v v v ; ; +---------------------------------------+ +-; Ser0 _| |_________________________| ; +---------------------------------------+ ; Ser2 _____| |________________________ ; +---------------------------------------------+ ; Ser3 _________| |______________ ; +------------------------------------------------+ ; Ser1 _____________| |_______ ; ; ; Para que todo funcione hay que establecer algunos lmites. El tiempo de cada servo puede estar a uno ; ser como mximo 2,3ms y el mnimo 0,3ms (en realidad se podra bajar hasta 0,2 pero creo que no hay ; ningn servo que trabaje en este rango). Esto nos va a dar que el tiempo mnimo entre dos interrupciones ; va a ser 0,05ms (50us). En este tiempo nos tiene que dar tiempo para ejecutar la rutinas de interrupcin ; completa. A 4Mhz (que no creo que de tiempo) podramos ejecutar 50 instrucciones, a 20Mhz 250 ; instrucciones. Cuando acabe har la suma de tiempos para comprobar el procesador mnimo.

; ; Para que el tiempo sea muy exacto lo que se ha hecho es que en cada ciclo lo primero que se hace es poner ; el bit al valor que sea y despus calcular el valor del siguiente bit a cambiar (y los tiempos y todas ; esas cosas). ; ; Por la forma que se ha hecho el control de tiempo va a ser muy exacto (est hecho en ASM para tener ; control exacto hasta el nivel de instruccin). ; ; El "problema" que hay es que el PIC en el que yo estoy pensando (Pic16F877A), no tiene 32 salidas ; disponibles para poner 32 servos :-D Pero tampoco tengo proyecto para poner ms de 20 servos :-) ; Se podra hacer con chips externos o con otro pic ms grande utilizando la misma lgica. ; ; Con esta misma idea se podra aumentar el tiempo de cada servo a ms de 2,3 (por ejemplo 2,5) bajando ; el nmero de servos a controlar (por ejemplo si controlamos 7*4=28 servos, tendramos 20/7=2,85ms para ; cada grupo de servos). De esta misma forma se podra tambin bajar la frecuencia de reloj (por ejemplo, ; si controlasemos 5*4 servos, podramos hacer cada ciclo de 4 servos en 20/5=4ms, de esta forma ; podramos tener (4-2,3)/4=425us para ejecutar las instrucciones de la rutina de interrupcin. ; ; No debemos aadir nuevos tipos de interrupciones en este programa (afectara a la exactitud del tiempo). ; Lo que si se podra hacer es aadir pequeas gestiones en la rutina de interrupciones. Inicialmente ; pens en meter la gestin del puerto serie ah, pero no se puede. A 38400 bps podramos recibir un ; byte cada 1/3840=260uS, y las interrupciones de 3-4 (la ms larga) podra ser hasta de ; 2,3-0,15=2150uS (por lo que se podran recibir ms de 8 caracteres, por lo que nos perderamos). ifdef __16F88 include "p16f88.inc" endif ifdef __16F873A include "p16f873a.inc" endif ifdef __16F874A include "p16f874a.inc" ; else ; error "Debes seleccionar chip 16F88, 16F873A, 16F874A o 16F877A" endif ifdef __16F877A include "p16f877a.inc" endif

__config _CONFIG1, _CP_OFF & _CCP1_RB0 & _DEBUG_OFF & _WRT_PROTECT_OFF & _CPD_OFF & _LVP_OFF & _BODEN_OFF & _MCLR_OFF & _PWRTE_OFF & _WDT_OFF & _HS_OSC __config _CONFIG2, _IESO_OFF & _FCMEN_OFF else __config _XT_OSC & _WDT_OFF & _PWRTE_ON & _LVP_OFF & _CP_OFF ; (3FF1h [CPoff] o 0001h [CPon]) endif radix dec

ifdef __16F88

; Algunas Macros para hacernos la vida ms sencilla #define skipz btfss STATUS,Z #define skipnz btfsc STATUS,Z #define skipc btfss STATUS,C #define skipnc btfsc STATUS,C

jz macro label btfsc STATUS,Z goto label endm jnz macro label btfss STATUS,Z goto label endm jc macro label btfsc STATUS,C goto label endm jnc macro label btfss STATUS,C goto label endm org NextValueServo res NextStep PosServo Cuartet0_Value Cuartet1_Value Cuartet2_Value Cuartet3_Value Cuartet0_LongH Cuartet0_LongL Cuartet1_LongH Cuartet1_LongL Cuartet2_LongH Cuartet2_LongL Cuartet3_LongH Cuartet3_LongL ADDRH ADDRL DATAH DATAL TEMP res res res res res res res res res res res res res res res res res 1 1 1 1 1 res res res res res res res res res res res res 1 1 1 1 1 1 1 1 1 1 1 1 org res res res org libres res Servos res 16 32*2 ; variable temporal para las interrupciones ; numero de servo ; las siguientes variables son por la rutina de 1 res res 1 1 1 1 1 1 1 1 1 1 1 1 20h 1 1

NumServo CocienteH dividir CocienteL ResultadoH ResultadoL SumadorH SumadorL DivisorH DivisorL TempInt TempInt2 TempInt3

W_TEMP STATUS_TEMP PCLATH_TEMP

1 1 1

070h ; variables especiales ; variables para el gestor de interrupciones

0a0h ; aqui quedan 16 bytes para uso libre ; valor de los servos

StepsSer IncrementSer StepsSerNew NextValueSer

res

org res 32*2 org res 32*2

110h 32 ; aqui no queda nada libre 190h 32 ; aqui no queda nada libre

res

TIME50US equ velocidad de la CPU 4Mhz...) TIMEMIN TIMEMAX TIMESEP TIMECYCLE equ equ equ

250

; numero de ins para 50us (dependera de la ; sera 250 para 20Mhz, 50 para

TIME50US*6 TIME50US*46 TIME50US equ (TIME50US*50)

; 0,3ms ; 2,3ms

; Esta macro sirve para comparar dos valores de 16 bits (Long0 y Long1) y en ; caso de que Long1 sea menor que Long0 intercambiar los valores de Long0 y Long1 ; y de Value0 y Value1 ; Con esto ordenaremos una lista de 4 valores ejecutandolo 6 veces (0 con 1, 1 con 2, ; 2 con 3, tendriamos el mayor en 3, luego otra vez 0 con 1 y 1 con 2, tendriamos ; los dos mayores en 2 y 3, y por ultimo 0 con 1 otra vez con lo que la lista estaria ; ordenada, creo que este metodo de ordenacion se llama el de la burbuja) CompareAndSwap macro valor,LongH0,LongL0,LongH1,LongL1,Value0,Value1 movf LongH1,w ; maximo 28 ciclos subwf LongH0,w jnc finser#V(valor) ; si el 1 es mayor que el 0 saltar jnz swapser#V(valor) ; si es menor saltar a cambiar movf LongL0,w ; si parte alta es igual, otra comparacion subwf LongL1,w jc finser#V(valor) ; si el 1 es mayor o igual que el 0 saltar swapser#V(valor): movf LongH0,w ; intercambiar LongH0 por LongH1 movwf TempInt movf LongH1,w movwf LongH0 movf TempInt,w movwf LongH1 movf movwf movf movwf movf movwf movf movwf movf movwf movf movwf LongL0,w TempInt LongL1,w LongL0 TempInt,w LongL1 Value0,w TempInt Value1,w Value0 TempInt,w Value1 ; intercambiar LongL0 por LongL1

; intercambiar Value0 por Value1

finser#V(valor): endm

; Hacer una suma de 16 bits, dejando el resultado en Res (Res+=Val) add16 macro ValH,ValL,ResH,ResL movf ValL,w addwf ResL,f movf ValH,w skipnc addlw 1 addwf ResH,f

endm ; sumar a res el complemento a1 de Val (puede valer para hacer resta ; incrementando luego el ResH,ResL) add16a1 macro ValH,ValL,ResH,ResL comf ValL,w addwf ResL,f comf ValH,w skipnc addlw 1 addwf ResH,f endm add16val macro movlw addwf movlw skipnc addlw addwf VAL16,ResH,ResL (VAL16) & 255 ResL,f (VAL16)/256 1 ResH,f

endm ; Incrementar el valor de un campo de 16bits inc16 macro ResH,ResL incf ResL,f skipnz incf ResH,f endm ;****************************************************************************** ; Vector de reset ;****************************************************************************** org 0h clrf PCLATH call FirmwareUpdate goto main ;****************************************************************************** ; Rutina interrupcin ;****************************************************************************** ; lleva la gestion de 32 servos... es demasiado compleja y un poco desordenada por ; no malgastar ciclos de reloj org 4h MOVWF W_TEMP ;Copy W to TEMP register SWAPF STATUS,W ;Swap status to be saved into W CLRF STATUS ;bank 0, regardless of current bank, Clears IRP,RP1,RP0 MOVWF STATUS_TEMP ;Save status to bank zero STATUS_TEMP register MOVF PCLATH, W ;Only required if using pages 1, 2 and/or 3 MOVWF PCLATH_TEMP ;Save PCLATH into W CLRF PCLATH ;Page zero, regardless of current page call bcf incf btfss goto clrf PutValueServo ; put value ; interrupcion timer1 tratada

PIR1,TMR1IF

NextStep,f ; NextStep++ NextStep,3 ; if step 8, dont jump NoNextCuartet ; if step<>8 jump NextStep ; poner que siguiente pasp NextStep=0 ; actualizar timer ; siguiente int en Timer1+=-(2500us-50us*3-

Cuartet3_Long) add16

; =-2350us+Cuartet3_Long Cuartet3_LongH,Cuartet3_LongL,TMR1H,TMR1L

add16val (65536-(TIMECYCLE-3*TIMESEP)),TMR1H,TMR1L ; actualizar posicion de los servos movlw movwf buclenewpos: movf 4 TempInt PosServo,w ; 4 posiciones

addlw movwf bsf movf jz decfsz goto ; ; ; ; ;

(StepsSer)&255 FSR STATUS,IRP INDF,w nonewpos INDF,f calculenew

; posicion de steps ; banco 2-3 para indirecto ; si era 0 no cambiar nada ; si era 1 copiar ultimo valor ; si <>1 saltar a sumar incremento

movwf PosServo,w andlw 31 addlw -24 skipnc clrf PosServo PosServo,w (31*2) NextValueSer&255 FSR INDF,w TempInt2 FSR,f INDF,w TempInt3 STATUS,IRP ; calcular w=posservo*2+NextValueSer ; leer valor incremento ; copiar nuevo valor en TempInt2 y 3 ; (con el fin de hacer menos ins cambiando ; FSR y bits correspondientes) ; banco 0-1 para indirecto

rlf andlw addlw movwf movf movwf incf movf movwf bcf movlw addwf movf movwf decf movf movwf goto calculenew: incremento que sea rlf andlw addlw movwf movf movwf incf movf movwf bcf movlw addwf movf Servo addwf movf skipnc addlw decf addwf bcf decfsz goto movlw addwf rlf andlw addlw movwf movf incf movwf

Servos-(NextValueSer & 255)+1 FSR,f ; nueva posicin para acceder a servos TempInt3,w INDF FSR,f TempInt2,w INDF nonewpos ; si era otra cosa sumarle el PosServo,w (31*2) IncrementSer&255 FSR INDF,w TempInt2 FSR,f INDF,w TempInt3 STATUS,IRP ; calcular w=posservo*2 ; leer valor incremento ; mover valor de nuevo servo ; primero parte baja ; ahora parte alta

; banco 0-1 para indirecto

Servos-(IncrementSer & 255) FSR,f ; nueva posicin para acceder a servos TempInt3,w INDF,f TempInt2,w 1 FSR,f INDF,f STATUS,IRP TempInt,f buclenewpos -4 PosServo,f ; banco 0-1 para indirecto ; poner PosServo otra vez con su valor ; sumar a valor de tabla IncrementSer a ; que corresponda

nonewpos:

(31*2) Servos FSR ; calculamos dir como (PosServo and 31)*2+Servos INDF,w ; ir copiando los 8 valores FSR,f Cuartet0_LongH

; copiar valores PosServo,w

movf incf movwf movf incf movwf movf incf movwf movf incf movwf movf incf movwf movf incf movwf movf movwf mismos movf andlw movwf addlw movwf addlw movwf addlw movwf addlw movwf con una macro

INDF,w FSR,f Cuartet0_LongL INDF,w FSR,f Cuartet1_LongH INDF,w FSR,f Cuartet1_LongL INDF,w FSR,f Cuartet2_LongH INDF,w FSR,f Cuartet2_LongL INDF,w FSR,f Cuartet3_LongH INDF,w Cuartet3_LongL PosServo,w 31 Cuartet0_Value 1 Cuartet1_Value 1 Cuartet2_Value 1 Cuartet3_Value 1 PosServo ; ordenar los valores de menor a mayor, lo hacemos ; ahora crear tabla con el orden de los

CompareAndSwap 1,Cuartet0_LongH,Cuartet0_LongL,Cuartet1_LongH,Cuartet1_LongL,Cuartet0_Value,Cuartet1_Va lue CompareAndSwap 2,Cuartet1_LongH,Cuartet1_LongL,Cuartet2_LongH,Cuartet2_LongL,Cuartet1_Value,Cuartet2_Va lue CompareAndSwap 3,Cuartet2_LongH,Cuartet2_LongL,Cuartet3_LongH,Cuartet3_LongL,Cuartet2_Value,Cuartet3_Va lue CompareAndSwap 4,Cuartet0_LongH,Cuartet0_LongL,Cuartet1_LongH,Cuartet1_LongL,Cuartet0_Value,Cuartet1_Va lue CompareAndSwap 5,Cuartet1_LongH,Cuartet1_LongL,Cuartet2_LongH,Cuartet2_LongL,Cuartet1_Value,Cuartet2_Va lue CompareAndSwap 6,Cuartet0_LongH,Cuartet0_LongL,Cuartet1_LongH,Cuartet1_LongL,Cuartet0_Value,Cuartet1_Va lue rlf iorlw movwf goto NoNextCuartet: movf andlw addlw movwf rlf andlw btfss iorlw movwf NextStep,w 3 Cuartet0_Value FSR INDF,w 62 NextStep,2 1 NextValueServo ; calculate next value Cuartet0_Value,w ; next value is this 1 NextValueServo finint

; w=(Cuartet0_Value[NextStep&3] and 31)*2 ; if step 0-3... ; w=w or 1 (put servo to 1)

movlw addwf jc

-3-1 NextStep,w nomenor3 ; si el paso es 1-2-3

siguiente en 50us, Timer1+=-50us add16val (65536-TIMESEP),TMR1H,TMR1L goto nomenor3: finint

noescuatro ; si el paso es 4 siguiente en Cuartet0_Long50us*3; hacer Timer1-=Cuartet0_Long-50us*3 ; es decir sumar 50us*3 y -Cuartet0_Long, o (Cuartet0_Long xor 65535 +1) add16val (TIMESEP*3+1),TMR1H,TMR1L ; TMR1+=150us+1 add16a1 Cuartet0_LongH,Cuartet0_LongL,TMR1H,TMR1L ; TMR1+=(Cuartet0_Long xor 0ffffh) goto noescuatro: addlw jnz Cuartet0_Long+50us -1 noescinco ; si es el 5 siguiente Cuartet1_Longfinint

jnz

; sumar a timer1 -(50us)+Cuartet0_Long+ (Cuartet1_Long xor 65535 +1) add16 Cuartet0_LongH,Cuartet0_LongL,TMR1H,TMR1L add16a1 Cuartet1_LongH,Cuartet1_LongL,TMR1H,TMR1L add16val (65536-TIMESEP+1),TMR1H,TMR1L goto noescinco: addlw jnz Cuartet1_Long+50us -1 noesseis ; si es el 6 siguiente Cuartet2_Longfinint

; sumar a timer1 -(50us)+Cuartet1_Long+ (Cuartet2_Long xor 65535 +1) add16 Cuartet1_LongH,Cuartet1_LongL,TMR1H,TMR1L add16a1 Cuartet2_LongH,Cuartet2_LongL,TMR1H,TMR1L add16val (65536-TIMESEP+1),TMR1H,TMR1L goto noesseis: Cuartet2_Long+50us ; si es el 7 siguiente Cuartet3_Long; sumar a timer1 -(50us)+Cuartet2_Long+ (Cuartet3_Long xor 65535 +1) add16 Cuartet2_LongH,Cuartet2_LongL,TMR1H,TMR1L add16a1 Cuartet3_LongH,Cuartet3_LongL,TMR1H,TMR1L add16val (65536-TIMESEP+1),TMR1H,TMR1L finint: MOVF MOVWF SWAPF MOVWF SWAPF SWAPF retfie PCLATH_TEMP, W ;Restore PCLATH PCLATH ;Move W into PCLATH STATUS_TEMP,W ;Swap STATUS_TEMP register into W ;(sets bank to original state) STATUS ;Move W into STATUS register W_TEMP,F ;Swap W_TEMP W_TEMP,W ;Swap W_TEMP into W finint

;****************************************************************************** ; Funciones del inicializacion ;****************************************************************************** ; InicializarPIC ; Esta rutina inicializa todo el chipito InicializarPIC: clrf PORTA ; inicializar a 0 todos los reg de entrada clrf PORTB ifdef __16F873A clrf PORTC endif ifdef __16F874A clrf PORTC clrf PORTD clrf PORTE endif ifdef __16F877A clrf PORTC clrf PORTD clrf PORTE endif clrf STATUS ; borrar status para tenerlo en estado conocido bsf configuracin de puertos) movlw movwf movlw movwf movlw movwf movwf movwf movwf movlw movwf endif ifdef __16F877A movwf movwf movlw movwf bcf TRISC & 7Fh TRISD & 7Fh 00000111b TRISE & 7Fh ; poner en entrada los REx STATUS,RP0 6 ADCON1 & 7fh 000000000b TRISA & 7Fh 000000000b TRISB & 7Fh TRISC & 7Fh TRISC & 7Fh TRISD & 7Fh 00000111b TRISE & 7Fh ; poner en entrada los REx ; poner pgina 1 (pgina de

; poner tomas en digital ; Todo en salida ; todo en salida

ifdef __16F873A endif ifdef __16F874A

endif

STATUS,RP0

; poner pgina 0

; inicializar interrupciones bsf movlw movwf bcf return InitServo: central 1500ms STATUS,RP0 1 OPTION_REG & 7fh STATUS,RP0 ; T0CS=0, PSA=0,

PS<2:0>=0(1:2)

movlw movwf movlw movwf

32 TEMP Servos FSR

; inicializar los servos a posicion

bucinise: movlw movwf incf movlw movwf incf (TIME50US*30)/256 INDF FSR,f (TIME50US*30)&255 INDF FSR,f ; bucle inicializacion servos ; inicializar servos a 1500ms

decfsz TEMP,f goto bucinise bsf movlw movwf movlw movwf buinise2: clrf incf decfsz goto movlw movwf movlw movwf buinise3: clrf incf decfsz goto bcf movlw movwf clrf clrf clrf movlw movwf STATUS,IRP 32 TEMP (StepsSerNew&255) FSR INDF FSR,f TEMP,f buinise2 32 TEMP StepsSer & 255 FSR INDF FSR,f TEMP,f buinise3 STATUS,IRP 7

; inicializar StepsSerNew

; borrar variable

; inicializar StepSers

; borrar variable

primera int

; inicializar NextStep para que en

NextStep ; calcule todo lo que tiene que calcular NextValueServo ; y que primera vez ponga a 0 el servo 0 PosServo ; empezamos por primer servo TMR1L 255 TMR1H ; inicializar timer1 a FF00h

; configurar timer1 movlw 1 ; T1OSCEN=0, T1CKPS1:T1CKPS0 preescaler a 1:1, TMR1ON=1, TMR1CS=0 (Internal Clock) movwf T1CON bsf bsf bcf bcf bsf bsf return STATUS,RP0 ; poner pgina 1 PIE1 & 7Fh,TMR1IE ; habilitar interrupciones de timer1 STATUS,RP0 ; poner pgina 0 PIR1,TMR1IF ; inicializar a int no ha ocurrido INTCON,PEIE ; habilitar interrupciones por INTCON,GIE ; habilitar interrupciones

dispositivos

InitSerial: movlw movwf bsf movlw movwf movlw movwf ; ; bsf bsf 090h RCSTA

; inicializar puerto serie ; SPEN=1(7) RX9=0(6) CREN=1(4) ADDEN=0(3) STATUS,RP0 ; poner pgina 1

24h ; TX9=0(6) TXEN=1(5) SYNC=0(4) BRGH=1(2) TXSTA & 7fh 64 SPBRG & 7fh PIE1,RCIE PIE1,TXIE ; 19200 a 20Mhz (seria 12 a 4Mhz) y BRGH=1 ; habilitar interrupcion de recepcin ; habilitar interrupcin de envio

bcf return

STATUS,RP0

;****************************************************************************** ; Varias rutinas encargadas de hacer divisiones de 16 bits ;****************************************************************************** ; Esta rutina va a dividir un numero de 15 bits (sin signo) entre uno de 8 bits ; Divide Cociente/DivisorL y deja resultado en Resultado, el cociente queda el resto dividesinsignoconredon: bcf STATUS,C rrf DivisorL,w ; sumar el divisor/2 addwf CocienteL,f skipnc incf CocienteH,f dividesinsigno: movf DivisorL,w movwf DivisorH iorlw 0 jz divideby0 addlw -1 jz divideby1 addlw -1 rotacion jz divideby2 clrf DivisorL bcf STATUS,C rrf DivisorH,f rrf DivisorL,f movlw 128 movwf SumadorL clrf SumadorH clrf ResultadoL clrf ResultadoH bucdivisor: btfsc DivisorH,13-8 goto FinDivisor bcf STATUS,C rlf SumadorL,f rlf SumadorH,f bcf STATUS,C con lo que deberia ser necesaria esta ins rlf DivisorL,f rlf DivisorH,f goto bucdivisor FinDivisor: Divisor movf subwf jnc jnz movf subwf jnc saltar divisormayor: comf DivisorL,w addwf CocienteL,f decir el complementoa2) skipnc incf CocienteH,f comf DivisorH,w addwf CocienteH,f incf CocienteL,f skipnz incf CocienteH,f movf SumadorL,w addwf ResultadoL,f skipnc ; si divisor es mayor ; restar de cociente el divisor ; para ello sumar el complementoa1 +1 (es DivisorH,w CocienteH,w divimenor divisormayor DivisorL,w CocienteL,w divimenor ; leemos valor por el que dividir ; y lo guardamos en parte alta ; si es 0 no se puede dividir ; si es 1 devolver lo mismo ; si es 2 hacer division por

; al hacer rotaciones metemos 0 ; lo dejamos en Divisor*128 ; inicialmente 7 pasos de resta ; sumador con valor 2^7 ; inicialmente resultado es 0 ; comprobar si divisor es numero pequeo ; no, saltar ; si, aadir pasos a la division ; sumador=sumador*2 ; el anterior rlf como no puede desbordar, ; divisor=divisor*2

; comparar Cociente con

; si el 1 es mayor que el 0 ; si es menor saltar a cambiar ; si parte alta es igual, otra ; si el 1 es mayor que el 0

saltar comparacion

; sumar el +1

; sumar a resultado el sumador

incf movf addwf divimenor:

ResultadoH,f SumadorH,w ResultadoH,f ; dividimos divisor entre 2 ; dividimos sumador entre 2 ; si se queda a 0 hemos acabado,

bcf STATUS,C rrf DivisorH,f rrf DivisorL,f bcf STATUS,C rrf SumadorH,f rrf SumadorL,f jnc FinDivisor sino saltar arriba (al bucle) return divideby0: resultado 0 clrf ResultadoH clrf ResultadoL return movf movwf movf movwf return CocienteH,w ResultadoH CocienteL,w ResultadoL

; si dividir por 0 deja como

divideby1:

; si dividir por 1 dejar lo mismo ; dar lo mismo

divideby2: rotacion

; si dividir por 2, hacerlo por

bcf STATUS,C rrf CocienteH,w movwf ResultadoH rrf CocienteL,w movwf ResultadoL return

; igual que la anterior pero considerando el signo en el Cociente divideconsigno: btfss CocienteH,7 ; si el signo es negativo seguir goto dividesinsigno ; si es positivo dividirlo sin mas comf comf incf skipnz incf call comf comf incf skipnz incf return CocienteH,f CocienteL,f CocienteL,f ; cambiarlo de signo el cociente

CocienteH,f dividesinsigno ; dividimos sin signo ResultadoH,f ; cambiar de signo el resultado ResultadoL,f ResultadoL,f ResultadoH,f

divideconsignoredondeo: btfss CocienteH,7 ; si el signo es negativo seguir goto dividesinsignoconredon; si es positivo dividirlo sin mas comf comf incf skipnz incf call comf comf incf skipnz incf return CocienteH,f CocienteL,f CocienteL,f ; cambiarlo de signo el cociente

CocienteH,f dividesinsignoconredon; dividimos sin signo ResultadoH,f ; cambiar de signo el resultado ResultadoL,f ResultadoL,f ResultadoH,f

;****************************************************************************** ; Varias rutinas encargadas de gestionar valores servos ;****************************************************************************** ; Poner el valor de un servo (NumServo) en el array NextValueSer. El valor lo toma de ; la variable Resultado

PonerValorServo: rlf andlw addlw movwf bsf bcf return

NumServo,w (31*2) NextValueSer & 0ffh FSR STATUS,IRP STATUS,IRP

; banco 2-3 para indirecto ; banco 0-1 para indirecto

; Hacer un commit de todos los cambios que se hayan cambiado. Para ello mira aquellos servos ; que se CommitChangesServos: return ;****************************************************************************** ; Rutinas tratamiento puerto serie ;****************************************************************************** ResetearSerie: bcf RCSTA,CREN ; deshabilitar puerto serie nop bsf RCSTA,CREN ; y volverlo a conectar return TrataSerie: btfsc call btfsc call btfss serie? RCSTA,FERR ResetearSerie RCSTA,OERR ResetearSerie PIR1,RCIF ; comprobar si se ha producido error ; si hay error resetear el puerto

; se ha recibido un caracter por puerto ; no, volver ; leer caracter recibido ; guardar en variable temporal ; recibido un 0

return movf RCREG,w movwf sublw jnz bsf movlw movwf movlw movwf bcf return movf sublw jnz bsf movlw movwf movlw movwf bcf return TEMP '0'

TrataCarSerie:

sigcar1 STATUS,RP0 (800*5)/256 Servos & 7Fh (800*5)&255 (Servos+1) & 7Fh STATUS,RP0 TEMP,w '1' sigcar2 STATUS,RP0 (1500*5)/256 Servos & 7Fh (1500*5)&255 (Servos+1) & 7Fh STATUS,RP0 TEMP,w '2'

sigcar1:

sigcar2:

movf sublw jnz bsf movlw movwf movlw movwf bcf return return

sigcar3 STATUS,RP0 (2200*5)/256 Servos & 7Fh (2200*5)&255 (Servos+1) & 7Fh STATUS,RP0

sigcar3:

;****************************************************************************** ; Rutina principal

;****************************************************************************** main: call InicializarPIC ; inicializar algunas cosas call InitServo call InitSerial movlw call bsf movlw movwf clrf movlw movwf bcf mainbucle: call goto '0' TrataCarSerie STATUS,RP1 50 StepsSer & 127 IncrementSer & 7Fh 32*5 (IncrementSer+1) & 7Fh STATUS,RP1 TrataSerie mainbucle

;****************************************************************************** ; Rutina gestion de puertos de los servos ;****************************************************************************** ; Esta rutina pone a 0 o a 1 el puerto correspondiente a un servo. Para ello leera ; de la variable NextValueServo lo que hay que cambiar. El valor que recibe es el ; nmero de servo multiplicado por 2, y en el bit bajo 0 el valor a poner en el ; puerto . Por ejemplo 20*2+1 pondra a 1 el valor del puerto 20. org 2048-256-6-128 PutValueServo: movlw 2048/256-2 ; this routine (including the call, uses 11 clock cycles) movwf PCLATH rlf NextValueServo,w ; w=NextValueServo*2 andlw 62 ; w=w and (31*2) addwf PCL,f ; PCL=PCL+W bcf PORTB,7 ; set 0 port 0 return bsf PORTB,7 ; set 1 port 0 return bcf PORTB,6 ; set 0 port 1 return bsf PORTB,6 ; set 1 port 1 return bcf PORTB,5 ; set 0 port 2 return bsf PORTB,5 ; set 1 port 2 return bcf PORTB,4 ; set 0 port 3 return bsf PORTB,4 ; set 1 port 3 return bcf PORTB,3 ; set 0 port 4 return bsf PORTB,3 ; set 1 port 4 return bcf PORTB,2 ; set 0 port 5 return bsf PORTB,2 ; set 1 port 5 return bcf PORTB,1 ; set 0 port 6 return bsf PORTB,1 ; set 1 port 6 return bcf PORTB,0 ; set 0 port 7 return bsf PORTB,0 ; set 1 port 7 return nop ; set 0 port ? return nop ; set 1 port ? return nop ; set 0 port ? return nop ; set 1 port ? return

nop return nop return nop return nop return nop return nop return nop return nop return nop return nop return nop return nop return nop return nop return nop return nop return nop return nop return nop return nop return nop return nop return nop return nop return nop return nop return nop return nop return nop return nop return nop return nop return nop return nop return nop return nop return nop return nop return nop

; set 0 port ? ; set 1 port ? ; set 0 port ? ; set 1 port ? ; set 0 port ? ; set 1 port ? ; set 0 port ? ; set 1 port ? ; set 0 port ? ; set 1 port ? ; set 0 port ? ; set 1 port ? ; set 0 port ? ; set 1 port ? ; set 0 port ? ; set 1 port ? ; set 0 port ? ; set 1 port ? ; set 0 port ? ; set 1 port ? ; set 0 port ? ; set 1 port ? ; set 0 port ? ; set 1 port ? ; set 0 port ? ; set 1 port ? ; set 0 port ? ; set 1 port ? ; set 0 port ? ; set 1 port ? ; set 0 port ? ; set 1 port ? ; set 0 port ? ; set 1 port ? ; set 0 port ? ; set 1 port ? ; set 0 port ? ; set 1 port ? ; set 0 port ?

return nop return nop return nop return nop return nop return

; set 1 port ? ; set 0 port ? ; set 1 port ? ; set 0 port ? ; set 1 port ?

;****************************************************************************** ; Rutina actualizacion del firmware ;****************************************************************************** ; algun dia la hare, la idea seria que si por el bucle se escriba en el segundo ; banco de memoria de instrucciones, y esta rutina comprobase si habia version ; nueva y si era ok. En caso de que fuese asi que escribiese los datos del ; banco 2 en el banco 1 (de direccin 1 a la 1791) org 1792 ; 2048-256 FirmwareUpdate: bcf STATUS,RP1 bcf STATUS,RP0 BCF INTCON, GIE ; deshabilitar interrupciones movlw movwf movlw movwf call (1791+2048)/256 ; poner direccion a leer ADDRH 1791 & 255 ADDRL ReadProgEEPROM ; leer valor direccin ; comprobar si leido es 3FFF

quizasact1: diferentes

incfsz DATAL,w goto quizasact1 movlw 3fh subwf DATAH,w skipnz return

; si es 3ffff no actualizar, salimos ; comprobar si versiones son ; comprobar un checksum para

que sepamos que es correcto clrf movlw movwf bucleactf: movlw addwf call movlw addwf call incfsz incf movlw subwf jnz salir ReadProgEEPROM return

ADDRH 2 ADDRL

; comenzar desde direccion 2

2048/256 ; leemos en la direccion+2048 ADDRH,f ReadProgEEPROM -(2048/256) ADDRH,f WriteProgEEPROM ADDRL,f ADDRH,f 1742/256 ADDRH,w bucleactf ; por ahora no hacemos nada, solo

BCF STATUS, RP1 ; BCF STATUS, RP0 ;Bank 0 MOVF ADDRL, W ;Write the BSF STATUS, RP1 ;Bank 2 MOVWF EEADR & 7Fh ;address bytes BCF STATUS, RP1 ;Bank0 MOVF ADDRH,W ;for the desired BSF STATUS, RP1 ;Bank 2 MOVWF EEADRH ;address to read BSF STATUS, RP0 ;Bank 3 BSF EECON1, EEPGD ;Point to Program memory BSF EECON1, RD ;Start read operation NOP ;Required two NOPs NOP ;

BCF STATUS, RP0 ;Bank 2 MOVF EEDATA, W ;DATAL = EEDATA BCF STATUS, RP1 ;Bank0 MOVWF DATAL ; BSF STATUS, RP1 ;Bank 2 MOVF EEDATH,W ;DATAH = EEDATH BCF STATUS, RP1 ;Bank0 MOVWF DATAH ; return WriteProgEEPROM BSF STATUS, RP1 ; BCF STATUS, RP0 ;Bank 2 MOVF ADDRL, W ;Write address MOVWF EEADR ;of desired MOVF ADDRH, W ;program memory MOVWF EEADRH ;location MOVF DATAL, W ;Write value to MOVWF EEDATA ;program at MOVF DATAH, W ;desired memory MOVWF EEDATH ;location BSF STATUS, RP0 ;Bank 3 BSF EECON1, EEPGD ;Point to Program memory BSF EECON1, WREN ;Enable writes btfsc INTCON,GIE ; como estan las interrupciones? goto WriteNoInts ;Only disable interrupts BCF INTCON, GIE ;if already enabled, ;otherwise discard MOVLW 0x55 ;Write 55h to MOVWF EECON2 ;EECON2 MOVLW 0xAA ;Write AAh to MOVWF EECON2 ;EECON2 BSF EECON1, WR ;Start write operation NOP ;Two NOPs to allow micro NOP ;to setup for write ;Only enable interrupts BSF INTCON, GIE ;if using interrupts, ;otherwise discard BCF EECON1, WREN ;Disable writes return WriteNoInts: MOVLW 0x55 ;Write 55h to MOVWF EECON2 ;EECON2 MOVLW 0xAA ;Write AAh to MOVWF EECON2 ;EECON2 BSF EECON1, WR ;Start write operation NOP ;Two NOPs to allow micro NOP ;to setup for write BCF EECON1, WREN ;Disable writes return end