Upload
others
View
0
Download
0
Embed Size (px)
Citation preview
; 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
2,Cuartet1_LongH,Cuartet1_LongL,Cuartet2_LongH,Cuartet2_LongL,Cuartet1_Value,Cuartet2_Value
CompareAndSwap
3,Cuartet2_LongH,Cuartet2_LongL,Cuartet3_LongH,Cuartet3_LongL,Cuartet2_Value,Cuartet3_Value
CompareAndSwap
4,Cuartet0_LongH,Cuartet0_LongL,Cuartet1_LongH,Cuartet1_LongL,Cuartet0_Value,Cuartet1_Value
CompareAndSwap
5,Cuartet1_LongH,Cuartet1_LongL,Cuartet2_LongH,Cuartet2_LongL,Cuartet1_Value,Cuartet2_Value
CompareAndSwap
6,Cuartet0_LongH,Cuartet0_LongL,Cuartet1_LongH,Cuartet1_LongL,Cuartet0_Value,Cuartet1_Value
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)
add16 Cuartet0_LongH,Cuartet0_LongL,TMR1H,TMR1L
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)
add16 Cuartet1_LongH,Cuartet1_LongL,TMR1H,TMR1L
add16a1 Cuartet2_LongH,Cuartet2_LongL,TMR1H,TMR1L
add16val (65536-TIMESEP+1),TMR1H,TMR1L
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
add16a1 Cuartet3_LongH,Cuartet3_LongL,TMR1H,TMR1L
add16val (65536-TIMESEP+1),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
endif ifdef __16F874A
clrf PORTCclrf PORTDclrf PORTE
endif ifdef __16F877A
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
endif ifdef __16F874A
movwf TRISC & 7Fhmovwf TRISD & 7Fh
movlw 00000111b ; poner en entrada los RExmovwf TRISE & 7Fh
endif ifdef __16F877A
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