This document is posted to help you gain knowledge. Please leave a comment to let me know what you think about it! Share it to your friends and learn new things together.
LISTADO DEL PROGRAMA DE LA INTERFICIE: Formulario Principal: Private Sub Pantallaprincipal_Load() ‘Inicializa todas las variables del programa Memorizarrutina.RutinaSumarRestar(0).Value = 0 ‘Variables de los text box de los Memorizarrutina.RutinaSumarRestar(1).Value = 0 ‘grados donde debe ir cada servo. Memorizarrutina.RutinaSumarRestar(2).Value = 0 Memorizarrutina.RutinaGrados(0).Text = 0 Memorizarrutina.RutinaGrados(1).Text = 0 Memorizarrutina.RutinaGrados(2).Text = 0 Grados_Base = 0 ‘Variables de los grados que tiene cada servo. Grados_Hombro = 0 Grados_Codo = 0 Grados_Base_Anterior = 255 Grados_Hombro_Anterior = 255 Grados_Codo_Anterior = 255 Numero_Puerto = 1 ‘Inicializa el puerto serie de transmisión, como COM1. Call Memorizarrutina.enviar End Sub Private Sub Menumemorizacionderutina_Click() ‘Muestra el formulario de rutinas, si ha sido Memorizarrutina.Show vbModal, Me ‘seleccionado con el menu del formulario End Sub ‘principal. Private Sub Menuopcionesdecomunicacion_Click() ‘Muestra el formulario para cambiar el OpcionesComunicacion.Show vbModal, Me ‘el puerto de comunicación. End Sub Private Sub Menusalir_Click() ‘Para finalizar el programa, se selecciona desde el menu End ‘del formulario principal. End Sub Formulario opciones de comunicación: Private Sub OpcionesAceptar_Click(Index As Integer) ´Acepta el COM seleccionado como Unload Me ‘valido. End Sub Private Sub OpcionPuerto1_Click() ‘Selecciona el COM1 como activo. Numero_Puerto = 1 End Sub Private Sub OpcionPuerto2_Click() ‘Selecciona el COM2 como activo. Numero_Puerto = 2 End Sub
BRAZO DE ROBOT ANEXO 2. BUS I2C
- 71 -
Formulario para grabar, borrar y resetear el brazo de robot: Private Sub Borrar_Click() ‘Rutina para borrar la memoria I2C. Para ello se manda como Tarea_Realizar = "3" ‘Tarea a realizar el número 3. Mov_Text1.Text = 0 Mov_Text2.Text = 42 Grados_Base_Anterior = 255 Grados_Hombro_Anterior = 255 Grados_Codo_Anterior = 255 Call enviar End Sub Private Sub RutinaGrabar_Click() ‘Esta rutina sirve para grabar una secuencia de movimientos ‘en la memoria I2C. If (Grados_Base <> Grados_Base_Anterior Or Grados_Hombro <> Grados_Hombro_Anterior Or Grados_Codo <> Grados_Codo_Anterior) And (Mov_Text1.Text < 42) Then Mov_Text1 = Mov_Text1 + 1 Mov_Text2 = Mov_Text2 - 1 Grados_Base_Anterior = Grados_Base Grados_Hombro_Anterior = Grados_Hombro Grados_Codo_Anterior = Grados_Codo Tarea_Realizar = "1" ‘Para dar la orden de grabación de la posición actual del brazo. Call enviar End If End Sub If (RutinaGrados(0).Text >= 0 And RutinaGrados(0).Text <= 180) Or (RutinaGrados(1).Text >= 0 And RutinaGrados(1) <= 180) Or (RutinaGrados(2).Text >= 0 And RutinaGrados(2).Text <= 180) Then Grados_Base = RutinaGrados(0).Text RutinaSumarRestar(0).Value = RutinaGrados(0).Text ‘En esta rutina se actualiza la Grados_Hombro = RutinaGrados(1).Text ‘variable que contiene el valor RutinaSumarRestar(1).Value = RutinaGrados(1).Text ‘de los grados de cada servo a Grados_Codo = RutinaGrados(2).Text ‘partir del valor del campo de RutinaSumarRestar(2).Value = RutinaGrados(2).Text ‘texto de cada servo. End If End Sub Private Sub RutinaReset_Click() ‘Rutina para resetear el brazo de robot, es decir, RutinaGrados(0).Text = 0 ‘lleva todos los servos a la posición 0º, borra memoria RutinaGrados(1).Text = 0 ‘e inicializa todas las variables del interfície. RutinaGrados(2).Text = 0 RutinaSumarRestar(0).Value = 0 ‘Texto que indica la posición de cada servo. RutinaSumarRestar(1).Value = 0 RutinaSumarRestar(2).Value = 0 Grados_Base = 0 ‘Variable que guarda la posición de cada servo. Grados_Hombro = 0 Grados_Codo = 0 Grados_Base_Anterior = 255 ‘Posición anterior, cuando se inicializa se pone a Grados_Hombro_Anterior = 255 ‘valor 255. Grados_Codo_Anterior = 255 Mov_Text1.Text = 0 ‘Movimientos actuales grabados en memoria I2C. Mov_Text2.Text = 42 ‘Movimientos que faltan para llenar banco de memoria I2C. Tarea_Realizar = "2" ‘Indica al Brazo de robot que resetee la memoria y los servos. Call enviar
BRAZO DE ROBOT ANEXO 2. BUS I2C
- 72 -
End Sub Private Sub RutinaSalir_Click() ‘Vuelta al formulario principal. Unload Me End Sub Private Sub RutinaSumarRestar_Change(Index As Integer) If RutinaSumarRestar(0).Value <> Grados_Base Then ‘Texto de la posición del servo1, Grados_Base = RutinaSumarRestar(0).Value ‘Base. RutinaGrados(0).Text = RutinaSumarRestar(0).Value End If If RutinaSumarRestar(1).Value <> Grados_Hombro Then ‘Texto de la posición del servo Grados_Hombro = RutinaSumarRestar(1).Value ‘del Hombro. RutinaGrados(1).Text = RutinaSumarRestar(1).Value End If If RutinaSumarRestar(2).Value <> Grados_Codo Then ‘Texto de la posición del servo del Grados_Codo = RutinaSumarRestar(2).Value ‘Codo. RutinaGrados(2).Text = RutinaSumarRestar(2).Value End If Tarea_Realizar = "0" 'Aplicar a los servos sin grabar en EEPROM Call enviar End Sub Sub enviar() Inicio_Transmision_Centena = "2" ‘Bytes de sincronización. Inicio_Transmision_Decena = "5" Inicio_Transmision_Unidad = "5" If Grados_Base >= 0 And Grados_Base <= 180 Then ‘Se prepara la centena, decena y unidad If Grados_Base >= 0 And Grados_Base < 10 Then ‘Base < 10 Centena(0) = "0" Decena(0) = "0" Unidad(0) = Grados_Base End If If Grados_Base >= 10 And Grados_Base < 100 Then ‘Base <100 Centena(0) = "0" '_Base = "0" Decena(0) = Left(Grados_Base, 1) Unidad(0) = Right(Grados_Base, 1) End If If Grados_Base >= 100 Then ‘Base > 100 Centena(0) = Left(Grados_Base, 1) Decena(0) = Mid(Grados_Base, 2, 1) Unidad(0) = Right(Grados_Base, 1) End If End If If Grados_Hombro >= 0 And Grados_Hombro <= 180 Then If Grados_Hombro >= 0 And Grados_Hombro < 10 Then ‘Hombro <10 Centena(1) = "0" Decena(1) = "0" Unidad(1) = Grados_Hombro End If If Grados_Hombro >= 10 And Grados_Hombro < 100 Then ‘Hombro <100 Centena(1) = "0" Decena(1) = Left(Grados_Hombro, 1)
BRAZO DE ROBOT ANEXO 2. BUS I2C
- 73 -
Unidad(1) = Right(Grados_Hombro, 1) End If If Grados_Hombro >= 100 Then ‘Hombro > 99 Centena(1) = Left(Grados_Hombro, 1) Decena(1) = Mid(Grados_Hombro, 2, 1) Unidad(1) = Right(Grados_Hombro, 1) End If End If If Grados_Codo >= 0 And Grados_Codo <= 180 Then ‘Codo < 10 If Grados_Codo >= 0 And Grados_Codo < 10 Then Centena(2) = "0" Decena(2) = "0" Unidad(2) = Grados_Codo End If If Grados_Codo >= 10 And Grados_Codo < 100 Then ‘Codo < 100 Centena(2) = "0" Decena(2) = Left(Grados_Codo, 1) Unidad(2) = Right(Grados_Codo, 1) End If If Grados_Codo >= 100 Then ‘Codo > 99 Centena(2) = Left(Grados_Codo, 1) Decena(2) = Mid(Grados_Codo, 2, 1) Unidad(2) = Right(Grados_Codo, 1) End If End If 'Rutina de ordenacion del valor de grado de cada servo, según la diferencia entre el valor ‘siguiente y el anterior, para así saber el mayor desplazamiento y calcular el número de ‘pasos. If Grados_Base_Anter > Grados_Base Then Grados_Base_Resta = Grados_Base_Anter - Grados_Base End If If Grados_Base_Anter < Grados_Base Then Grados_Base_Resta = Grados_Base - Grados_Base_Anter End If Grados_Base_Anter = Grados_Base If Grados_Hombro_Anter > Grados_Hombro Then Grados_Hombro_Resta = Grados_Hombro_Anter - Grados_Hombro End If If Grados_Hombro_Anter < Grados_Hombro Then Grados_Hombro_Resta = Grados_Hombro - Grados_Hombro_Anter End If Grados_Hombro_Anter = Grados_Hombro If Grados_Codo_Anter > Grados_Codo Then Grados_Codo_Resta = Grados_Codo_Anter - Grados_Codo End If If Grados_Codo_Anter < Grados_Codo Then Grados_Codo_Resta = Grados_Codo - Grados_Codo_Anter End If Grados_Codo_Anter = Grados_Codo
BRAZO DE ROBOT ANEXO 2. BUS I2C
- 74 -
Ordenados(0) = Grados_Base_Resta Ordenados(1) = Grados_Hombro_Resta Ordenados(2) = Grados_Codo_Resta For J = 0 To 2 ‘Ordenación por el método de la burbuja, el mayor queda For I = 0 To 1 ‘al final del vector. If Ordenados(I) > Ordenados(I + 1) Then aux = Ordenados(I) Ordenados(I) = Ordenados(I + 1) Ordenados(I + 1) = aux End If Next I Next J 'calculo del número de paso de cada servo dividiendo el mayor desplazamiento entre los ‘los otros. 'Evita la division por cero Grados_Base_Paso = Grados_Base_Resta Grados_Hombro_Paso = Grados_Hombro_Resta Grados_Codo_Paso = Grados_Codo_Resta If Grados_Base_Paso = 0 Then Grados_Base_Paso = Grados_Base_Paso + 1 End If If Grados_Hombro_Paso = 0 Then Grados_Hombro_Paso = Grados_Hombro_Paso + 1 End If If Grados_Codo_Paso = 0 Then Grados_Codo_Paso = Grados_Codo_Paso + 1 End If 'Cuidado que Str mete un signo a la izda, caracter blanco si es positivo '- si es negativo. Pasos_Servo(0) = Str(Int(Ordenados(2) / Grados_Base_Paso)) ‘Cálculo de pasos. Pasos_Servo(1) = Str(Int(Ordenados(2) / Grados_Hombro_Paso)) Pasos_Servo(2) = Str(Int(Ordenados(2) / Grados_Codo_Paso)) 'Aqui elimino el signo de la izquierda que se coloca con Str() If Pasos_Servo(0) >= 0 And Pasos_Servo(0) <= 180 Then If Pasos_Servo(0) >= 0 And Pasos_Servo(0) < 10 Then Pasos_Servo_Centena(0) = "0" Pasos_Servo_Decena(0) = "0" Pasos_Servo_Unidad(0) = Mid(Pasos_Servo(0), 2, 1) End If If Pasos_Servo(0) >= 10 And Pasos_Servo(0) < 100 Then Pasos_Servo_Centena(0) = "0" Pasos_Servo_Decena(0) = Mid(Pasos_Servo(0), 2, 1) Pasos_Servo_Unidad(0) = Mid(Pasos_Servo(0), 3, 1) End If If Pasos_Servo(0) >= 100 Then Pasos_Servo_Centena(0) = Mid(Pasos_Servo(0), 2, 1) Pasos_Servo_Decena(0) = Mid(Pasos_Servo(0), 3, 1) Pasos_Servo_Unidad(0) = Mid(Pasos_Servo(0), 4, 1) End If End If
BRAZO DE ROBOT ANEXO 2. BUS I2C
- 75 -
If Pasos_Servo(1) >= 0 And Pasos_Servo(1) <= 180 Then If Pasos_Servo(1) >= 0 And Pasos_Servo(1) < 10 Then Pasos_Servo_Centena(1) = "0" Pasos_Servo_Decena(1) = "0" Pasos_Servo_Unidad(1) = Mid(Pasos_Servo(1), 2, 1) End If If Pasos_Servo(1) >= 10 And Pasos_Servo(1) < 100 Then Pasos_Servo_Centena(1) = "0" Pasos_Servo_Decena(1) = Mid(Pasos_Servo(1), 2, 1) Pasos_Servo_Unidad(1) = Mid(Pasos_Servo(1), 3, 1) End If If Pasos_Servo(1) >= 100 Then Pasos_Servo_Centena(1) = Mid(Pasos_Servo(1), 2, 1) Pasos_Servo_Decena(1) = Mid(Pasos_Servo(1), 3, 1) Pasos_Servo_Unidad(1) = Mid(Pasos_Servo(1), 4, 1) End If End If If Pasos_Servo(2) >= 0 And Pasos_Servo(2) <= 180 Then If Pasos_Servo(2) >= 0 And Pasos_Servo(2) < 10 Then Pasos_Servo_Centena(2) = "0" Pasos_Servo_Decena(2) = "0" Pasos_Servo_Unidad(2) = Mid(Pasos_Servo(2), 2, 1) End If If Pasos_Servo(2) >= 10 And Pasos_Servo(2) < 100 Then Pasos_Servo_Centena(2) = "0" Pasos_Servo_Decena(2) = Mid(Pasos_Servo(2), 2, 1) Pasos_Servo_Unidad(2) = Mid(Pasos_Servo(2), 3, 1) End If If Pasos_Servo(2) >= 100 Then Pasos_Servo_Centena(2) = Mid(Pasos_Servo(2), 2, 1) Pasos_Servo_Decena(2) = Mid(Pasos_Servo(2), 3, 1) Pasos_Servo_Unidad(2) = Mid(Pasos_Servo(2), 4, 1) End If End If ‘Se manda la trama de comunicación por el puerto serie RS-232. CommSerie.CommPort = Numero_Puerto CommSerie.Settings = "9600,N,8,1" CommSerie.PortOpen = True CommSerie.Output = Inicio_Transmision_Centena CommSerie.Output = Inicio_Transmision_Decena CommSerie.Output = Inicio_Transmision_Unidad CommSerie.Output = Tarea_Realizar For x = 0 To 2 '3 CommSerie.Output = Pasos_Servo_Centena(x) CommSerie.Output = Pasos_Servo_Decena(x) CommSerie.Output = Pasos_Servo_Unidad(x) CommSerie.Output = Centena(x) CommSerie.Output = Decena(x) CommSerie.Output = Unidad(x) Next x
BRAZO DE ROBOT ANEXO 2. BUS I2C
- 76 -
CommSerie.PortOpen = False Tarea_Realizar = "0" End Sub
BRAZO DE ROBOT ANEXO 2. BUS I2C
- 77 -
CEREBRO DEL BRAZO DE ROBOT: PROGRAMA PRINCIPAL DE LA APLICACIÓN:
Organigrama 7.19. Rutina principal del brazo de robot.
Programa principal
¿Modo Autonómo?
¿Recibido por el puerto serie el valor
"255"?
Recibir trama por el puerto serie: Tarea a
Realizar yPosición de los servos
¿Tarea a Realizar = 0?
Actuar sobre servos
¿Tarea a Realizar = 1?Grabar memoria
¿Tarea a Realizar = 2?
Posicionar servos en posicion 0º y borrar memoria
Borrar memoria¿Tarea a
Realizar = 3?
Leer memoriaActuar sobre servos
¿Todos los servos en posición?
N S
Modo AutonómoModo Programación
N S
N SS
N
S
S
S
N
N
N
BRAZO DE ROBOT ANEXO 2. BUS I2C
- 78 -
RUTINA PARA GRABAR EN LA MEMORIA I2C
Organigramas 7.20. Rutina grabar memoria.
Rutina Grabar Memoria
¿Memoria llena?
Fin Rutina Grabar
Grabar: Posición servo1 y nº de pasos 1Posición servo2 y nº de pasos 2Posición servo3 y nº de pasos 3
Numero Total Movimientos+1y grabar en memoria
S
N
BRAZO DE ROBOT ANEXO 2. BUS I2C
- 79 -
RUTINA LEER MEMORIA:
Organigrama 7.21. Rutina leer de EEPROM.
Rutina Leer Memoria
¿Número de movimiento actual > número total de
movimientos?
Fin Rutina Leer
Leer en EEPROM: Posición servo1 y nº de pasos1 Posición servo2 y nº de pasos2 Posición servo3 y nº de pasos3
Incrementar número de movimiento actual
¿Número de movimiento actual = número total de
movimientos?
Inicializar: Número de movimiento actual
S
N
S
N
BRAZO DE ROBOT ANEXO 2. BUS I2C
- 80 -
RUTINA SERVICIO DE INTERRUPCION (PWM):
Organigrama 7.22. Atención de interrupción, genera PWM para los servos.
RSI
¿Interrupcion por TMR1?
¿Variable = 2? Subir pin servo 1Cargar TMR1
¿Interrupcion por TMR0?
¿Variable = 2?Bajar pin servo 1
¿Variable = 1?Subir pin servo 2
Cargar TMR1
Subir pin servo 3Cargar TMR1
¿Variable = 1?Bajar pin servo 2
Bajar pin servo 3Variable = 3
Desactivar TMR1Variable = Variable - 1
Fin RSI
S N
N
S
S
S
N
NN
N
S
S
BRAZO DE ROBOT ANEXO 2. BUS I2C
- 81 -
LISTADO DEL PROGRAMA GRABADO EN EL PIC 16F873 ;Programa para realizar las siguientes operaciones pertenecientes al brazo de robot con 3 ;servos, estas acciones son: Controlar servos, borrar, grabar y leer la EEPROM I2C. ;Los 3 servos incrementan y decrementan grados según una tabla, de aproximadamente 1 ;grado. El duty-cycle sera una interrupcion provocada por TMR1. El resto del periodo será una ;interrupcion provocada por el TMR0. El TMR0 provoca interrupcion cada 5 milisegundos, por lo ; que se necesitara una variable contador que cada 4 veces que se produzca una TMR0 ;interrupción actuará sobre cada servo, con intervalo de 5 mS. Dejando así tiempo de sobra ;para para que no se afecte el PWM de los demás servos. ;Además se le pasaran las posiciones a la que debe ir cada servo desde el PC, por el puerto ;serie, desde el programa hecho en VB para tal tarea. LIST P=16F873 ;Tipo de procesador. INCLUDE "P16F873.INC" ;Variables N_SERVO EQU 20h ;Nº de servo para actuar el PWM, en cada INT. Leido EQU 21h ;Leido de RCREG, después de restar 30h ASCI I. GRADOS_SERVO1 EQU 22h ;Grado al que debe ir el servo 1. GRADOS_SERVO2 EQU 23h ;Grado al que debe ir el servo 2. GRADOS_SERVO3 EQU 24h ;Grado al que debe ir el servo 3. GRADO_LECTURA EQU 26h ;Grado para leer tabla. Tarea_Realizar EQU 27h ;Tarea a realizar. W_TEMP EQU 30h ;Para guardar el Counter program y registro W. STATUS_TEMP EQU 31h ;al ejecutar el servicio de interrupción. PCLATH_TEMP EQU 32h Unidad EQU 33h ;Unidad leída de RS-232. Decena EQU 34h ;Decena leída de RS-232. Centena EQU 35h ;Centena leída de RS-232. Cifra_Leida EQU 36h ;Obtiene la cifra real. Pasos_Servo1 EQU 38h ;Nº de pasos de espera para servo 1. Pasos_Servo2 EQU 39h ;Idem para servo 2 y 3. Pasos_Servo3 EQU 3Ah GR_DEST_SERVO1 EQU 42h ;Variables para saber el grado destino al que GR_DEST_SERVO2 EQU 43h ;debe ir cada servo. GR_DEST_SERVO3 EQU 44h Pas_act_Servo1 EQU 46h ;Paso actual en el que se encuentra cada Pas_act_Servo2 EQU 47h ;servo para repetir cada grado. Pas_act_Servo3 EQU 48h Pas_act_Servo4 EQU 49h GRADOS_SERVO EQU 4Bh Pasos_Servo EQU 4Ch GR_DEST_SERVO EQU 4Dh Pas_act_Servo EQU 4Eh Mira_si_Paso EQU 4Fh ;Mira si se puede sumar pasos o grados, para ;sincronizar con los servos. Retardo_1 EQU 50h ;Necesario en el Delay de 4,7mS
BRAZO DE ROBOT ANEXO 2. BUS I2C
- 82 -
Retardo_2 EQU 51h ;el I2C y la EEPROM 24LC02B WORD_ADDRESS EQU 52h ;Palabra de direccion para el I2C. Total_Mov EQU 53h ;Total movimientos de la secuencia. N_Mov_Act EQU 54h ;El numero por el que va en la secuencia de lectura. ;********** Programa. ORG 00h ;Vector de reset GOTO Inicio ORG 04h ;Vector de interrupcion GOTO RSI ;Configuración de registros página 1 de memoria. Inicio BCF STATUS,RP1 ;PAGINA 1 BSF STATUS,RP0 MOVLW b'00000111' ;TMR0 con preescaler 1:256 MOVWF OPTION_REG MOVLW 0x06 ;PORTA como salidas/entradas digitales MOVWF ADCON1 MOVLW b'00110000' ;RA0,1,2,3 salida, el resto son entradas MOVWF TRISA MOVLW b'00000000' ;PORTB como salidas digitales MOVWF TRISB MOVLW b'10111111' ;RC7/Rx entrada, RC6/Tx salida MOVWF TRISC ;RC3/SCL entrada, RC4/SDA entrada MOVLW .15 ;9600 baudios con FREC=9,830400 MOVWF SPBRG ;segun la formula MOVLW b'10000000' ;Velocidad estandard con niveles I2C MOVWF SSPSTAT MOVLW .24 ;Velocidad del bus aprox 100 khz MOVWF SSPADD ;Cristal de 9,8340 Mhz MOVLW b'11100000' ;Interrupciones generales, perifericas y MOVWF INTCON ;de TMR0 por overflow MOVLW b'00000001' ;Habilita INT por TMR1 overflow MOVWF PIE1 ;Configuración de registros página 0 de memoria. BCF STATUS,RP0 ;Pagina 0 MOVLW b'00101000' ;Modulo MSSP en ON MOVWF SSPCON MOVLW .207 ;TMR0 provocara INT cada 5 mS MOVWF TMR0
CLRF PORTA ;Borrar la puerta A,B y C CLRF PORTB CLRF PORTC MOVLW b'10010000' ;Configura recepcion asincrona MOVWF RCSTA ;de 8 bits sin paridad MOVLW b'00000000' ;Resetea FLAG de int por tmr1 overflow y el MOVWF PIR1 ;que indica que el buffer de recepcion esta lleno MOVLW b'00110000' ;Desactiva TMR1 y preescaler 1:8 MOVWF T1CON MOVLW 02h ;Inicializa la variable N_SERVO MOVWF N_SERVO ;para actuar sobre los 3 servos MOVLW .1 ;Si se puede incrementar/decrementar grado MOVWF Mira_si_Paso movlw 0x00 ;Inicializa la Tarea a Realizar movwf Tarea_Realizar
BRAZO DE ROBOT ANEXO 2. BUS I2C
- 83 -
;Condiciones iniciales del brazo de robot Condi_Inicial MOVLW .0 ;Servos a posición de 0º MOVWF GRADOS_SERVO1 MOVWF GR_DEST_SERVO1 MOVLW .0 MOVWF GRADOS_SERVO2 MOVWF GR_DEST_SERVO2 MOVLW .0 MOVWF GRADOS_SERVO3 MOVWF GR_DEST_SERVO3 MOVLW .0 ;Empezará la secuencia desde el
MOVWF N_Mov_Act ;principio de la memoria. MOVWF WORD_ADDRESS
;****Rutina Principal Principal BTFSC PORTC,0 ;Si es 0 entra en el modo programacion GOTO Modo_Autonomo ;Aqui en realidad tiene que ir Modo Autonomo GOTO Modo_Programacion ;Interruptor externo. Fin_Principal GOTO Principal Modo_Autonomo BCF STATUS,Z MOVLW .1 ;Comprueba si puede seguir mirando pasos o ;incrementando/decrementando grados SUBWF Mira_si_Paso,w ;segun la condicion de sincronizacion BTFSC STATUS,Z GOTO Modo_Aut_1 BSF PCLATH,0 ;Cambio de banco de memoria BSF PCLATH,1 CALL Modo_Autonomo_1 Modo_Aut_1 BSF PCLATH,0 BSF PCLATH,2 CALL Fin_Modo_Auton Fin_Modo_Auton_1 GOTO Modo_Autonomo Modo_Programacion ;En este bucle leeremos continuamente si llega algo por el puerto ;serie, en este orden: ;1º,2º y 3º byte, de start, si es 255 se sigue la lectura sino a esperar que ;llegue. ;4º , tarea a realizar, 0 es mover servo ;5º, 6º y 7º byte, numero de pasos que debe esperar el Servo1 ;8º, 9º y 10º numero de grado al que debe ir el servo1 (0...180º) ;11º, 12º y 13º byte, numero de pasos que debe esperar el Servo2 ;14º, 15º y 16º numero de grado al que debe ir el servo2 (0...180º) ;17º, 18º y 19º byte, numero de pasos que debe esperar el Servo3 ;20º, 21º y 22º numero de grado al que debe ir el servo3 (0...180º) ;Se leen 3 bytes y a continuacion cuando se comprueba si es el byte de start 255, ;si es ese byte se sigue con la lectura, si no se vuelve a esperar que llegue. BCF STATUS,Z CALL Obtiene_Cifra_3bytes MOVLW .255
BRAZO DE ROBOT ANEXO 2. BUS I2C
- 84 -
SUBWF Cifra_Leida,w BTFSC STATUS,Z GOTO Leer_Tarea GOTO Principal Leer_Tarea CALL Obtiene_Cifra_1byte MOVF Cifra_Leida,w MOVWF Tarea_Realizar Leer_Pasos_Servo1 CALL Obtiene_Cifra_3bytes ;3 bytes, centena, decena y unidad MOVF Cifra_Leida,w MOVWF Pasos_Servo1 ;Se guarda en la variable Leer_Grados_Servo1 CALL Obtiene_Cifra_3bytes ;Centena, decena y unidad MOVF Cifra_Leida,w MOVWF GRADOS_SERVO1 Leer_Pasos_Servo2 CALL Obtiene_Cifra_3bytes ;3bytes MOVF Cifra_Leida,w MOVWF Pasos_Servo2 Leer_Grados_Servo2 CALL Obtiene_Cifra_3bytes MOVF Cifra_Leida,w MOVWF GRADOS_SERVO2 Leer_Pasos_Servo3 CALL Obtiene_Cifra_3bytes ;3byte MOVF Cifra_Leida,w MOVWF Pasos_Servo3 Leer_Grados_Servo3 CALL Obtiene_Cifra_3bytes MOVF Cifra_Leida,w MOVWF GRADOS_SERVO3 Comprobar_Tarea MOVF Tarea_Realizar,w ADDWF PCL,f ;Se comprueba si la tarea a realizar es: GOTO Principal ;0 -> Al Servo GOTO Grabar_Posicion ;1 -> Grabar Coordenada en la EEPROM GOTO Reset ;2 -> Inicializa puntero memoria, borrandola, ;y posicionando el servo a 0º GOTO Borrar_Memoria ;3 -> Borra Memoria Obtiene_Cifra_1byte CALL Leer_Rx ;Obtiene la cifra leída de Tarea a Realizar MOVWF Unidad BSF PCLATH,1 MOVF Unidad,w CALL TABLA_UNIDADES MOVWF Cifra_Leida BCF PCLATH,1 RETURN Obtiene_Cifra_3bytes CALL Leer_Rx ;Obtiene la cifra leída MOVWF Centena CALL Leer_Rx MOVWF Decena CALL Leer_Rx MOVWF Unidad BSF PCLATH,1 MOVF Centena,w CALL TABLA_CENTENAS MOVWF Cifra_Leida MOVF Decena,w CALL TABLA_DECENAS ADDWF Cifra_Leida,f MOVF Unidad,w CALL TABLA_UNIDADES
BRAZO DE ROBOT ANEXO 2. BUS I2C
- 85 -
ADDWF Cifra_Leida,f BCF PCLATH,1 RETURN Leer_Rx BTFSS PIR1,5 ;Lee el buffer de entrada, RS-232 GOTO Leer_Rx BCF PIR1,5 MOVF RCREG,w MOVWF Leido MOVLW 30h SUBWF Leido,f MOVF Leido,w RETURN Grabar_Posicion BSF PCLATH,2 ;Grabar los pasos y grados en CALL Grabar_Memoria ;la memoria GOTO Principal Reset ;Lleva todo el sistema a condiciones iniciales, los servos y el puntero de ;memoria tambien, borrandola de esta manera MOVLW .0 MOVWF GR_DEST_SERVO1 MOVWF GR_DEST_SERVO2 MOVWF GR_DEST_SERVO3 MOVWF GRADOS_SERVO1 MOVWF GRADOS_SERVO2 MOVWF GRADOS_SERVO3 BSF PCLATH,2 CALL Reset_Total_Mov GOTO Principal Borrar_Memoria BSF PCLATH,2 CALL Reset_Total_Mov GOTO Principal RSI ;Rutina de servicio de interrupcion, genera PWM movwf W_TEMP ;Guarda Contador de programa swapf STATUS,w ;acumulador y registro de banderas CLRF STATUS movwf STATUS_TEMP movf PCLATH,w movwf PCLATH_TEMP clrf PCLATH MIRAR_INT BTFSS INTCON,2 GOTO TMR1_INT ;Se ha producido INT por overflow de TMR1 GOTO TMR0_INT ;Se ha producido INT por overflow de TMR0 TMR1_INT BTFSS PIR1,0 ;INT por TMR1 ? GOTO FIN_RSI ;Falsa INT ;TMR1_int, bajar PWM MOVLW .207 ;TMR0 cada 5 mS MOVWF TMR0
MOVF N_SERVO,W ADDWF PCL,F
BRAZO DE ROBOT ANEXO 2. BUS I2C
- 86 -
GOTO BAJAR_SER3 GOTO BAJAR_SER2 GOTO BAJAR_SER1
GOTO FIN_RSI ;Si hay alguna cosa incontrolada BAJAR_SER1 BCF PORTA,0 ;Nivel bajo del PWM de cada servo GOTO TMR1_INT_1 ;segun corresponda BAJAR_SER2 BCF PORTA,1 GOTO TMR1_INT_1 BAJAR_SER3 BCF PORTA,2 MOVLW 03h ;Inicializa N_SERVO MOVWF N_SERVO TMR1_INT_1 DECF N_SERVO,F
BCF T1CON,0 ;Desactiva TMR1 BCF PIR1,0 ;Habilita INT por TMR1 overflow GOTO FIN_RSI TMR0_INT movlw .0 ;Para sincronizar para que todos los servos movwf Mira_si_Paso ;lleguen juntos a la posicion destino MOVF N_SERVO,W ADDWF PCL,F GOTO SUBIR_SER3 ;pwm a nivel alto GOTO SUBIR_SER2 GOTO SUBIR_SER1 SUBIR_SER1 BSF PORTA,0 MOVF GRADOS_SERVO1,W GOTO CARGAR_TMR1 SUBIR_SER2 BSF PORTA,1 MOVF GRADOS_SERVO2,W GOTO CARGAR_TMR1 SUBIR_SER3 BSF PORTA,2 MOVF GRADOS_SERVO3,W CARGAR_TMR1 MOVWF GRADO_LECTURA BSF PCLATH,0 CALL DUTY_LOW MOVWF TMR1L
MOVF GRADO_LECTURA,W BCF PCLATH,0 BSF PCLATH,1 CALL DUTY_HIGH MOVWF TMR1H BCF PCLATH,1 FIN_TMR1 BSF T1CON,0 ;Activar TMR1 BCF INTCON,2 ;Restablece FLAG de TMR0 FIN_RSI movf PCLATH_TEMP,w movwf PCLATH swapf STATUS_TEMP,w movwf STATUS swapf W_TEMP,F swapf W_TEMP,w RETFIE ;Retorno de interrupcion
RETLW low .64907 ;175º RETLW low .64905 ;176º RETLW low .64903 ;177º RETLW low .64901 ;178º RETLW low .64899 ;179º RETLW low .64897 ;180º ORG 200h DUTY_HIGH ADDWF PCL,F ;Carga en TMR1 la parte alta, "HIGH" del duty ;del PWM correspondiente. RETLW high .65259 ;0º RETLW high .65257 ;1º RETLW high .65255 ;2º RETLW high .65253 ;3º RETLW high .65251 ;4º RETLW high .65249 ;5º RETLW high .65247 ;6º RETLW high .65245 ;7º RETLW high .65243 ;8º RETLW high .65241 ;9º RETLW high .65239 ;10º RETLW high .65237 ;11º RETLW high .65235 ;12º RETLW high .65233 ;13º RETLW high .65231 ;14º RETLW high .65229 ;15º RETLW high .65227 ;16º RETLW high .65225 ;17º RETLW high .65223 ;18º RETLW high .65221 ;19º RETLW high .65219 ;20º RETLW high .65217 ;21º RETLW high .65215 ;22º RETLW high .65213 ;23º RETLW high .65211 ;24º RETLW high .65209 ;25º RETLW high .65207 ;26º RETLW high .65205 ;27º RETLW high .65203 ;28º RETLW high .65201 ;29º RETLW high .65199 ;30º RETLW high .65197 ;31º RETLW high .65195 ;32º RETLW high .65193 ;33º RETLW high .65191 ;34º RETLW high .65189 ;35º RETLW high .65187 ;36º RETLW high .65185 ;37º RETLW high .65183 ;38º RETLW high .65181 ;39º RETLW high .65179 ;40º RETLW high .65177 ;41º RETLW high .65175 ;42º RETLW high .65173 ;43º RETLW high .65171 ;44º RETLW high .65169 ;45º RETLW high .65167 ;46º RETLW high .65165 ;47º
BRAZO DE ROBOT ANEXO 2. BUS I2C
- 91 -
RETLW high .65163 ;48º RETLW high .65161 ;49º RETLW high .65159 ;50º RETLW high .65157 ;51º RETLW high .65155 ;52º RETLW high .65153 ;53º RETLW high .65151 ;54º RETLW high .65149 ;55º RETLW high .65147 ;56º RETLW high .65145 ;57º RETLW high .65143 ;58º RETLW high .65141 ;59º RETLW high .65139 ;60º RETLW high .65137 ;61º RETLW high .65135 ;62º RETLW high .65133 ;63º RETLW high .65131 ;64º RETLW high .65129 ;65º RETLW high .65127 ;66º RETLW high .65125 ;67º RETLW high .65121 ;68º RETLW high .65119 ;69º RETLW high .65117 ;70º RETLW high .65115 ;71º RETLW high .65113 ;72º RETLW high .65111 ;73º RETLW high .65109 ;74º RETLW high .65107 ;75º RETLW high .65105 ;76º RETLW high .65103 ;77º RETLW high .65101 ;78º RETLW high .65099 ;79º RETLW high .65097 ;80º RETLW high .65095 ;81º RETLW high .65093 ;82º RETLW high .65091 ;83º RETLW high .65089 ;84º RETLW high .65087 ;85º RETLW high .65085 ;86º RETLW high .65083 ;87º RETLW high .65081 ;88º RETLW high .65079 ;89º RETLW high .65077 ;90º RETLW high .65075 ;91º RETLW high .65073 ;92º RETLW high .65071 ;93º RETLW high .65069 ;94º RETLW high .65067 ;95º RETLW high .65065 ;96º RETLW high .65063 ;97º RETLW high .65061 ;98º RETLW high .65059 ;99º RETLW high .65057 ;100º RETLW high .65055 ;101º RETLW high .65053 ;102º RETLW high .65051 ;103º RETLW high .65049 ;104º RETLW high .65047 ;105º RETLW high .65045 ;106º RETLW high .65043 ;107º
BRAZO DE ROBOT ANEXO 2. BUS I2C
- 92 -
RETLW high .65041 ;108º RETLW high .65039 ;109º RETLW high .65037 ;110º RETLW high .65035 ;111º RETLW high .65033 ;112º RETLW high .65031 ;113º RETLW high .65029 ;114º RETLW high .65027 ;115º RETLW high .65025 ;116º RETLW high .65023 ;117º RETLW high .65021 ;118º RETLW high .65019 ;119º RETLW high .65017 ;120º RETLW high .65015 ;121º RETLW high .65013 ;122º RETLW high .65011 ;123º RETLW high .65009 ;124º RETLW high .65007 ;125º RETLW high .65005 ;126º RETLW high .65003 ;127º RETLW high .65001 ;128º RETLW high .64999 ;129º RETLW high .64997 ;130º RETLW high .64995 ;131º RETLW high .64993 ;132º RETLW high .64991 ;133º RETLW high .64989 ;134º RETLW high .64987 ;135º RETLW high .64985 ;136º RETLW high .64983 ;137º RETLW high .64981 ;138º RETLW high .64979 ;139º RETLW high .64977 ;140º RETLW high .64975 ;141º RETLW high .64973 ;142º RETLW high .64971 ;143º RETLW high .64969 ;144º RETLW high .64967 ;145º RETLW high .64965 ;146º RETLW high .64963 ;147º RETLW high .64961 ;148 RETLW high .64959 ;149º RETLW high .64957 ;150º RETLW high .64955 ;151º RETLW high .64953 ;152º RETLW high .64951 ;153º RETLW high .64949 ;154º RETLW high .64947 ;155º RETLW high .64945 ;156º RETLW high .64943 ;157º RETLW high .64941 ;158º RETLW high .64939 ;159º RETLW high .64937 ;160º RETLW high .64935 ;161º RETLW high .64933 ;162º RETLW high .64931 ;163º RETLW high .64929 ;164º RETLW high .64927 ;165º RETLW high .64925 ;166º RETLW high .64923 ;167º
BRAZO DE ROBOT ANEXO 2. BUS I2C
- 93 -
RETLW high .64921 ;168º RETLW high .64919 ;169º RETLW high .64917 ;170º RETLW high .64915 ;171º RETLW high .64913 ;172º RETLW high .64911 ;173º RETLW high .64909 ;174º RETLW high .64907 ;175º RETLW high .64905 ;176º RETLW high .64903 ;177º RETLW high .64901 ;178º RETLW high .64899 ;179º RETLW high .64897 ;180º TABLA_CENTENAS ADDWF PCL,F RETLW 0x00 ;Devuelve 0 RETLW .100 ;Devuelve el 100 decimal RETLW .200 ;Devuelve el 200 decimal TABLA_DECENAS ADDWF PCL,F RETLW .0 ;Devuelve el 0 RETLW .10 ;Devuelve el 10 RETLW .20 ;Devuelve el 20 RETLW .30 ;Devuelve el 30 RETLW .40 ;Devuelve el 40 RETLW .50 ;Devuelve el 50 RETLW .60 ;Devuelve el 60 RETLW .70 ;Devuelve el 70 RETLW .80 ;Devuelve el 80 RETLW .90 ;Devuelve el 90 TABLA_UNIDADES ADDWF PCL,F RETLW .0 ;Devuelve el 0
RETLW .1 ;Devuelve el 1 RETLW .2 ;Devuelve el 2 RETLW .3 ;Devuelve el 3 RETLW .4 ;Devuelve el 4 RETLW .5 ;Devuelve el 5 RETLW .6 ;Devuelve el 6 RETLW .7 ;Devuelve el 7 RETLW .8 ;Devuelve el 8 RETLW .9 ;Devuelve el 9 ORG 300h Modo_Autonomo_1 BCF STATUS,C ;Aquí se va incrementando o decrementando MOVF N_SERVO,w ;los grados, teniendo en cuenta el nº de pasos ADDWF PCL,f ;que el servo tiene que esperar cada vez. GOTO Calculo_Servo3 GOTO Calculo_Servo2 GOTO Calculo_Servo1
MOVF GR_DEST_SERVO,w ;GRADOS_SERVO<GRADOS_DESTINO SUBWF GRADOS_SERVO,w BTFSS STATUS,C GOTO I NCREMENTAR GOTO Fin_Modo_Autonomo ;Caso que sean iguales INCREMENTAR BCF STATUS,Z MOVF Pasos_Servo,w ;Comprueba si el numero de pasos SUBWF Pas_act_Servo,w ;que ha dado es el que hay que dar BTFSC STATUS,Z ;antes de incrementar un grado GOTO INCREMENTAR_GRADO INCF Pas_act_Servo,f GOTO Guardar_Servo INCREMENTAR_GRADO
DECREMENTAR BCF STATUS,Z MOVF Pasos_Servo,w ;Comprueba si el numero de pasos SUBWF Pas_act_Servo,w ;que ha dado es el que hay que dar BTFSC STATUS,Z ;antes de decrementar un grado GOTO DECREMENTAR_GRADO INCF Pas_act_Servo,f GOTO Guardar_Servo DECREMENTAR_GRADO
DECF GRADOS_SERVO,f MOVLW .1 MOVWF Pas_act_Servo GOTO Guardar_Servo Guardar_Servo MOVF N_SERVO,w ADDWF PCL,f GOTO Guardar_Servo3 GOTO Guardar_Servo2 GOTO Guardar_Servo1 Guardar_Servo1 MOVF GRADOS_SERVO,w MOVWF GRADOS_SERVO1 MOVF Pas_act_Servo,w MOVWF Pas_act_Servo1 GOTO Fin_Modo_Autonomo Guardar_Servo2 MOVF GRADOS_SERVO,w MOVWF GRADOS_SERVO2 MOVF Pas_act_Servo,w MOVWF Pas_act_Servo2 GOTO Fin_Modo_Autonomo Guardar_Servo3 MOVF GRADOS_SERVO,w MOVWF GRADOS_SERVO3 MOVF Pas_act_Servo,w MOVWF Pas_act_Servo3 Fin_Modo_Autonomo movlw .1 ;No realiza ningun incremento/decremento hasta movwf Mira_si_Paso ;nueva interrupción BCF PCLATH,0 BCF PCLATH,1 RETURN ORG 0x400 ;Tratamiento de memoria EEPROM Grabar_Memoria ;Random Read para leer el numero de movimientos totales, antes de ;grabar y la palabra de dirección actual CALL Delay_I2C ;Lee los movimientos totales, si son menores CALL START ;de 42 graba MOVLW b'10100000' CALL I2C_ENVIAR ;Envia el Control Byte como escritura ;(datagrama) MOVLW .252
BRAZO DE ROBOT ANEXO 2. BUS I2C
- 96 -
CALL I2C_ENVIAR ;Envia la Word Address del total de ;movimientos CALL RESTART MOVLW b'10100001' CALL I2C_ENVIAR ;Manda el nuevo Control Byte como lectura ;(datagrama) CALL I2C_LEER ;Lee el total de movimientos MOVWF Total_Mov CALL I2C_MAN_NO_ACK CALL STOP ;Lee como CURRENT ADDRESS READ la Word Address ;Empieza la grabacion de los diferentes valores de Servo1,2,3 BCF STATUS,Z ;Comprueba si hay 42 movimientos grabados MOVLW .42 SUBWF Total_Mov,w BTFSC STATUS,Z GOTO Fin_Grabar Grabar_Mem ;Graba como 'PAGE WRITE' los 6 valores correspondientes a los 3 Servos
;Graba el numero de movimientos como 'BYTE WRITE' INCF Total_Mov,f CALL Delay_I2C
BSF PCLATH,0 CALL START_GRABACION MOVLW .252 CALL I2C_ENVIAR ;Manda la WORD ADDRESS de la posicion de ;grabación MOVF Total_Mov,w CALL I2C_ENVIAR ;Manda la el Total de movimientos de la ;secuencia actual del movimiento CALL STOP ;'PAGE WRITE' CALL Delay_I2C