'||===============================================================================||
'||* Nombre Rutina: ESC_FINAL.BAS *||
'||* Entradas : PORTA.2 SEÑAL DEL RX. *||
'||* *||
'||* Salidas : PORTB.0 LED ENCENDIDO *||
'||* PORTB.2 MARCA AVANCE *||
'||* PORTB.3 PULSO PWM *||
'||* PORTB.5 MARCA RETROCESO *||
'||* *||
'||* Objetivo : controlar las señales enviadas por el RX e interpretarlas *||
'||* como aceleracion según sea el grado de inclinacion de la *||
'||* palanca del canal usado. *||
'||* *||
'||* Autor : Hugo Sagardía Aguilera. *||
'||* Proyecto : ACELERADOR ELECTRONICO *||
'||* *||
'||* Micro : 16F628A *||
'||* Oscilador : IntRC IO *||
'||* Velocidad : 4Mhz *||
'||* Flags : WDT OFF *||
'||* PWRT OFF *||
'||* MCLR OFF *||
'||* BODEN OFF *||
'||* LVP OFF *||
'||* CPD OFF *||
'||* CP OFF *||
'||* *||
'||* ----------- ------- ------------------------------------------------------- *||
'||* Fecha Versión Descripción Cambio *||
'||* ----------- ------- ------------------------------------------------------- *||
'||* 11/Ago/2009 1.0.0 (Version Original) *||
'||* *||
'||* 24/Ago/2009 1.0.1 (1.-se cambio BODEN a OFF para evitar reinicios. *||
'||* 2.-se ignoran pulsos invalidos o fuera del intervalo *||
'||* [97 - 201]. *||
'||* 3.-se aumentaron de 8 a 40 los pulsos PWM. *||
'||* 4.-se agregaron constantes. *||
'||* 5.-se ignoran pulsos invalidos al determinar el centro.*||
'||* 6.-no se realiza calculo de intervalos, se determina *||
'||* con valores escalares el pwm. *||
'||* 7.-en el 1er ciclo, del bucle principal no se *||
'||* ejecuta PWMduty para evitar accionar el motor al *||
'||* partir el circuito. *||
'||* 8.-si el numero de pulsos invalidos en el ciclo *||
'||* principal supera los 25 se anula el pwm actual y *||
'||* quedan desctivados los pines de avance y retroceso *||
'||* 25 ciclos de 20ms osea 20ms*25=500ms=0.5s, *||
'||* si hay mas de 0.5s sin señal se detiene todo. *||
'||* *||
'||* 25/Ago/2009 1.0.2 (1.-se volvio el centro del servo a un intervalo, por la*||
'||* irregularidad que presenta al estar en el centro la *||
'||* palanca de mandos. *||
'||* *||
'||* 29/Oct/2009 1.0.3 (1.-control del maximo) *||
'||===============================================================================||
AllDigital
Config PORTA = Input
Config PORTB = Output
Config PORTB.6 = Input
'-------------------------------------------------------------------
'Define SIMULATION_WAITMS_VALUE = 1 'ignora los Waitms
'Define SEROUT_DELAYUS = 0
'-------------------------------------------------------------------
'CONSTANTES USADAS EN EL PROGRAMA
Const k_cero = 0
Const k_uno = 1
Const k_dos = 2
Const k_tres = 3
Const k_cuatro = 4
Const k_cinco = 5
Const k_seis = 6
Const k_siete = 7
Const k_ocho = 8
Const k_nueve = 9
Const k_diez = 10
Const k_once = 11
Const k_doce = 12
Const k_trece = 13
Const k_catorce = 14
Const k_quince = 15
Const k_dieciseis = 16
Const k_diecisiete = 17
Const k_dieciocho = 18
Const k_diecinueve = 19
Const k_veinte = 20
Const k_veintiuno = 21
Const k_veintidos = 22
Const k_veintitres = 23
Const k_veinticuatro = 24
Const k_veinticinco = 25
Const k_veintiseis = 26
Const k_veintisiete = 27
Const k_veintiocho = 28
Const k_veintinueve = 29
Const k_treinta = 30
Const k_treintiuno = 31
Const k_treintidos = 32
Const k_treintitres = 33
Const k_treinticuatro = 34
Const k_treinticinco = 35
Const k_treintiseis = 36
Const k_treintisiete = 37
Const k_treintiocho = 38
Const k_treintinueve = 39
Const k_cuarenta = 40
Const k_cincuenta = 50
Const k_cien = 100
Const k_pulso_min = 96
Const k_pulso_max = 202
'VARIABLES USADAS EN EL CICLO PRINCIPAL
Dim pulso_pwm(40) As Byte
Dim duty As Byte '0..255
Dim pos As Byte '0..255
Dim largo_pulso As Byte '0..255
'VARIABLES USADAS PARA DETERMINAR LA POSICION CENTRAL
Dim cuenta_pulsos(10) As Byte
Dim muestra_pulsos(10) As Byte
Dim centro_servo As Byte
Dim i As Byte
Dim j As Byte
Dim inicio As Byte
Dim muestra_estable As Byte
Dim indice_estable As Byte
Dim pulso_tmp As Byte
'VARIABLES USADAS PARA CALCULAR EL INTERVALO CENTRAL
Dim centro_min As Byte
Dim centro_max As Byte
'PARA EVITAR ACCIONAR EL MOTOR AL ENCENDER EL CIRCUITO
Dim inicio_flag As Byte
inicio_flag = 1
'PARA EL GLITCH
Dim pulso_min As Byte
Dim pulso_max As Byte
'AQUI INICIALIZA EL ARREGLO CON LOS PULSOS PWM
pulso_pwm(0) = 177
pulso_pwm(1) = 179
pulso_pwm(2) = 181
pulso_pwm(3) = 183
pulso_pwm(4) = 185
pulso_pwm(5) = 187
pulso_pwm(6) = 189
pulso_pwm(7) = 191
pulso_pwm(8) = 193
pulso_pwm(9) = 195
pulso_pwm(10) = 197
pulso_pwm(11) = 199
pulso_pwm(12) = 201
pulso_pwm(13) = 203
pulso_pwm(14) = 205
pulso_pwm(15) = 207
pulso_pwm(16) = 209
pulso_pwm(17) = 211
pulso_pwm(18) = 213
pulso_pwm(19) = 215
pulso_pwm(20) = 217
pulso_pwm(21) = 219
pulso_pwm(22) = 221
pulso_pwm(23) = 223
pulso_pwm(24) = 225
pulso_pwm(25) = 227
pulso_pwm(26) = 229
pulso_pwm(27) = 231
pulso_pwm(28) = 233
pulso_pwm(29) = 235
pulso_pwm(30) = 237
pulso_pwm(31) = 239
pulso_pwm(32) = 241
pulso_pwm(33) = 243
pulso_pwm(34) = 245
pulso_pwm(35) = 247
pulso_pwm(36) = 249
pulso_pwm(37) = 251
pulso_pwm(38) = 253
pulso_pwm(39) = 255
'DEFINE LOS NOMBRES DE LOS PINES POR FUNCION
Symbol pin_led_verde = PORTB.0
Symbol pin_rx = PORTA.2
Symbol pin_avance = PORTB.2
Symbol pin_retroceso = PORTB.5
Symbol pin_pwm = PORTB.3
'-----------------------------------------------------------------------
'----------------------------- P A S O 1 -----------------------------
'-----------------------------------------------------------------------
'AQUI TOMA MUESTRAS DE LOS PULSOS QUE ESTAN LLEGANDO
'IGNORA LOS PULSOS INVALIDOS
i = k_cero
'pulsos_de_muestra:
While i < k_diez
ServoIn pin_rx, pulso_tmp
If pulso_tmp > k_pulso_min And pulso_tmp < k_pulso_max Then
muestra_pulsos(i) = pulso_tmp
i = i + k_uno
Endif
Wend
'AQUI CUENTO LAS VECES QUE APARECE UN MISMO PULSO DENTRO DEL MUESTREO
For j = k_cero To k_nueve
inicio = j + k_uno
cuenta_pulsos(j) = k_uno
For i = inicio To k_nueve
If muestra_pulsos(j) = muestra_pulsos(i) Then
cuenta_pulsos(j) = cuenta_pulsos(j) + k_uno
Endif
Next i
Next j
'AQUI VE CUAL ES LA MUESTRA QUE MAS SE REPITE
muestra_estable = k_cero
For i = k_cero To k_nueve
If cuenta_pulsos(i) > muestra_estable Then
muestra_estable = cuenta_pulsos(i)
indice_estable = i
Endif
Next i
'AQUI OBTIENE EL VALOR QUE CORRESPONDE A LA POSICION CENTRAL DEL CENTRO DEL STICK
centro_servo = muestra_pulsos(indice_estable)
'PARA ESTE CENTRO DE SERVO DETERMINA EL PULSO MAXIMO Y EL PULSO MINIMO VALIDO
pulso_min = (centro_servo - k_treintisiete) - k_uno
pulso_max = (centro_servo + k_cuarenta) + k_uno
'POSICION CENTRAL DEL INTERVALO CERRADO [ X ]
centro_min = centro_servo - k_uno
centro_max = centro_servo + k_uno
'TODO OK ENCIENDE LED VERDE Y APAGA EL DE FALLO
pin_led_verde = k_uno
'-----------------------------------------------------------------------
'----------------------------- P A S O 2 -----------------------------
'-----------------------------------------------------------------------
'AQUI ESTA EL CICLO PRINCIPAL EN DONDE SE CALCULA LA ACELERACION
largo_pulso = k_cero
PWMon k_uno, k_nueve
'AQUI CALCULA LOS INTERVALOS USADOS PARA DETERMINAR LAQ ACELERACION
''|------R E T R O C E S O------| |---------A V A N C E---------|
'(00](01](..](..](..](..](38](39][ X ][41)[42)[..)[..)[..)[..)[79)[80)
''| | | | | | | | | | | | | | | |
''| | | | | | | ----------- | | | | | | |
''| | | | | | ------------------- | | | | | |
''| | | | | --------------------------- | | | | |
''| | | | ----------------------------------- | | | |
''| | | ------------------------------------------- | | |
''| | --------------------------------------------------- | |
''| ----------------------------------------------------------- |
''-------------------------------------------------------------------
'-----------------------------------------------------------------------
loop:
largo_pulso = k_cero
ServoIn pin_rx, largo_pulso
'IGNORA LOS PULSOS INVALIDOS
If largo_pulso > pulso_min And largo_pulso < pulso_max Then
'SI RECIBIO UN PULSO VALIDO REINICIA EL CONTADOR DE GLITCH
pin_led_verde = k_uno
'SI EL PULSO ESTA EN EL CENTRO DETIENE TODO
If largo_pulso >= centro_min And largo_pulso <= centro_max Then
duty = k_cero
pin_avance = k_cero
pin_retroceso = k_cero
Else
If largo_pulso < centro_servo Then
'SI ES RETROCESO
pos = (centro_servo - largo_pulso) - k_uno
pin_avance = k_cero
WaitUs k_cinco
pin_retroceso = k_uno
Else
'SI ES AVANCE
pos = (largo_pulso - centro_servo) - k_uno
pin_retroceso = k_cero
WaitUs k_cinco
pin_avance = k_uno
Endif
'SI PASA DEL PWM MAXIMO SE ACOTA A ESTE SOLAMENTE
If pos > k_treintinueve Then
pos = k_treintinueve
Endif
duty = pulso_pwm(pos)
Endif
Endif
'PARA EVITAR MOVER EL MOTOR AL ACCIONAR EL SWITCH
If inicio_flag = k_cero Then
PWMduty k_uno, duty
Else
inicio_flag = k_cero
Endif
Goto loop
End