; ControlServos€¦ · Web view; 4 interrupciones, separadas por 0,05ms (50us) en la que iremos...

; 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/by-nc-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 señal 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; TratarÃamos 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 despuÃ©s 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 lÃmites. El tiempo de cada servo puede estar a uno; serÃ¡ como mÃ¡ximo 2,3ms y el mÃnimo 0,3ms (en realidad se podrÃa bajar hasta 0,2 pero creo que no hay; ningÃºn servo que trabaje en este rango). Esto nos va a dar que el tiempo mÃnimo entre dos interrupciones; va a ser 0,05ms (50us). En este tiempo nos tiene que dar tiempo para ejecutar la rutinas de interrupciÃ³n; completa. A 4Mhz (que no creo que de tiempo) podrÃamos ejecutar 50 instrucciones, a 20Mhz 250 ; instrucciones. Cuando acabe harÃ© la suma de tiempos para comprobar el procesador mÃ-nimo.

;; 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 despuÃ©s 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 instrucciÃ³n).;; 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 mÃ¡s de 20 servos :-); Se podrÃa hacer con chips externos o con otro pic mÃ¡s grande utilizando la misma lÃ³gica.;; Con esta misma idea se podrÃa aumentar el tiempo de cada servo a mÃ¡s de 2,3 (por ejemplo 2,5) bajando; el nÃºmero de servos a controlar (por ejemplo si controlamos 7*4=28 servos, tendrÃamos 20/7=2,85ms para; cada grupo de servos). De esta misma forma se podrÃa tambiÃ©n bajar la frecuencia de reloj (por ejemplo,; si controlasemos 5*4 servos, podrÃamos hacer cada ciclo de 4 servos en 20/5=4ms, de esta forma; podrÃamos tener (4-2,3)/4=425us para ejecutar las instrucciones de la rutina de interrupciÃ³n.;; No debemos aÃ±adir nuevos tipos de interrupciones en este programa (afectarÃa a la exactitud del tiempo).; Lo que si se podrÃa hacer es aÃ±adir pequeÃ±as gestiones en la rutina de interrupciones. Inicialmente; pensÃ© en meter la gestiÃ³n del puerto serie ahÃ, pero no se puede. A 38400 bps podrÃ-amos recibir un; byte cada 1/3840=260uS, y las interrupciones de 3-4 (la mÃ¡s larga) podrÃa ser hasta de; 2,3-0,15=2150uS (por lo que se podrÃan recibir mÃ¡s de 8 caracteres, por lo que nos perderÃamos).

ifdef __16F88include "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

ifdef __16F88__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

; Algunas Macros para hacernos la vida más sencilla#define skipz btfss STATUS,Z#define skipnz btfsc STATUS,Z

#define skipc btfss STATUS,C#define skipnc btfsc STATUS,C

jz macro labelbtfsc STATUS,Z goto label

endm

jnz macro labelbtfss STATUS,Z goto label

endm

jc macro labelbtfsc STATUS,C goto label

endm

jnc macro labelbtfss STATUS,C goto label

endm

org 20h

NextValueServo res 1NextStep res 1PosServo res 1

Cuartet0_Value res 1Cuartet1_Value res 1Cuartet2_Value res 1Cuartet3_Value res 1Cuartet0_LongH res 1Cuartet0_LongL res 1Cuartet1_LongH res 1Cuartet1_LongL res 1Cuartet2_LongH res 1Cuartet2_LongL res 1Cuartet3_LongH res 1Cuartet3_LongL res 1

ADDRH res 1ADDRL res 1DATAH res 1DATAL res 1

TEMP res 1

NumServo res 1 ; numero de servo

CocienteH res 1 ; las siguientes variables son por la rutina de dividirCocienteL res 1ResultadoH res 1ResultadoL res 1SumadorH res 1SumadorL res 1DivisorH res 1DivisorL res 1

TempInt res 1 ; variable temporal para las interrupcionesTempInt2 res 1TempInt3 res 1

org 070h ; variables especialesW_TEMP res 1 ; variables para el gestor de interrupcionesSTATUS_TEMP res 1PCLATH_TEMP res 1

org 0a0hlibres res 16 ; aqui quedan 16 bytes para uso libreServos res 32*2 ; valor de los servos

org 110hStepsSer res 32IncrementSer res 32*2 ; aqui no queda nada libre

org 190hStepsSerNew res 32NextValueSer res 32*2 ; aqui no queda nada libre

TIME50US equ 250 ; numero de ins para 50us (dependera de la velocidad de la CPU

; sera 250 para 20Mhz, 50 para 4Mhz...)

TIMEMIN equ TIME50US*6 ; 0,3msTIMEMAX equ TIME50US*46 ; 2,3ms

TIMESEP equ TIME50USTIMECYCLE equ (TIME50US*50)

; 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 ciclossubwf LongH0,wjnc finser#V(valor) ; si el 1 es

mayor que el 0 saltarjnz swapser#V(valor) ; si es menor saltar a

cambiarmovf LongL0,w ; si parte alta es

igual, otra comparacionsubwf LongL1,wjc finser#V(valor) ; si el 1 es

mayor o igual que el 0 saltarswapser#V(valor):

movf LongH0,w ; intercambiar LongH0 por LongH1movwf TempIntmovf LongH1,wmovwf LongH0movf TempInt,wmovwf LongH1

movf LongL0,w ; intercambiar LongL0 por LongL1movwf TempIntmovf LongL1,wmovwf LongL0movf TempInt,wmovwf LongL1

movf Value0,w ; intercambiar Value0 por Value1movwf TempIntmovf Value1,wmovwf Value0movf TempInt,wmovwf 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,waddwf ResL,fmovf ValH,wskipnc addlw 1addwf 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,waddwf ResL,fcomf ValH,wskipnc addlw 1addwf ResH,f

endm

add16val macro VAL16,ResH,ResLmovlw (VAL16) & 255addwf ResL,fmovlw (VAL16)/256skipnc addlw 1addwf ResH,f

endm

; Incrementar el valor de un campo de 16bitsinc16 macro ResH,ResL

incf ResL,fskipnz incf ResH,f

endm

;******************************************************************************; Vector de reset;******************************************************************************

org 0hclrf PCLATHcall FirmwareUpdategoto main

;******************************************************************************; Rutina interrupción;******************************************************************************; lleva la gestion de 32 servos... es demasiado compleja y un poco desordenada por; no malgastar ciclos de reloj

org 4hMOVWF W_TEMP ;Copy W to TEMP registerSWAPF STATUS,W ;Swap status to be saved into WCLRF STATUS ;bank 0, regardless of current bank, Clears

IRP,RP1,RP0MOVWF STATUS_TEMP ;Save status to bank zero STATUS_TEMP registerMOVF PCLATH, W ;Only required if using pages 1, 2 and/or 3MOVWF PCLATH_TEMP ;Save PCLATH into WCLRF PCLATH ;Page zero, regardless of current page

call PutValueServo ; put value

bcf PIR1,TMR1IF ; interrupcion timer1 tratada

incf NextStep,f ; NextStep++btfss NextStep,3 ; if step 8, dont jump goto NoNextCuartet ; if step<>8 jump

clrf NextStep ; poner que siguiente pasp NextStep=0

; actualizar timer; siguiente int en Timer1+=-(2500us-50us*3-

Cuartet3_Long); =-2350us+Cuartet3_Long

add16 Cuartet3_LongH,Cuartet3_LongL,TMR1H,TMR1L

add16val (65536-(TIMECYCLE-3*TIMESEP)),TMR1H,TMR1L

; actualizar posicion de los servosmovlw 4movwf TempInt ; 4 posiciones

buclenewpos:movf PosServo,w

addlw (StepsSer)&255 ; posicion de stepsmovwf FSRbsf STATUS,IRP ; banco 2-3 para indirectomovf INDF,w ; si era 0 no cambiar nadajz nonewposdecfsz INDF,f ; si era 1 copiar ultimo valor goto calculenew ; si <>1 saltar a sumar incremento

; movwf PosServo,w; andlw 31; addlw -24; skipnc; clrf PosServo

rlf PosServo,w ; calcular w=posservo*2+NextValueSerandlw (31*2)addlw NextValueSer&255 ; leer valor incrementomovwf FSRmovf INDF,wmovwf TempInt2 ; copiar nuevo valor en TempInt2 y 3incf FSR,f ; (con el fin de hacer menos ins cambiandomovf INDF,w ; FSR y bits correspondientes)movwf TempInt3bcf STATUS,IRP ; banco 0-1 para indirecto

movlw Servos-(NextValueSer & 255)+1addwf FSR,f ; nueva posición para acceder a servos

movf TempInt3,w ; mover valor de nuevo servomovwf INDF ; primero parte bajadecf FSR,fmovf TempInt2,w ; ahora parte altamovwf INDF

goto nonewposcalculenew: ; si era otra cosa sumarle el incremento que sea

rlf PosServo,w ; calcular w=posservo*2andlw (31*2)addlw IncrementSer&255 ; leer valor incrementomovwf FSRmovf INDF,wmovwf TempInt2incf FSR,fmovf INDF,wmovwf TempInt3bcf STATUS,IRP ; banco 0-1 para indirecto

movlw Servos-(IncrementSer & 255)addwf FSR,f ; nueva posición para acceder a servos

movf TempInt3,w ; sumar a valor de tabla IncrementSer a Servo

addwf INDF,f ; que correspondamovf TempInt2,wskipnc addlw 1decf FSR,faddwf INDF,f

nonewpos:bcf STATUS,IRP ; banco 0-1 para indirectodecfsz TempInt,f goto buclenewposmovlw -4 ; poner PosServo otra vez con su valoraddwf PosServo,f

; copiar valoresrlf PosServo,wandlw (31*2)addlw Servosmovwf FSR ; calculamos dir como (PosServo and 31)*2+Servos

movf INDF,w ; ir copiando los 8 valoresincf FSR,fmovwf Cuartet0_LongH

movf INDF,wincf FSR,fmovwf Cuartet0_LongLmovf INDF,wincf FSR,fmovwf Cuartet1_LongHmovf INDF,wincf FSR,fmovwf Cuartet1_LongLmovf INDF,wincf FSR,fmovwf Cuartet2_LongHmovf INDF,wincf FSR,fmovwf Cuartet2_LongLmovf INDF,wincf FSR,fmovwf Cuartet3_LongHmovf INDF,wmovwf Cuartet3_LongL

movf PosServo,w ; ahora crear tabla con el orden de los mismos

andlw 31movwf Cuartet0_Valueaddlw 1movwf Cuartet1_Valueaddlw 1movwf Cuartet2_Valueaddlw 1movwf Cuartet3_Valueaddlw 1movwf PosServo

; ordenar los valores de menor a mayor, lo hacemos con una macro

CompareAndSwap

1,Cuartet0_LongH,Cuartet0_LongL,Cuartet1_LongH,Cuartet1_LongL,Cuartet0_Value,Cuartet1_Value

CompareAndSwap


CompareAndSwap


CompareAndSwap


CompareAndSwap


CompareAndSwap


rlf Cuartet0_Value,w ; next value is thisiorlw 1movwf NextValueServo

goto finint

NoNextCuartet:movf NextStep,w ; calculate next valueandlw 3addlw Cuartet0_Valuemovwf FSRrlf INDF,wandlw 62 ; w=(Cuartet0_Value[NextStep&3] and 31)*2btfss NextStep,2 ; if step 0-3... iorlw 1 ; w=w or 1 (put servo to 1)movwf NextValueServo

movlw -3-1addwf NextStep,wjc nomenor3

; si el paso es 1-2-3 siguiente en 50us, Timer1+=-50us

add16val (65536-TIMESEP),TMR1H,TMR1L

goto finint

nomenor3:jnz noescuatro

; si el paso es 4 siguiente en Cuartet0_Long-50us*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 finint

noescuatro:addlw -1jnz noescinco

; si es el 5 siguiente Cuartet1_Long-Cuartet0_Long+50us

; sumar a timer1 -(50us)+Cuartet0_Long+(Cuartet1_Long xor 65535 +1)


add16a1 Cuartet1_LongH,Cuartet1_LongL,TMR1H,TMR1L

add16val (65536-TIMESEP+1),TMR1H,TMR1L

goto finint

noescinco:addlw -1jnz noesseis

; si es el 6 siguiente Cuartet2_Long-Cuartet1_Long+50us

; sumar a timer1 -(50us)+Cuartet1_Long+(Cuartet2_Long xor 65535 +1)




goto finint

noesseis:; si es el 7 siguiente Cuartet3_Long-

Cuartet2_Long+50us; sumar a timer1 -(50us)+Cuartet2_Long+

(Cuartet3_Long xor 65535 +1)add16 Cuartet2_LongH,Cuartet2_LongL,TMR1H,TMR1L



finint:MOVF PCLATH_TEMP, W ;Restore PCLATHMOVWF PCLATH ;Move W into PCLATHSWAPF STATUS_TEMP,W ;Swap STATUS_TEMP register into W ;(sets bank to original state)MOVWF STATUS ;Move W into STATUS registerSWAPF W_TEMP,F ;Swap W_TEMPSWAPF W_TEMP,W ;Swap W_TEMP into Wretfie

;******************************************************************************; Funciones del inicializacion;******************************************************************************; InicializarPIC; Esta rutina inicializa todo el chipitoInicializarPIC:

clrf PORTA ; inicializar a 0 todos los reg de entradaclrf PORTB

ifdef __16F873Aclrf PORTC


clrf PORTCclrf PORTDclrf PORTE


clrf PORTCclrf PORTDclrf PORTE

endifclrf STATUS ; borrar status para tenerlo en estado

conocido

bsf STATUS,RP0 ; poner página 1 (página de configuración de puertos)

movlw 6 ; poner tomas en digitalmovwf ADCON1 & 7fh

movlw 000000000b ; Todo en salidamovwf TRISA & 7Fh

movlw 000000000b ; todo en salidamovwf TRISB & 7Fh

ifdef __16F873Amovwf TRISC & 7Fh


movwf TRISC & 7Fhmovwf TRISD & 7Fh

movlw 00000111b ; poner en entrada los RExmovwf TRISE & 7Fh


movwf TRISC & 7Fhmovwf TRISD & 7Fh

movlw 00000111b ; poner en entrada los RExmovwf TRISE & 7Fh

endif

bcf STATUS,RP0 ; poner página 0

; inicializar interrupciones

bsf STATUS,RP0movlw 1 ; T0CS=0, PSA=0,

PS<2:0>=0(1:2)movwf OPTION_REG & 7fhbcf STATUS,RP0

return

InitServo:movlw 32 ; inicializar los servos a posicion

central 1500msmovwf TEMPmovlw Servosmovwf FSR

bucinise:movlw (TIME50US*30)/256 ; inicializar servos a 1500msmovwf INDFincf FSR,f

movlw (TIME50US*30)&255movwf INDFincf FSR,f

decfsz TEMP,f ; bucle inicializacion servos goto bucinise

bsf STATUS,IRPmovlw 32 ; inicializar StepsSerNewmovwf TEMPmovlw (StepsSerNew&255)movwf FSR

buinise2:clrf INDF ; borrar variableincf FSR,fdecfsz TEMP,f goto buinise2

movlw 32 ; inicializar StepSersmovwf TEMPmovlw StepsSer & 255movwf FSR

buinise3:clrf INDF ; borrar variableincf FSR,fdecfsz TEMP,f goto buinise3bcf STATUS,IRP

movlw 7 ; inicializar NextStep para que en primera int

movwf NextStep ; calcule todo lo que tiene que calcularclrf NextValueServo ; y que primera vez ponga a 0 el servo 0clrf PosServo ; empezamos por primer servo

clrf TMR1L ; inicializar timer1 a FF00hmovlw 255movwf TMR1H

; configurar timer1movlw 1 ; T1OSCEN=0, T1CKPS1:T1CKPS0

preescaler a 1:1, TMR1ON=1, TMR1CS=0 (Internal Clock)movwf T1CON

bsf STATUS,RP0 ; poner página 1bsf PIE1 & 7Fh,TMR1IE ; habilitar interrupciones de timer1bcf STATUS,RP0 ; poner página 0bcf PIR1,TMR1IF ; inicializar a int no ha ocurridobsf INTCON,PEIE ; habilitar interrupciones por

dispositivosbsf INTCON,GIE ; habilitar interrupciones

return

InitSerial:; inicializar puerto serie

movlw 090h ; SPEN=1(7) RX9=0(6) CREN=1(4) ADDEN=0(3)movwf RCSTA

bsf STATUS,RP0 ; poner página 1

movlw 24h ; TX9=0(6) TXEN=1(5) SYNC=0(4) BRGH=1(2) movwf TXSTA & 7fh

movlw 64 ; 19200 a 20Mhz (seria 12 a 4Mhz) y BRGH=1movwf SPBRG & 7fh

; bsf PIE1,RCIE ; habilitar interrupcion de recepción; bsf PIE1,TXIE ; habilitar interrupción de envio

bcf STATUS,RP0return

;******************************************************************************; 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 restodividesinsignoconredon:

bcf STATUS,Crrf DivisorL,w ; sumar el divisor/2addwf CocienteL,fskipnc incf CocienteH,f

dividesinsigno:movf DivisorL,w ; leemos valor por el que dividirmovwf DivisorH ; y lo guardamos en parte altaiorlw 0 ; si es 0 no se puede dividirjz divideby0addlw -1 ; si es 1 devolver lo mismojz divideby1addlw -1 ; si es 2 hacer division por

rotacionjz divideby2clrf DivisorLbcf STATUS,C ; al hacer rotaciones metemos 0rrf DivisorH,frrf DivisorL,f ; lo dejamos en Divisor*128movlw 128 ; inicialmente 7 pasos de restamovwf SumadorL ; sumador con valor 2^7clrf SumadorHclrf ResultadoL ; inicialmente resultado es 0clrf ResultadoH

bucdivisor:btfsc DivisorH,13-8 ; comprobar si divisor es numero pequeño goto FinDivisor ; no, saltarbcf STATUS,C ; si, añadir pasos a la divisionrlf SumadorL,f ; sumador=sumador*2rlf SumadorH,fbcf STATUS,C ; el anterior rlf como no puede desbordar,

con lo que deberia ser necesaria esta insrlf DivisorL,f ; divisor=divisor*2rlf DivisorH,fgoto bucdivisor

FinDivisor:; comparar Cociente con

Divisormovf DivisorH,wsubwf CocienteH,wjnc divimenor ; si el 1 es mayor que el 0

saltarjnz divisormayor ; si es menor saltar a cambiarmovf DivisorL,w ; si parte alta es igual, otra

comparacionsubwf CocienteL,wjnc divimenor ; si el 1 es mayor que el 0

saltar

divisormayor: ; si divisor es mayorcomf DivisorL,w ; restar de cociente el divisoraddwf CocienteL,f ; para ello sumar el complementoa1 +1 (es

decir el complementoa2)skipnc incf CocienteH,fcomf DivisorH,waddwf CocienteH,f

incf CocienteL,f ; sumar el +1skipnz incf CocienteH,f

movf SumadorL,w ; sumar a resultado el sumadoraddwf ResultadoL,fskipnc

incf ResultadoH,fmovf SumadorH,waddwf ResultadoH,f

divimenor:bcf STATUS,C ; dividimos divisor entre 2rrf DivisorH,frrf DivisorL,fbcf STATUS,C ; dividimos sumador entre 2rrf SumadorH,frrf SumadorL,fjnc FinDivisor ; si se queda a 0 hemos acabado,

sino saltar arriba (al bucle)return

divideby0: ; si dividir por 0 deja como resultado 0

clrf ResultadoHclrf ResultadoLreturn

divideby1: ; si dividir por 1 dejar lo mismomovf CocienteH,w ; dar lo mismo movwf ResultadoHmovf CocienteL,wmovwf ResultadoLreturn

divideby2: ; si dividir por 2, hacerlo por rotacion

bcf STATUS,Crrf CocienteH,wmovwf ResultadoHrrf CocienteL,wmovwf ResultadoLreturn

; igual que la anterior pero considerando el signo en el Cocientedivideconsigno:

btfss CocienteH,7 ; si el signo es negativo seguir goto dividesinsigno ; si es positivo dividirlo sin mas

comf CocienteH,f ; cambiarlo de signo el cocientecomf CocienteL,fincf CocienteL,fskipnz incf CocienteH,fcall dividesinsigno ; dividimos sin signocomf ResultadoH,f ; cambiar de signo el resultadocomf ResultadoL,fincf ResultadoL,fskipnz incf ResultadoH,freturn

divideconsignoredondeo:btfss CocienteH,7 ; si el signo es negativo seguir goto dividesinsignoconredon; si es positivo dividirlo sin mas

comf CocienteH,f ; cambiarlo de signo el cocientecomf CocienteL,fincf CocienteL,fskipnz incf CocienteH,fcall dividesinsignoconredon; dividimos sin signocomf ResultadoH,f ; cambiar de signo el resultadocomf ResultadoL,fincf ResultadoL,fskipnz incf ResultadoH,freturn

;******************************************************************************; 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 NumServo,wandlw (31*2)addlw NextValueSer & 0ffhmovwf FSRbsf STATUS,IRP ; banco 2-3 para indirecto

bcf STATUS,IRP ; banco 0-1 para indirectoreturn

; 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 serienopbsf RCSTA,CREN ; y volverlo a conectarreturn

TrataSerie:btfsc RCSTA,FERR ; comprobar si se ha producido error call ResetearSerie ; si hay error resetear el puertobtfsc RCSTA,OERR call ResetearSerie

btfss PIR1,RCIF ; se ha recibido un caracter por puerto serie?

return ; no, volvermovf RCREG,w ; leer caracter recibido

TrataCarSerie:movwf TEMP ; guardar en variable temporalsublw '0' ; recibido un 0jnz sigcar1bsf STATUS,RP0movlw (800*5)/256movwf Servos & 7Fhmovlw (800*5)&255movwf (Servos+1) & 7Fhbcf STATUS,RP0return

sigcar1:movf TEMP,wsublw '1'jnz sigcar2bsf STATUS,RP0movlw (1500*5)/256movwf Servos & 7Fhmovlw (1500*5)&255movwf (Servos+1) & 7Fhbcf STATUS,RP0return

sigcar2:movf TEMP,wsublw '2'jnz sigcar3bsf STATUS,RP0movlw (2200*5)/256movwf Servos & 7Fhmovlw (2200*5)&255movwf (Servos+1) & 7Fhbcf STATUS,RP0return

sigcar3:return

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

;******************************************************************************main:

call InicializarPIC ; inicializar algunas cosascall InitServocall InitSerial

movlw '0'call TrataCarSerie

bsf STATUS,RP1movlw 50movwf StepsSer & 127clrf IncrementSer & 7Fhmovlw 32*5movwf (IncrementSer+1) & 7Fhbcf STATUS,RP1

mainbucle:call TrataSeriegoto 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; número de servo multiplicado por 2, y en el bit bajo 0 el valor a poner en el; puerto . Por ejemplo 20*2+1 pondría a 1 el valor del puerto 20.

org 2048-256-6-128PutValueServo:

movlw 2048/256-2 ; this routine (including the call, uses 11 clock cycles)

movwf PCLATHrlf NextValueServo,w ; w=NextValueServo*2andlw 62 ; w=w and (31*2)addwf PCL,f ; PCL=PCL+Wbcf PORTB,7 ; set 0 port 0returnbsf PORTB,7 ; set 1 port 0returnbcf PORTB,6 ; set 0 port 1returnbsf PORTB,6 ; set 1 port 1returnbcf PORTB,5 ; set 0 port 2returnbsf PORTB,5 ; set 1 port 2returnbcf PORTB,4 ; set 0 port 3returnbsf PORTB,4 ; set 1 port 3returnbcf PORTB,3 ; set 0 port 4returnbsf PORTB,3 ; set 1 port 4returnbcf PORTB,2 ; set 0 port 5returnbsf PORTB,2 ; set 1 port 5returnbcf PORTB,1 ; set 0 port 6returnbsf PORTB,1 ; set 1 port 6returnbcf PORTB,0 ; set 0 port 7returnbsf PORTB,0 ; set 1 port 7returnnop ; set 0 port ?returnnop ; set 1 port ?returnnop ; set 0 port ?returnnop ; set 1 port ?return

nop ; set 0 port ?returnnop ; set 1 port ?returnnop ; set 0 port ?returnnop ; set 1 port ?returnnop ; set 0 port ?returnnop ; set 1 port ?returnnop ; set 0 port ?returnnop ; set 1 port ?returnnop ; set 0 port ?returnnop ; set 1 port ?returnnop ; set 0 port ?returnnop ; set 1 port ?returnnop ; set 0 port ?returnnop ; set 1 port ?returnnop ; set 0 port ?returnnop ; set 1 port ?returnnop ; set 0 port ?returnnop ; set 1 port ?returnnop ; set 0 port ?returnnop ; set 1 port ?returnnop ; set 0 port ?returnnop ; set 1 port ?returnnop ; set 0 port ?returnnop ; set 1 port ?returnnop ; set 0 port ?returnnop ; set 1 port ?returnnop ; set 0 port ?returnnop ; set 1 port ?returnnop ; set 0 port ?returnnop ; set 1 port ?returnnop ; set 0 port ?returnnop ; set 1 port ?returnnop ; set 0 port ?returnnop ; set 1 port ?returnnop ; set 0 port ?returnnop ; set 1 port ?returnnop ; set 0 port ?returnnop ; set 1 port ?returnnop ; set 0 port ?

returnnop ; set 1 port ?returnnop ; set 0 port ?returnnop ; set 1 port ?returnnop ; set 0 port ?returnnop ; set 1 port ?return

;******************************************************************************; 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 dirección 1 a la 1791)

org 1792 ; 2048-256FirmwareUpdate:

bcf STATUS,RP1bcf STATUS,RP0BCF INTCON, GIE ; deshabilitar interrupciones

movlw (1791+2048)/256 ; poner direccion a leermovwf ADDRHmovlw 1791 & 255movwf ADDRLcall ReadProgEEPROM ; leer valor dirección

incfsz DATAL,w ; comprobar si leido es 3FFF goto quizasact1movlw 3fhsubwf DATAH,wskipnz return ; si es 3ffff no actualizar, salimos

quizasact1:; comprobar si versiones son

diferentes; comprobar un checksum para

que sepamos que es correctoclrf ADDRH ; comenzar desde direccion 2movlw 2movwf ADDRL

bucleactf:movlw 2048/256 ; leemos en la direccion+2048addwf ADDRH,fcall ReadProgEEPROMmovlw -(2048/256)addwf ADDRH,fcall WriteProgEEPROMincfsz ADDRL,f incf ADDRH,fmovlw 1742/256subwf ADDRH,wjnz bucleactf

return ; por ahora no hacemos nada, solo salir

ReadProgEEPROMBCF STATUS, RP1 ;BCF STATUS, RP0 ;Bank 0MOVF ADDRL, W ;Write theBSF STATUS, RP1 ;Bank 2MOVWF EEADR & 7Fh ;address bytesBCF STATUS, RP1 ;Bank0MOVF ADDRH,W ;for the desiredBSF STATUS, RP1 ;Bank 2MOVWF EEADRH ;address to readBSF STATUS, RP0 ;Bank 3BSF EECON1, EEPGD ;Point to Program memoryBSF EECON1, RD ;Start read operationNOP ;Required two NOPsNOP ;

BCF STATUS, RP0 ;Bank 2MOVF EEDATA, W ;DATAL = EEDATABCF STATUS, RP1 ;Bank0MOVWF DATAL ;BSF STATUS, RP1 ;Bank 2MOVF EEDATH,W ;DATAH = EEDATHBCF STATUS, RP1 ;Bank0MOVWF DATAH ;return

WriteProgEEPROMBSF STATUS, RP1 ;BCF STATUS, RP0 ;Bank 2MOVF ADDRL, W ;Write addressMOVWF EEADR ;of desiredMOVF ADDRH, W ;program memoryMOVWF EEADRH ;locationMOVF DATAL, W ;Write value toMOVWF EEDATA ;program atMOVF DATAH, W ;desired memoryMOVWF EEDATH ;locationBSF STATUS, RP0 ;Bank 3BSF EECON1, EEPGD ;Point to Program memoryBSF EECON1, WREN ;Enable writesbtfsc INTCON,GIE ; como estan las interrupciones? goto WriteNoInts;Only disable interruptsBCF INTCON, GIE ;if already enabled,;otherwise discardMOVLW 0x55 ;Write 55h toMOVWF EECON2 ;EECON2MOVLW 0xAA ;Write AAh toMOVWF EECON2 ;EECON2BSF EECON1, WR ;Start write operationNOP ;Two NOPs to allow microNOP ;to setup for write;Only enable interruptsBSF INTCON, GIE ;if using interrupts,;otherwise discardBCF EECON1, WREN ;Disable writesreturn

WriteNoInts:MOVLW 0x55 ;Write 55h toMOVWF EECON2 ;EECON2MOVLW 0xAA ;Write AAh toMOVWF EECON2 ;EECON2BSF EECON1, WR ;Start write operationNOP ;Two NOPs to allow microNOP ;to setup for write

BCF EECON1, WREN ;Disable writesreturn

end

; ControlServos€¦ · Web view; 4 interrupciones, separadas por 0,05ms (50us) en la que iremos...

Documents

Transcript of ; ControlServos€¦ · Web view; 4 interrupciones, separadas por 0,05ms (50us) en la que iremos...