Codigo para posicionar servos con microcotrolador Pic

De Asociación de Robotica y Domótica de España (A.R.D.E.)

Codigo para posicionar servos con microcotrolador Pic

Según los valores colocados en DuServ1L-DuServ1H Posiciona el servo 1 de -90º para 0 a +90º para 1000. En DuServ2L-DuServ2H se coloca el valor del servo dos.

Siguiendo este procedimiento es muy sencillo llegar a los 8 servos.


Enviado por: Bactering

Fichero fuente original: Descargar 


  1. ;	  Servo motor		
  2. ;	José Vte Mateo		   
  3. ;********************************************************************
  4. ;El TMR1 en el modo temporizador.
  5. ;Revisión: 00.6		12-04-2007 Fin a la incorporación dos servos
  6. ;
  7. ;Salida Servo1 PORTB,0. Servo2 PORTB,1
  8. ;********************************************************************
  9.  
  10. Servo	equ	0x2d
  11. Comp	equ	0x2e		;Valor a complementar
  12. Dutty	equ	0x2f
  13. Dutty1L	equ	0x30
  14. Dutty1H	equ	0x31
  15. TimeL	equ	0x32
  16. TimeH	equ	0x33
  17. TempL	equ	0x34
  18. TempH	equ	0x35
  19. DuServ2L	equ	0x36
  20. DuServ2H	equ	0x37
  21. DuServ1L equ	0x38		; de 0 a 1000 valor a carvar posición servo
  22. DuServ1H equ	0x39		;de 0 a 1000 valor a carvar posición servo
  23. RESH	equ	0x40
  24. RESM	equ	0x41
  25. RESL	equ	0x42
  26. SUSH	equ	0x44
  27. SUSL	equ	0x43
  28. SUM1H	equ	0x45
  29. SUM1L	equ	0x46
  30. SUM2H	equ	0x47
  31. SUM2L	equ	0x48
  32.  
  33.  
  34. 		list p=16f877,	f=INHX8M, r=hex
  35. 		include "p16f877.inc"
  36.  
  37. 			org	0
  38. 			goto	Inicio
  39.  
  40. 			org	4
  41.  
  42. ;********************************************************************
  43. ;TMR1 trabaja con oscilador interno y preescaler de 1:8. Si se trabaja
  44. ;a una frecuencia de 4 MHz, TMR1 deberá ser cargado con 62500 para que
  45. ;provoque interrupción a los 0,5 segundos.
  46. ;********************************************************************
  47.  
  48. Inter:			btfss	PIR1,TMR1IF	;	¿Inter por TIMER1?
  49. 			retfie
  50.  
  51. 			btfsc	Servo,0		; es el primer servo?
  52. 			goto	Servo0
  53. 			btfsc	Servo,1
  54. 			goto	Servo1
  55. 			retfie
  56.  
  57. Servo0			btfss	Dutty,0		; esta activa la salida?
  58. 			goto	FinServo0
  59. 			nop 
  60. 			nop
  61. 			bcf	PORTB,0	
  62. 			call	Resta		; tiempo que queda por estar a cero		
  63. 			call	Carga22	
  64. 		;Desconecta el dutty servo 0
  65. 			bcf	Dutty,0
  66. 			goto	SalIntTiemr1
  67.  
  68. FinServo0		BCF	Servo,0
  69. 			BSF	Servo,1
  70. 			movfw	Dutty1L
  71. 			movwf	SUM1L
  72. 			movfw	Dutty1H
  73. 			movwf	SUM1H
  74. 			movfw	DuServ2L
  75. 			movwf	SUM2L
  76. 			movfw	DuServ2H
  77. 			movwf	SUM2H	
  78. 			CALL	Suma
  79. 			Call	Carga22		
  80. 			bsf	Dutty,1	;Activa tiempo servo 1
  81. 			bsf	PORTB,1	; ACTIVA EL SERVO
  82. 			goto	SalIntTiemr1
  83.  
  84. Servo1			btfss	Dutty,1		; esta activa la salida?
  85. 			goto	FinServo1
  86. 			bcf	PORTB,1	
  87. 			call	Resta		; tiempo que queda por estar a cero	
  88. 			Call	Carga22			
  89. 		;Desconecta el dutty servo 0
  90. 			bcf	Dutty,1
  91. 			goto	SalIntTiemr1
  92.  
  93. FinServo1		bcf	Servo,1		; desconecta 	el servo y conecta siguiente
  94. 			bsf	Servo,0	
  95. 			movfw	Dutty1L
  96. 			movwf	SUM1L
  97. 			movfw	Dutty1H
  98. 			movwf	SUM1H
  99. 			movfw	DuServ1L
  100. 			movwf	SUM2L
  101. 			movfw	DuServ1H
  102. 			movwf	SUM2H	
  103. 			CALL	Suma
  104. 			Call	Carga22					
  105. 			bsf	Dutty,0	;Activa tiempo servo 1
  106. 			bsf	PORTB,0	; ACTIVA EL SERVO					
  107. SalIntTiemr1		bcf	PIR1,TMR1IF		;Desconecta el flag del TMR1
  108. 			retfie	
  109. ;¿¿¿¿¿¿¿¿¿¿¿¿¿¿¿¿¿¿¿¿¿¿¿¿¿¿¿¿¿¿¿¿¿¿¿¿¿¿¿¿¿¿¿¿¿¿¿¿¿¿¿¿¿¿¿¿¿¿¿¿¿¿¿¿¿¿¿¿¿¿¿¿¿¿¿¿¿¿¿¿¿¿¿¿¿¿¿¿¿¿¿¿¿¿¿¿¿¿¿¿¿¿
  110. ;Conversión analógica
  111.  
  112. 			movfw	ADRESL
  113. 			movwf	DuServ2L	
  114. 			movfw	ADRESH
  115. 			movwf	DuServ2H
  116.  
  117. Inicio			clrf 		PORTB	;Borra los latch de salida.
  118. 		; Con una temporización de 991 se queda a -90º.
  119. 			movlw		LOW .991		;Carga en TMR1, en hexadecimal, 
  120. 			movwf		Dutty1L				;el complemento de 62500 decimal.
  121. 			movlw		HIGH .991	;	"
  122. 			movwf		Dutty1H	
  123. 			movlw		LOW .10000		;Carga en TMR1, en hexadecimal, 
  124. 			movwf		TimeL				;el complemento de 62500 decimal.
  125. 			movlw		HIGH .10000	;	"
  126. 			movwf		TimeH	
  127. ;_______________________________________________________________________________________		
  128. 			movlw		LOW .251	;				|
  129. 			movwf		DuServ1L	;				|
  130. 			movlw		HIGH .251	;	"			|
  131. 			movwf		DuServ1H	;				|
  132. 			movlw		LOW .751	;				|
  133. 			movwf		DuServ2L	;				|
  134. 			movlw		HIGH .751	;	"			|
  135. 			movwf		DuServ2H	;				|
  136. ;_______________________________________________________________________________________|			
  137. 			bsf		STATUS,RP0		;Selecciona banco 1.
  138. 			clrf		TRISB				;Puerto B: salida.
  139. 			movlw		b'11111111'		;Preescaler = 128, asociado al WDT.
  140. 			movwf		OPTION_REG		;	"
  141. 			bsf		PIE1,TMR1IE		;Habilita interrupción del TMR1.
  142. 			bcf		STATUS,RP0		;Selecciona banco 0.
  143. 			clrf	PORTB
  144. 			clrf	Dutty
  145. 			clrf	Servo
  146.  
  147. 			movfw	Dutty1L
  148. 			movwf	SUM1L
  149. 			movfw	Dutty1H
  150. 			movwf	SUM1H
  151. 			movfw	DuServ1L
  152. 			movwf	SUM2L
  153. 			movfw	DuServ1H
  154. 			movwf	SUM2H
  155.  
  156. 			CALL	Suma
  157. 			call	Carga22
  158. 			bsf	Dutty,0
  159. 			bsf	Servo,0	
  160. 			bsf	PORTB,0	;activa salida			;	"
  161. 			movlw	b'11000000'		;Habilita interrupciones generales y de
  162. 			movwf	INTCON			;periféricos.
  163. 			movlw	b'00000001'		;Timer1 habilitado, reloj interno y 1:8 de
  164. 			movwf	T1CON				;predivisión.	
  165. Bucle:			clrwdt				;Inicializa el Supervisor.
  166. 			goto	Bucle
  167.  
  168. ; Resta el intervalo del ciclo, Tiempo a cero del serco			
  169. Resta			clrf	RESH			;Borra registros de resultados.
  170. 			clrf	RESM			;		"
  171. 			clrf	RESL			;
  172. 			movfw	Dutty1L
  173. 			movwf	SUSL
  174. 			movfw	Dutty1H
  175. 			movwf	SUSH		
  176.  
  177. 			movf	SUSL,W		;Carga el sustraendo bajo en W.
  178. 			subwf	TimeL,W		;Ejecuta MINL - SUSL y deja en W el resultado.
  179. 			movwf	RESL			;Carga resultado bajo en registro 42.
  180. 			btfsc	STATUS,C		;¿Resultado negativo en operación anterior?
  181. 			goto	uno			;No: salta a restar operandos altos.
  182. 			incf	SUSH,F		;Sí: incrementa sustraendo alto.
  183. 			btfsc	STATUS,Z		;¿Hay desbordamiento en incremento?
  184. 			decf	RESH,F		;Sí: pone resultado en negativo.
  185. uno:			movf	SUSH,W		;No: carga sustraendo alto en W.
  186. 			subwf	TimeH,W		;Resta bytes altos.
  187. 			movwf	RESM			;Deja resultado alto en registro 41.
  188. 			btfss	STATUS,C		;¿Resultado negativo en resta?
  189. 			decf	RESH,F		;Sí: escribe resultado negativo.
  190. 			return
  191. ;Suma el valor mínimo del dutty a la posición del servo.			
  192. Suma			clrf	RESL			;Borra registros de resultados.
  193. 			clrf	RESM			;	"
  194. 			clrf	RESH			;	"
  195. 			movf	SUM1L,W		;Se carga en W el byte bajo del sumando 1.
  196. 			addwf	SUM2L,W		;Se suman los bytes de menor peso.
  197. 			movwf	RESL			;Se coloca el byte bajo del resultado.
  198. 			btfss	STATUS,C		;¿Ha habido Acarreo en la suma anterior?
  199. 			goto	DOS			;No: salta a sumar los bytes altos.
  200. 			incf	SUM1H,F		;Sí: incrementa uno de los sumandos altos.
  201. 			btfsc	STATUS,Z		;¿Hay desbordamiento en el incremento?
  202. 			incf	RESH,F		;Sí: incrementa el resultado alto.
  203. DOS:			movf	SUM1H,W		;Carga en W uno de los sumandos altos.
  204. 			addwf	SUM2H,W		;Suma los bytes de peso alto.
  205. 			movwf	RESM			;Coloca en registro el resultado alto.
  206. 			btfsc	STATUS,C		;¿Hay acarreo en la suma final?
  207. 			incf	RESH,F		;Sí: incrementa resultado alto.
  208. 			return
  209.  
  210. Carga22			movfw	RESL		;Carga en TMR1, en hexadecimal, 
  211. 			xorlw	0xff
  212. 			movwf	TMR1L				;el complemento de 62500 decimal.
  213. 			movfw	RESM	;	"
  214. 			xorlw	0xFF
  215. 			movwf	TMR1H				
  216. 			return
  217.  
  218. 	end
Obtenido de "http://wiki.webdearde.com/index.php/Codigo_para_posicionar_servos_con_microcotrolador_Pic"
Herramientas personales