Dos motores iguales... Dos velocidades diferentes !?!?!?!?

[manex_1987]

Hola y muy buenas de nuevo! Hacia mucho tiempo que no posteaba en estos lares, pero vuelvo a la carga, ahora como estudiante de 2º de electronica... y pensar que era un estudiantillo de 3º de eso cuando os conoci... xDDDDD bueno en fin, espero que podais ayudarme con mi robotito.

Resulta que ya me costó fijar los motores a la estructura (metacrilato contra plastico, pasando de usar loctite jajaja), para que luego me de cuenta de que el robot va trazando curva a la derecha segun avanza. No es demasiado, pero soy muy exigente y quiero que mi robot ande recto. Total que al principio pensaba qque seria el emplazamiento de los motores, uno mas adelante que el otro... y que no. Se cumplen las peores expectativas: Un motor avanza mas rapido que el otro!!! Y ambos son los mismos FUTABA S3003 trucados de siempre!!!

Necesito vuestra ayuda, este problema supera mis capacidades de bricolaje (que ya de por si rozan el cero absoluto). He probado a intercambiar sus sistemas de engranajes pero un motor sigue imponiendose sobre el otro. Y se nota sobre todo en el ruido, uno hace un ruido mas agudo que el otro, como si "tirara" mas. Aun prescindiendo de los reductores, el sonido sigue siendo el mismo.

Es algo caracteristico del motor? Problema del driver L293? Friccion dentro del motor? No creo que sea friccion pues yo nisiquiera los he tocado por dentro. Solo los abri para trucarlos, lo cual no implica manipular internamente el bloque del motor.

Espero vuestra ayuda!gracias!

PD: Mi ultima esperanza será el sistema PWM que quiero implantar al robot, para regular la velocidad de cada motor... ya tendreis mas noticia de mi jeje.

[Chaly29]

Hola manex_1987, es normal que 2 motores (aunque parescan iguales) en realidad funcionen distinto, esto se puede deber a infinidad de condiciones al momento de fabricarlos y que estan fuera del alcance de la fábrica poder correjir.

Para solucionar ese problema tienes varias posibilidades, como ser por software o sea realizando un control independiente sobre los motores, de esta manera compensas el error que hay entre ellos o por hardware colocandole una resistencia de sificiente potencia y el valor necesario en serie sobre el motor que funciona más rapido, con esto le bajaras la tensión y por consiguiente las RPM que este entrega, pero puedes tener más problemas con este ultimo método que con el primero.

Espero te sea de ayuda. Un saludo.

Atte. CARLOS.

[aitopes]

Coincido totalmente con Carlos. En un desarrollo que tengo con 10 motores de CC, no hay dos que giren a la misma velocidad. Es medio una pesadilla, pero tiene solucion: un sensor (HALL, optico, etc) + PIC + PWM. De esa manera, ajustas el PWM a las revoluciones reales de cada motor en particular.

Saludos!

[manex_1987]

Muchas gracias... aun asi, no he visto ningun robot en internet que mencione problemas de ese tipo, aun usando los mismos futaba s3003. Acaso la "desviacion" que menciono es completamente aceptable? Soy un perfeccionista?

Aun asi, voy a empaparme un poco de programacion DsPic, pues mi siguiente meta deberia ser un controlador pwm de 4 canales (pues el L293 tiene 4 entradas). Ideas? Alguien ha hecho algo parecido?

[aitopes]

Sinceramente, y aunque no heu sado servos trucados, dudo mucho que despues de anadar un par de metros no se haya desviado algo. Como generalemente estan montados en robot sigue-lineas o cosas por el estilo, se van haciendo correcciones todo el tiempo y el defecto no se nota.

Respecto del PWM, no lo he hecho con cuatro canales a la vez, pero es perfectamente posible usando interrupciones (RedPic tiene en este foro un excelente tutorial de como manejar servos en C, y la idea es aplicable a tu problema). Basicamente, tenes que medir el tiempo que tarda la rueda en dar una vuelta, compararlo con el que deberia tardar si fuera a la velocidad correcta, y en base a ese dato modificar el duty cicle del PWM.

Saludos!

[dogflu66]

Hola manex me alegra tu regreso.
pero si es un tema que se a tratado
http://www.todopic.com.ar/foros/index.php?topic=10746.105
http://www.freewebs.com/glafebre/miguelito.htm

[dogflu66]

Son encoder con cny70 montados en futabas 3003 para corregir ese problema y depende para que sea la utilidad es un grabe problema el que siempre un motor gire mas rapido que otro y desde luego si hay realimentación de por medio no es necesario hacer la corrección.

[manex_1987]

Muchisimas gracias campeon! Nunca se me habia ocurrido tal idea! Ademas, si el encoder va en los engranajes, ganamos una bastardada de puntos en precision... pues el engranaje bajo el que has puesto el encoder, es el primero en el desarrollo...

Me he bajado tu dossier sobre el robot miguelito pero aun asi tengo una duda. Como "codificas" el engranaje? cuantas marcas o con qué o...?

Aun asi, para que no me "robe" potencia todo el tinglado que quiero montar, estoy pensando en independizar la parte de los CNY70 en un solo micro. Este me daria las RPM por una parte (mediante I2C o asi), y por otra, las señales de los CNY (estas acopladas directamente pin a pin). Sigo pensando en usar un dspic para controlar por PWM los dos servos; para regular su velocidad/par. Necesitaria 4 canales PWM no? Porque cada motor tiene 2 polos.

un saludo!

[dogflu66]

Uso un trocito de cinta aislante de color negro que cubre aproximadamente la mitad del engranaje, con lo que da 45 puntos de resolución por vuelta de la rueda, se podría colocar 4 trocitos de cinta aislante ya que tienen que ser algo mas anchos que la optica del cny, para que de buen resultado la lectura de los encoder y mantener el pwm de los motores al mismo tiempo, mas la logica de control del programa.
Desde luego si quieres buena resolución lo mejor es utilizar un micro a parte.

Para controlar los servos, en el caso del miguelito son servos convertidos a motores dc, utilizo tres lineas de salida por motor, dos para controlar el sentido del motor y activar el freno motor, y una mas para el pwm, por lo tanto 1 pwm por motor.

Y a lo de utilizar un dspic, pues no tengo palabras para ello.. jeje 

[manex_1987]

Ok, 'amos a ver... usas un pwm por motor... y la señal pwm va a parar al pin ENABLE de cada canal en el L293, no?

Un motor ya lo he trucado, he pintado con un rotulador gordo de estos el engranaje de negro la mitad, y en el banco de pruebas ha funcionado bien; el polimetro me marcaba 54Hz de frecuencia constantes, por lo que se puede decir que la onda cuadrada es bien cuadrada.

El segundo motor... ufa... me acabo de dar cuenta de que mi CNY70 está quemado. Si, como lo oís. El diodo emisor del cny no ilumina por mucho que me empeñe con 5V y con o sin resistencia. Tendre que sacar otro CNY70 de la manga.... joderrrrrrrrrrrrrrrrr

ya os contare mas.

Por cierto, micro aparte si, pero tal y como estan las cosas, y ya que me comentas lo del pwm... podria usar un pic no? Pero hay pics con 2 canales pwm???

un saludo

[ESTECA55]

Hola,  por lo general el PWM se aplica el enable del puente H, pero no nesarimente puedes usar el pwm incorporado del pic, puedes creoarlo por soft, es muy sentillo utilizando los timers de los pic

[dogflu66]

El modulo ccp es el que se puede utilizar para generar pwm de una forma directa, pero tienes que tener en cuenta que se ve afectado directamente por la frecuencia del cristal que estes utilizando en el microcontrolador.

por el momento no he llegado a utilizarlos porque comencé trabajando con el micro de la controladora a 20Mhz y con servos, y a esta velocidad de clock los modulos trabajan tan deprisa que no se puede controlar los servos directamente con el modulo ccp. Asi que solo he generado pwm utilizando las interrupciones del pic para controlar servos y servos trucados, como te indica el pana esteca.

[aitopes]

Hola Dogflu!
Tienes publicado en algun hilo un ejemplo de como controlar los servos desde PIC SIMULATOR IDE? Te pregunto por que yo uso ese Basic, y me estoy rompiendo la cabeza con C por que pense que no iba a poder hacerlo desde ese Basic......
Si lo tienes (y es publico, claro!) me gustaria me pases el link.

Saludos!

[dogflu66]

Aqui te dejo el ultimo codigo operativo del Bot Explorer, a ver si un dia de estos le echo otro rato y termino el articulo y dejo un video para que lo veais funcionar.

Código: Visual Basic

1. '***************** Explorer RX ***************************************
2. 'NOMBRE: Explorer_Rx_Tx_18
3. 'MICRO: PIC16f877A
4. 'DESCRIPCION: COMUNICACION RF PARA CONTROLAR 2 MOTORES y 5 Servos, tipo Mando/Receptor
5. 'tiempo de lectura entre byte y byte del puerto serie +-10mseg maximo y un +-milisegundo minimo
6. 'Version 1.0
7. 'Programa Bot_Miguelito_RF_10
8. 'Se le añade una quinta tecla o maniobra
9. 'Version 1.1
10. 'Implementacion de hasta 11 byte en la trama de datos
11. 'Version 1.2
12. 'Implementacion de un servo_1 en el PROTC.2
13. 'Implementacion de up/down cada 100ms para servo_1
14. 'Version 1.3
15. 'Mejora de la resolucion del servo_1
16. 'Eliminacion de los 100ms de retrazo para servo_1
17. 'Version 1.4
18. 'Implementacion de cinco servos
19. 'No funciona bien el PORTA.4 para manejar servos, asi que se cambia al PORTA.5
20. 'Implementacion del control de los servos con el teclado de cinco teclas
21. 'Version 1.5
22. 'Se mejora la rutina del control de los servos para que actue sobre todo el recorrido de estos
23. 'Nuevo calibrado del punto de reposo de los servos y punto maximo y minimo de cada articulacion
24. 'Version 1.6
25. 'Se cambia la rutina de las bases de tiempo
26. 'Se refresca el display cada 100ms
27. 'Se hace independiente el posicionamiento de los servos (del main) de la llamada a la lectura del bufer
28. 'Se refresca la nueva posicion de los servos cada 100ms
29. 'Version 1.7
30. 'Se amplia el numero de timer
31. 'Se refresca el display cada 50ms
32. 'Se aumenta el paso de los codos a 200ms y el resto se disminulle a 50ms
33. 'Se asocia la pinza/muñeca a la misma maniobra piza/codo, se mantiene tambien la anterior asociacion
34. 'Se rectifica la rutina de los servos y pequeño calibrado de los puntos de trabajo
35. 'No se ejecuta el codigo de la usart ubicado en el vector interrupciones, mientras funciona el codigo de los servos
36. 'para evitar temblores en las articulaciones en reposo
37. 'Version 1.8
38. 'Incremeto de la velocidad de los comandos que manejan el display
39. '************************************************************************************************
40. '-------------------------------------Lo primero Programa-----------------------------------------
41. '-------------------------------------PUERTOS DEFINIDOS POR EL SISTEMA----------------------------
42. '-------------------------------------Puertos del LCD---------------------------------------------
43. Define LCD_BITS = 8
44. Define LCD_DREG = PORTB
45. Define LCD_DBIT = 0
46. Define LCD_RSREG = PORTC
47. Define LCD_RSBIT = 0
48. Define LCD_EREG = PORTC
49. Define LCD_EBIT = 4
50. Define LCD_RWREG = PORTC
51. Define LCD_RWBIT = 1
52. Define LCD_COMMANDUS = 2000     'Tiempo de espera despues de ejecutar un comand del lcd
53. Define LCD_DATAUS = 50  'Tiempo de espera despues de enviar un dato al lcd
54. Define LCD_INITMS = 50  'Tiempo de espera despues de inicializar el display, solo se ejecuta una vez
55. '--------------------------------------DEFINICION DE VARIABLES-------------------------------------
56. '-------------------------------------Variables sub_rutinas del puerto serie---------------------
57. Dim assi As Byte  'Contiene el valor de lectura del puerto serie
58. Dim n_usart As Byte  'Numero de datos del USART y del bufer IMAGEN de control
59. Dim usart As Byte  'Variable indice que apunta al ultimo dato adquirido por la USART
60. Dim aux_usart As Byte  'Auxiliar de la variable indice usart del USART
61. Dim c_serial As Byte  'Como USART pero para la lectura de control del bufer IMAGEN
62. Dim bufer As Byte  'Indica si hay datos por leer en el bufer USART
63. '-------------------------------------Variables del contador del TIMER1----------------------------
64. Dim aux_pause As Word  'variable auxiiar
65. Dim pause As Word  'contiene el valor para simular el comando Waitms
66. Dim basetimer_1ms As Byte  'base de tiempos raiz
67. Dim timer_1ms As Word  'base de tiempos se incrementa cada 1mSeg
68. Dim timer1_10ms As Byte  'base de tiempos se incrementa cada 10mSeg minimo
69. Dim timer2_10ms As Byte
70. Dim timer3_10ms As Byte
71. Dim timer4_10ms As Byte
72. Dim timer5_10ms As Byte
73. Dim timer6_10ms As Byte
74. Dim timer1 As Byte  'fija el valor real de los timer
75. Dim timer2 As Byte
76. Dim timer3 As Byte
77. Dim timer4 As Byte
78. Dim timer5 As Byte
79. Dim timer6 As Byte
80. '--------------------------------------Variables R/W de la Trama de Datos y Lectura Entradas --------
81. Dim arranque As Byte    'arranque, estabiliza el bit de bajada
82. Dim grupo As Byte               'byte de sincronismo (grupo de trabajo)
83. Dim checsum As Byte             'interviene en la deteccion de errores en las tramas de datos
84. Dim w_posrx As Byte             'Tiempo entre lectura y lectura del bufer
85. Dim rd_grupo As Byte    'Variables imagen (contienen los datos leidos del puerto serie)
86. Dim rd_checsum As Byte
87. Dim rd_ent0 As Byte
88. Dim rd_ent1 As Byte
89. Dim rd_ent2 As Byte
90. Dim rd_ent3 As Byte
91. Dim rd_ent4 As Byte
92. Dim rd_ent5 As Byte
93. Dim rd_ent6 As Byte
94. Dim rd_ent7 As Byte
95. Dim rd_ent8 As Byte
96. Dim rd_ent9 As Byte
97. Dim rd_ent10 As Byte
98. Dim entrada As Byte             'contiene el valor de las entradas
99. Dim aux_ent0 As Byte    'Contienen el estado de las entradas para enviar en la trama
100. Dim aux_ent1 As Byte
101. Dim aux_ent2 As Byte
102. Dim aux_ent3 As Byte
103. Dim aux_ent4 As Byte
104. Dim aux_ent5 As Byte
105. Dim aux_ent6 As Byte
106. Dim aux_ent7 As Byte
107. Dim aux_ent8 As Byte
108. Dim aux_ent9 As Byte
109. Dim aux_ent10 As Byte
110. Dim e_0 As Byte  'contiene los ultimos valores para aplicar leidos de la trama
111. Dim e_1 As Byte
112. Dim e_2 As Byte
113. Dim e_3 As Byte
114. Dim     e_4 As Byte
115. Dim     e_5 As Byte
116. Dim     e_6 As Byte
117. Dim     e_7 As Byte
118. Dim     e_8 As Byte
119. Dim     e_9 As Byte
120. Dim     e_10 As Byte
121. '---------------------------------------Variables del Control de los Servos ------------------------
122. Dim servos_control As Byte      'Controla que se ejecute la rutina de control de los servos
123. Dim servos_contador As Byte     'Indica el tiempo de control transcurrido en la rutina de servos
124. Dim servos_aux_contador As Byte
125. Dim servo_1 As Byte     'Pinza/Pinza
126. Dim servo_2 As Byte     'Pinza/Muñeca
127. Dim servo_3 As Byte     'Pinza/Codo
128. Dim servo_4 As Byte     'Camara/Muñeca
129. Dim servo_5 As Byte     'Camara/Codo
130. Dim mecanismo As Byte  'Selecciona el mecanismo a controlar, 0 = motores ruedas, 1 = pinza/pinza y pinza/muñeca
131. '2 = pinza/muñeca y pinza/codo, 3 = camara/muñeca y camara/codo
132. '---------------------------------------Variables del Programa -------------------------------------
133. Dim n As Byte  'variable auxiliar para las rutinas del programa
134. '------------------------------------VARIABLES tipo array programa----------------------------------
135. '------------------------------------VARIABLES tipo array sub_rutinas-------------------------------
136. '------------------------------------Variables tipo array puerto serie------------------------------
137. Dim usart_imagen(31) As Byte  'Componen el bufer IMAGEN para control del bufer USART
138. Dim usart_bufer(31) As Byte  'Componen el bufer de la USART de bajo nivel
139. '-----------------------------------------------ASIGNACIONES----------------------------------------
140. '-------------------------------------Asigna valores a las variables de la rutina de los servos----
141. servos_control = 0
142. servos_contador = 0
143. servos_aux_contador = 0
144. servo_1 = 63  'Pinza/Pinza
145. servo_2 = 46  'Pinza/Muñeca
146. servo_3 = 61  'Pinza/Codo
147. servo_4 = 47  'Camara/Muñeca
148. servo_5 = 50  'Camara/Codo
149. mecanismo = 1
150. '-------------------------------------Asigna valores a las variables de la rutina del puerto serie--
151. usart = 0
152. aux_usart = 0
153. c_serial = 0
154. bufer = 0
155. n_usart = 30
156. assi = 0
157. '-------------------------------------Asigna valores a las variables del TIMER1---------------------
158. aux_pause = 0
159. pause = 0
160. basetimer_1ms = 0
161. timer_1ms = 0
162. timer1_10ms = 0
163. timer2_10ms = 0
164. timer3_10ms = 0
165. timer4_10ms = 0
166. timer5_10ms = 0
167. timer6_10ms = 0
168. timer1 = 5      '50ms
169. timer2 = 35     '350ms
170. timer3 = 5      '50ms
171. timer4 = 20     '200ms
172. timer5 = 5      '50ms
173. timer6 = 20     '200ms
174. '--------------------------------------asignacion de valores generales-----------------------------
175. arranque = 0
176. grupo = 250
177. rd_ent0 = 1
178. rd_ent1 = 1
179. rd_ent2 = 1
180. rd_ent3 = 1
181. rd_ent4 = 1
182. rd_ent5 = 1
183. rd_ent6 = 1
184. rd_ent7 = 1
185. rd_ent8 = 1
186. rd_ent9 = 1
187. rd_ent10 = 1
188. aux_ent0 = 1
189. aux_ent1 = 1
190. aux_ent2 = 1
191. aux_ent3 = 1
192. aux_ent4 = 1
193. aux_ent5 = 1
194. aux_ent6 = 1
195. aux_ent7 = 1
196. aux_ent8 = 1
197. aux_ent9 = 1
198. aux_ent10 = 1
199. entrada = 0
200. entrada.0 = 1
201. entrada.1 = 1
202. entrada.2 = 1
203. entrada.3 = 1
204. entrada.4 = 1
205. e_0 = 1
206. e_1 = 1
207. e_2 = 1
208. e_3 = 1
209. e_4 = 1
210. e_5 = 1
211. e_6 = 1
212. e_7 = 1
213. e_8 = 1
214. e_9 = 1
215. e_10 = 1
216. w_posrx = 10  '10mSeg rf, se espera un maximo de tiempo antes de leer el bufer si no hay datos
217. '--------------------------------------DEFINICION DE PUERTOS------------------------------------------------
218. '--------------------------------ASIGNACION DE I/O y valores de inicion de las salidas----------------------
219. ADCON1 = 0x07
220. TRISA = 0xff
221. TRISE.0 = 1
222. TRISE.1 = 1
223. TRISE.2 = 1
224. TRISC.5 = 0
225. TRISD = 0x00
226. TRISD.0 = 1
227. TRISD.1 = 1
228. PORTC.5 = 0
229. PORTD.2 = 1
230. PORTD.3 = 1
231. PORTD.4 = 1
232. PORTD.5 = 1
233. PORTD.6 = 1
234. PORTD.7 = 1
235. If    RE1 = 0 Then      'Si trabajo en modo receptor
236.         TRISA = 0x0
237.         PORTA.0 = 0
238.         PORTA.1 = 0
239.         PORTA.2 = 0
240.         PORTA.3 = 0
241.         PORTA.4 = 0
242.         PORTA.5 = 0
243. Endif
244. '------------------------------------INICIO DEL PUERTO SERIE, TIMER E INTERRUPCONES------------------
245. Hseropen 1200  'Velocidad del puerto serie e inicializa los comandos de bajo nivel que controlan la USART
246. INTCON.PEIE = 1  'bit de habilitacion de interrupciones de perifericos(USART), para que funcione la implementacio P.serie
247. PIE1.RCIE = 1  'Activa la interrupcion de la USART en modo rx
248. T1CON.TMR1ON = 1  'Configuracion y habilitacion del TMR1
249. T1CON.TMR1CS = 0
250. T1CON.T1CKPS0 = 0
251. T1CON.T1CKPS1 = 0
252. TMR1H = 0xec    'Para 1ms
253. TMR1L = 0x78
254. PIE1.TMR1IE = 1
255. Enable  ' INTCON.GIE habilita todas las interrupciones globales
256. '--------------------------------------Inicio Programa--------------------------------------------------
257. Lcdinit 'Inicializa el display
258. pause = 1000
259. Gosub pause_ms
260. Lcdcmdout LcdClear
261. Lcdout "HOLA TOY LISTO:"
262. Gosub stop_md  'para motores
263. Gosub stop_mi
264. Gosub error_usart  'Borra el BUFFER IMAGEN y por lo tanto tambien al USART, OBLIGATORIO POR LO MENOS una vez
265. pause = 500
266. Gosub pause_ms
267. Lcdcmdout LcdClear
268. '--------------------------------------Main Programa----------------------------------------------------
269. main:
270.         If RCSTA.OERR = 1 Then  'testea posible bloqueo del puerto fisico en modo Rx
271.                 Gosub error_usart
272.         Endif
273.         If RE1 = 1 Then  'trabajo en modo emisor
274.                 Gosub lectura_entradas  'lee las entradas en ent0...3, teclas
275.                 Gosub envio_comando     'envia la trama
276.                 Lcdcmdout LcdHome       'muestra las teclas pulsadas
277.                 Lcdout "Tx" #aux_ent0, #aux_ent1, #aux_ent2, #aux_ent3, #aux_ent4, " ", #aux_ent5, " ", #aux_ent6, " "
278.                 Lcdcmdout LcdLine2Home
279.                 Lcdout #aux_ent7, " ", #aux_ent8, " ", #aux_ent9, " ", #aux_ent10, " "
280.         Endif
281.         If RE1 = 0 Then  'trabajo en modo receptor
282.                 If timer1_10ms >= timer1 Then  'muestra maniobras y posicion de los servos, cada 50ms
283.                         Lcdcmdout LcdHome
284.                         Lcdout "Rx" #e_0, #e_1, #e_2, #e_3, #e_4, " ", #servo_1, " ", #servo_2, " "
285.                         Lcdcmdout LcdLine2Home
286.                         Lcdout #servo_3, " ", #servo_4, " ", #servo_5, " ", #mecanismo, "  "
287.                         timer1_10ms = 0
288.                 Endif
289.                 If bufer > 0 Then  'Rx, lee el puerto serie si hay datos y activo maniobras de los motores
290.                         Gosub recivo_comando   
291.                 Endif
292.                 If e_4 = 0 Then 'Selecciona el mecanismo a controlar
293.                         If timer2_10ms >= timer2 Then
294.                                 mecanismo = mecanismo + 1
295.                                 If mecanismo >= 4 Then mecanismo = 0
296.                                 timer2_10ms = 0
297.                         Endif
298.                 Endif
299.                 If timer3_10ms >= timer3 Then
300.                         If mecanismo = 1 Then   'Control de la Pinza
301.                                 If e_3 = 0 Then
302.                                         If servo_1 < 63 Then servo_1 = servo_1 + 1      'Pinza/Pinza, superior
303.                                 Endif
304.                                 If e_2 = 0 Then
305.                                         If servo_1 > 30 Then servo_1 = servo_1 - 1      'Pinza/Pinza, inferior
306.                                 Endif
307.                                 If e_1 = 0 Then
308.                                         If servo_2 < 75 Then servo_2 = servo_2 + 1      'Pinza/Muñeca
309.                                 Endif
310.                                 If e_0 = 0 Then
311.                                         If servo_2 > 18 Then servo_2 = servo_2 - 1
312.                                 Endif
313.                         Endif
314.                         timer3_10ms = 0
315.                 Endif
316.                 If mecanismo = 2 Then
317.                         If timer5_10ms >= timer5 Then
318.                                 If e_1 = 0 Then
319.                                         If servo_2 < 75 Then servo_2 = servo_2 + 1      'Pinza/Muñeca
320.                                 Endif
321.                                 If e_0 = 0 Then
322.                                         If servo_2 > 18 Then servo_2 = servo_2 - 1
323.                                 Endif
324.                                 timer5_10ms = 0
325.                         Endif
326.                         If timer4_10ms >= timer4 Then
327.                                 If e_3 = 0 Then
328.                                         If servo_3 < 73 Then servo_3 = servo_3 + 1      'Pinza/codo
329.                                 Endif
330.                                 If e_2 = 0 Then
331.                                         If servo_3 > 36 Then servo_3 = servo_3 - 1
332.                                 Endif
333.                                 timer4_10ms = 0
334.                         Endif
335.                 Endif  
336.                 If mecanismo = 3 Then   'Control de la Camara
337.                         If timer5_10ms >= timer5 Then
338.                                 If e_0 = 0 Then
339.                                         If servo_4 < 75 Then servo_4 = servo_4 + 1      'Camara/Muñeca
340.                                 Endif
341.                                 If e_1 = 0 Then
342.                                         If servo_4 > 19 Then servo_4 = servo_4 - 1
343.                                 Endif
344.                                 timer5_10ms = 0
345.                         Endif
346.                         If timer6_10ms >= timer6 Then
347.                                 If e_3 = 0 Then
348.                                         If servo_5 < 75 Then servo_5 = servo_5 + 1      'Camara/codo
349.                                 Endif
350.                                 If e_2 = 0 Then
351.                                         If servo_5 > 29 Then servo_5 = servo_5 - 1
352.                                 Endif
353.                                 timer6_10ms = 0
354.                         Endif
355.                 Endif                          
356.                 If mecanismo = 0 Then   'Control de los motores de avance
357.                         If e_0 = 0 Then
358.                                 Gosub fwd_md
359.                                 Gosub rev_mi
360.                                 Gosub on_md
361.                                 Gosub on_mi
362.                         Endif
363.                         If e_1 = 0 Then
364.                                 Gosub rev_md
365.                                 Gosub fwd_mi
366.                                 Gosub on_md
367.                                 Gosub on_mi
368.                         Endif
369.                         If e_2 = 0 Then
370.                                 Gosub fwd_md
371.                                 Gosub fwd_mi
372.                                 Gosub on_md
373.                                 Gosub on_mi
374.                         Endif
375.                         If e_3 = 0 Then
376.                                 Gosub rev_md
377.                                 Gosub rev_mi
378.                                 Gosub on_md
379.                                 Gosub on_mi
380.                         Endif
381.                         If e_0 = 1 And e_1 = 1 And e_2 = 1 And e_3 = 1 Then
382.                                 Gosub stop_md
383.                                 Gosub stop_mi  
384.                         Endif
385.                 Endif
386.         Endif
387. Goto main
388. End
389. '--------------------------------------FIN DEL MAIN---------------------------------------------------

[dogflu66]

El resto del programita, no coge de una sola vez... 

Código: Visual Basic

1. '--------------------------------------Sub_Rutinas Programa-----------------------------------------------
2. recivo_comando:  'leo el bufer del puerto serie
3.         Gosub readserial
4.         If assi = grupo Then
5.                 rd_grupo = assi
6.         Else
7.                 Return
8.         Endif
9.         Gosub readserial
10.         rd_ent0 = assi
11.         Gosub readserial
12.         rd_ent1 = assi
13.         Gosub readserial
14.         rd_ent2 = assi
15.         Gosub readserial
16.         rd_ent3 = assi
17.         Gosub readserial
18.         rd_ent4 = assi
19.         Gosub readserial
20.         rd_ent5 = assi
21.         Gosub readserial
22.         rd_ent6 = assi
23.         Gosub readserial
24.         rd_ent7 = assi
25.         Gosub readserial
26.         rd_ent8 = assi
27.         Gosub readserial
28.         rd_ent9 = assi
29.         Gosub readserial
30.         rd_ent10 = assi
31.         Gosub readserial
32.         rd_checsum = rd_ent0 + rd_ent1 + rd_ent2 + rd_ent3 + rd_ent4 + rd_ent5
33.         rd_checsum = rd_ent6 + rd_checsum + rd_ent7 + rd_ent8 + rd_ent9 + rd_ent10
34.         If assi = rd_checsum Then  'renuevo los valores de las entradas con los valores leidos del bufer
35.                 e_0 = rd_ent0
36.                 e_1 = rd_ent1
37.                 e_2 = rd_ent2
38.                 e_3 = rd_ent3
39.                 e_4 = rd_ent4
40.                 e_5 = rd_ent5
41.                 e_6 = rd_ent6
42.                 e_7 = rd_ent7
43.                 e_8 = rd_ent8
44.                 e_9 = rd_ent9
45.                 e_10 = rd_ent10
46.         Endif
47. Return
48. envio_comando:  'transmite la trama de datos, modo tx
49.         pause = 5       'pausa antes de enviar la trama
50.         Gosub pause_ms
51.         aux_ent0.0 = entrada.0
52.         aux_ent1.0 = entrada.1
53.         aux_ent2.0 = entrada.2
54.         aux_ent3.0 = entrada.3
55.         aux_ent4.0 = entrada.4
56.         checsum = aux_ent0 + aux_ent1 + aux_ent2 + aux_ent3 + aux_ent4 + aux_ent5
57.         checsum = checsum + aux_ent6 + aux_ent7 + aux_ent8 + aux_ent9 + aux_ent10
58.         Hserout arranque, grupo, aux_ent0, aux_ent1, aux_ent2, aux_ent3, aux_ent4, aux_ent5, aux_ent6, aux_ent7, aux_ent8, aux_ent9, aux_ent10, checsum
59.         pause = 1       'pausa despues de enviar la trama
60.         Gosub pause_ms
61. Return
62. lectura_entradas:  'lee las entradas o controles del bot, modo tx
63.         entrada.0 = RA0
64.         entrada.1 = RA1
65.         entrada.2 = RA2
66.         entrada.3 = RA3
67.         entrada.4 = RA4
68. Return
69. pause_ms:  'rutina de espera, rutina tipo Waitms, pause establese el tiempo total en mSeg
70.         aux_pause = 0  'variable auxiliar lleva la cuenta de los ms
71.         timer_1ms = 0  'al borrar el contador se activa y comienza la cuenta de 1mSeg
72.         While aux_pause <= pause  'tiempo en mSeg maximos a contar
73.                 aux_pause = aux_pause + 1  'variable auxiliar lleva la cuenta de los ms
74.                 If timer_1ms > 0 Then timer_1ms = 0  'al borrar el contador se activa y cuenta 1mSeg
75.                 While timer_1ms < 1  'espera un miliesegundo
76.                 Wend
77.         Wend
78. Return
79. pause_ms_rx:  'rutina de espera antes de leer el bufer, rutina tipo Waitms, w_posrx establese el tiempo total en mSeg
80.         aux_pause = 0  'variable auxiliar lleva la cuenta de los ms
81.         timer_1ms = 0  'al borrar el contador se activa y comienza la cuenta de 1mSeg
82.         While aux_pause <= w_posrx  'tiempo en mSeg maximos a contar
83.                 If bufer > 0 Then aux_pause = w_posrx  'si entran datos en el bufer se termina el contaje
84.                 aux_pause = aux_pause + 1  'variable auxiliar lleva la cuenta de los ms
85.                 If timer_1ms > 0 Then timer_1ms = 0  'al borrar el contador se acitva y cuenta 1mSeg
86.                 While timer_1ms < 1  'espera un miliesegundo
87.                 Wend
88.         Wend
89. Return
90. on_md:  'activa el motor derecho
91.         PORTD.7 = 1
92. Return
93. stop_md:  'para el motor derecho
94.         PORTD.7 = 0
95. Return
96. on_mi:  'activa el motor izquierdo
97.         PORTD.4 = 1
98. Return
99. stop_mi:  'para el motor izquierdo
100.         PORTD.4 = 0
101. Return
102. fwd_md:  ' sentido adelante motor derecho (no lo pone en marcha)
103.         PORTD.5 = 0
104.         PORTD.6 = 1
105. Return
106. rev_md:  ' sentido atras derecho (no activa el motor)
107.         PORTD.5 = 1
108.         PORTD.6 = 0
109. Return
110. fwd_mi:  ' sentido adelante izquierdo (no activa el motor)
111.         PORTD.2 = 0
112.         PORTD.3 = 1
113. Return
114. rev_mi:  ' sentido atras izquierdo (no activa el motor)
115.         PORTD.2 = 1
116.         PORTD.3 = 0
117. Return
118. '------------------------------ rutinas para el control del bufer del puerto serie-------------
119. readserial:
120. 'RUTINA DE LECTURA DEL BUFFER DEL PUERTO SERIE (ASSI)
121.         Gosub pause_ms_rx  'valor de entrada en mSeg de espera definido por w_posrx
122.         assi = 0  ' se borra el valor de assi, no se debe de utilizar cero como un valor de control
123.         If bufer = 1 Then  'si bufer tiene datos
124.                 assi = usart_bufer(c_serial)  'se lee el valor del bufer y se asigna assi
125.                 usart_imagen(c_serial) = 0  'se marca como leida para que pueda ser llenada de nuevo
126.                 c_serial = c_serial + 1  'se incrementa el indice del bufer
127.                 If c_serial >= n_usart Then c_serial = 0  'se verifica si se llego al final del bufer
128.                 If usart_imagen(c_serial) = 0 Then bufer = 0  'si no quedan mas datos en el bufer se marca como vacio
129.         Endif
130. Return
131. error_usart:
132.         Gosub clear_bufer
133. Return
134. clear_bufer:
135.         For n = 0 To 1  'desbloquea el puerto serie fisico en caso de bloqueo por exceso de entrada de datos
136.                 RCSTA.CREN = 0
137.                 RCSTA.CREN = 1
138.                 RCSTA.OERR = 0
139.                 Gosub clearbufer
140.                 PIR1.RCIF = 0  'borra el flag de salto de la usart
141.         Next n
142. Return
143. clearbufer:  'BORRA E INICIALIZA EL BUFFER IMAGEN DEL PUERTO serie
144.         For c_serial = 0 To n_usart
145.                 usart_imagen(c_serial) = 0
146.         Next c_serial
147.         c_serial = 0
148.         usart = 0
149.         bufer = 0
150.         assi = 0
151. Return
152. '--------------------------------------SIBRUTINAS ESPECIALES---------------------------------------------
153. On Interrupt  'Comienzan las rutinas de las interrupciones
154.         Save System  'Guarda los valores del sistema
155. '--------------------------------------RUTINA TIMER_1----------------------------------------------------
156.         If PIR1.TMR1IF = 1 Then  'comprueba que es esta la interrupcion activa
157.                 If servos_control = 0 Then      '*** rutina de control de los servos, sobre los 2ms
158.                         If servos_contador = 0 Then     'activo las salidas de control de los servos
159.                                 PORTA.0 = 1
160.                                 PORTA.1 = 1
161.                                 PORTA.2 = 1
162.                                 PORTA.3 = 1
163.                                 PORTA.5 = 1
164.                         Endif
165.                         servos_contador = servos_contador + 1  'base de tiempos para el control de los servos
166.                         If servo_1 <= servos_contador Then      'tiempo en que la salida de control del servo esta activada
167.                                 PORTA.0 = 0
168.                         Endif
169.                         If servo_2 <= servos_contador Then
170.                                 PORTA.1 = 0
171.                         Endif
172.                         If servo_3 <= servos_contador Then
173.                                 PORTA.2 = 0
174.                         Endif
175.                         If servo_4 <= servos_contador Then
176.                                 PORTA.3 = 0
177.                         Endif
178.                         If servo_5 <= servos_contador Then
179.                                 PORTA.5 = 0
180.                         Endif
181.                         If servos_contador >= 87 Then   'marca el fin de la rutina de control de servos y calibrado de esta
182.                                 servos_contador = 0
183.                                 servos_control = 1
184.                         Else    'recarga el contador del tiempo de control de la trama de los servos
185.                                 TMR1H = 0xff    '0.008ms, 125
186.                                 TMR1L = 0xd8    '0xd8
187.                                 PIR1.TMR1IF = 0  'borra el flag de salto del tmr1
188.                         Endif
189.                 Endif
190.                 If servos_control = 1 Then      'controla las bases de tiempos y el resto de tiempo de la señal de control servos
191.                         basetimer_1ms = basetimer_1ms + 1  'lleva error de +-2mSg cada 18mSg
192.                         If timer_1ms < 1 Then timer_1ms = timer_1ms + 1 'lleva error de +-2mSg cada 18mSg
193.                         If basetimer_1ms >= 10 Then
194.                                 If timer1_10ms < timer1 Then timer1_10ms = timer1_10ms + 1                     
195.                                 If timer2_10ms < timer2 Then timer2_10ms = timer2_10ms + 1
196.                                 If timer3_10ms < timer3 Then timer3_10ms = timer3_10ms + 1
197.                                 If timer4_10ms < timer4 Then timer4_10ms = timer4_10ms + 1                     
198.                                 If timer5_10ms < timer5 Then timer5_10ms = timer5_10ms + 1
199.                                 If timer6_10ms < timer6 Then timer6_10ms = timer6_10ms + 1
200.                                 basetimer_1ms = 0
201.                         Endif
202.                         servos_aux_contador = servos_aux_contador + 1   'control del resto del tiempo de la señal de los servos a nivel bajo
203.                         If servos_aux_contador >= 17 Then
204.                                 servos_control = 0
205.                                 servos_aux_contador = 0
206.                         Endif
207.                         TMR1H = 0xec    '1ms
208.                         TMR1L = 0x78
209.                         PIR1.TMR1IF = 0  'borra el flag de salto del tmr1
210.                 Endif
211.         Endif
212. '--------------------------------------BUFFER PUERTO SERIE-----------------------------------------------
213.         If servos_contador = 0 Then     'impide que se ejecute durante la generacion de la señal de control servos +-2mSg
214.                 If PIR1.RCIF = 1 Then  'comprueba que es esta la interrupcion activa
215.                         If usart < n_usart Then  'comprueba que el bufer de entrada no esta lleno
216.                                 If usart_imagen(usart) = 0 Then  'comprueba que la posicion esta vacia
217.                                         aux_usart = usart  'retiene la posicion antigua del bufer
218.                                         usart = usart + 1  'indica cual sera la posicion siguiente del bufer
219.                                         usart_imagen(aux_usart) = 1  'indica que hay dato en el bufer de entrada
220.                                         bufer = 1  'indica que el bufer tine datos
221.                                         Hserget usart_bufer(aux_usart)  ' carga el dato en el bufer
222.                                 Else
223.                                         Hserget usart_bufer(n_usart)  'descarga la usart para que no se bloquee porque el bufer de entrada esta lleno
224.                                 Endif
225.                         Else
226.                                 If usart_imagen(0) = 0 Then  'comprueba que esta vacia la primera posicion del bufer
227.                                         usart_imagen(0) = 1  'marca que hay dato en esa posicion del bufer
228.                                         bufer = 1  'marca que el bufer tiene datos
229.                                         usart = 1  'establece la proxima posicion del bufer
230.                                         Hserget usart_bufer(0)  'lee la usart y guarda el dato en el bufer
231.                                 Else
232.                                         Hserget usart_bufer(n_usart)  'descarga la usart porque el bufer esta lleno
233.                                 Endif
234.                         Endif
235.                         PIR1.RCIF = 0  'borra el flag de salto de la usart
236.                 Endif
237.         Endif
238. Resume
239. End