Proyecto de Tesis en Mecatrónica. Universidad Técnica del Norte. Ernesto Palacios <mecatronica.mid@gmail.com>
Dependencies: EthernetNetIf HTTPServer QEI_hw RPCInterface mbed
setup.cpp@35:92b0f1b75a51, 2014-07-07 (annotated)
- Committer:
- Yo_Robot
- Date:
- Mon Jul 07 15:41:20 2014 +0000
- Revision:
- 35:92b0f1b75a51
- Parent:
- 34:bdf918bc9b59
Intento de que el mbed devuelva los pulsos... no funciona, mejor implementar la misma funcionalidad en LabVIEW, pero aun se debe poder detener la generaci?n de pulsos.
Who changed what in which revision?
User | Revision | Line number | New contents of line |
---|---|---|---|
Yo_Robot | 4:552beeda4722 | 1 | /** |
Yo_Robot | 4:552beeda4722 | 2 | * @file setup.cpp |
Yo_Robot | 4:552beeda4722 | 3 | * @author Ernesto Palacios |
Yo_Robot | 20:4b154134ab20 | 4 | * @brief Codigo Fuente de las funciones para el deslizador. |
Yo_Robot | 4:552beeda4722 | 5 | * |
Yo_Robot | 4:552beeda4722 | 6 | * Created on 25 de Marzo de 2012 |
Yo_Robot | 4:552beeda4722 | 7 | * |
Yo_Robot | 4:552beeda4722 | 8 | * Licencia GPL v3.0 |
Yo_Robot | 4:552beeda4722 | 9 | * http://www.gnu.org/licenses/gpl-3.0.html |
Yo_Robot | 4:552beeda4722 | 10 | */ |
Yo_Robot | 4:552beeda4722 | 11 | |
Yo_Robot | 25:1910a55ff0a3 | 12 | #include "mbed.h" |
Yo_Robot | 25:1910a55ff0a3 | 13 | #include "qeihw.h" |
Yo_Robot | 3:8d5a9e3cd680 | 14 | #include "setup.h" |
Yo_Robot | 18:cf1e07d82630 | 15 | #include "EthernetNetIf.h" |
Yo_Robot | 25:1910a55ff0a3 | 16 | |
Yo_Robot | 3:8d5a9e3cd680 | 17 | |
Yo_Robot | 24:a1d16835201c | 18 | extern Serial pc; // Salida Serial de mbed |
Yo_Robot | 24:a1d16835201c | 19 | extern Serial RS_232; // Salida Serial a MAX232 |
Yo_Robot | 25:1910a55ff0a3 | 20 | extern QEIHW encoder; |
Yo_Robot | 35:92b0f1b75a51 | 21 | extern Timer cronometro; // Cronometro interno del microcontrolador |
Yo_Robot | 35:92b0f1b75a51 | 22 | extern Timeout temporizador; // temporizador para |
Yo_Robot | 24:a1d16835201c | 23 | extern DigitalIn isPC; // Bit de configuracion serial en la placa |
Yo_Robot | 12:c02b08dacc45 | 24 | extern DigitalOut pin_son; // SON |
Yo_Robot | 12:c02b08dacc45 | 25 | extern DigitalOut pin_dir; // SIGN+ |
Yo_Robot | 12:c02b08dacc45 | 26 | extern InterruptIn pin_alm; // ALM |
Yo_Robot | 12:c02b08dacc45 | 27 | extern AnalogOut aout; // +-10V |
Yo_Robot | 22:d5431fff164b | 28 | extern DigitalOut led_verde; // Led verde del conector Ethernet |
Yo_Robot | 24:a1d16835201c | 29 | extern DigitalOut led_rojo; // Led naranja del conector Ethernet |
Yo_Robot | 30:413d1a6648b5 | 30 | extern InterruptIn limite_1; |
Yo_Robot | 30:413d1a6648b5 | 31 | extern InterruptIn limite_2; |
Yo_Robot | 30:413d1a6648b5 | 32 | extern InterruptIn limite_3; |
Yo_Robot | 30:413d1a6648b5 | 33 | extern InterruptIn limite_4; |
Yo_Robot | 20:4b154134ab20 | 34 | |
Yo_Robot | 35:92b0f1b75a51 | 35 | extern int bandera_pulsos; //bandera del contador de PULSOS GENERADOS |
Yo_Robot | 35:92b0f1b75a51 | 36 | extern int num_pulsos; // Numero de pulsos generados |
Yo_Robot | 35:92b0f1b75a51 | 37 | |
Yo_Robot | 35:92b0f1b75a51 | 38 | |
Yo_Robot | 15:a1ffa32ce9d1 | 39 | int fq_posicion = 10000; // Variable global donde se almacenara |
Yo_Robot | 15:a1ffa32ce9d1 | 40 | // la velocidad de posicionamiento en Hz |
Yo_Robot | 24:a1d16835201c | 41 | float t_alto; // para el posicionamiento del motor |
Yo_Robot | 23:2126e38bb48c | 42 | |
Yo_Robot | 26:dad0b2031173 | 43 | int fq_actual = 0; // Ultimo valor seteado para el tren de pulsos |
Yo_Robot | 22:d5431fff164b | 44 | |
Yo_Robot | 35:92b0f1b75a51 | 45 | int bandera_inicio = 0; //bandera de ir a inicio HOME |
Yo_Robot | 35:92b0f1b75a51 | 46 | |
Yo_Robot | 33:e6ff02c3e0f5 | 47 | |
Yo_Robot | 35:92b0f1b75a51 | 48 | |
Yo_Robot | 35:92b0f1b75a51 | 49 | void posicion_ok() |
Yo_Robot | 35:92b0f1b75a51 | 50 | { |
Yo_Robot | 35:92b0f1b75a51 | 51 | cronometro.stop(); |
Yo_Robot | 35:92b0f1b75a51 | 52 | stopTimer2(); //Detener tren de pulsos |
Yo_Robot | 35:92b0f1b75a51 | 53 | float tiempo_pulsos_generados = cronometro.read_ms(); |
Yo_Robot | 35:92b0f1b75a51 | 54 | int num_pulsos = (int) (fq_posicion * tiempo_pulsos_generados) / 1000; |
Yo_Robot | 35:92b0f1b75a51 | 55 | |
Yo_Robot | 35:92b0f1b75a51 | 56 | bandera_pulsos = 1; // En la funcion main() se imprimiran los resultados; |
Yo_Robot | 35:92b0f1b75a51 | 57 | |
Yo_Robot | 35:92b0f1b75a51 | 58 | } |
Yo_Robot | 35:92b0f1b75a51 | 59 | |
Yo_Robot | 29:52932326c45a | 60 | int velocidad_rpm() |
Yo_Robot | 22:d5431fff164b | 61 | { |
Yo_Robot | 33:e6ff02c3e0f5 | 62 | return encoder.GetVelocityCap(); |
Yo_Robot | 22:d5431fff164b | 63 | } |
Yo_Robot | 22:d5431fff164b | 64 | |
Yo_Robot | 22:d5431fff164b | 65 | void clear_encoder() |
Yo_Robot | 22:d5431fff164b | 66 | { |
Yo_Robot | 25:1910a55ff0a3 | 67 | encoder.Reset( QEI_RESET_POS ); // reset position |
Yo_Robot | 22:d5431fff164b | 68 | } |
Yo_Robot | 22:d5431fff164b | 69 | |
Yo_Robot | 4:552beeda4722 | 70 | void setTimer2() |
Yo_Robot | 3:8d5a9e3cd680 | 71 | { |
Yo_Robot | 3:8d5a9e3cd680 | 72 | // Encender Timer2 (PCONP[22]) |
Yo_Robot | 3:8d5a9e3cd680 | 73 | LPC_SC->PCONP |= 1 << 22; |
Yo_Robot | 3:8d5a9e3cd680 | 74 | |
Yo_Robot | 3:8d5a9e3cd680 | 75 | // Resetear y parar el Timer |
Yo_Robot | 3:8d5a9e3cd680 | 76 | LPC_TIM2->TCR = 0x2; |
Yo_Robot | 3:8d5a9e3cd680 | 77 | LPC_TIM2->CTCR = 0x0; |
Yo_Robot | 3:8d5a9e3cd680 | 78 | |
Yo_Robot | 4:552beeda4722 | 79 | // Establecer el Preescaler en cero |
Yo_Robot | 4:552beeda4722 | 80 | // SIn Preesclaer |
Yo_Robot | 4:552beeda4722 | 81 | LPC_TIM2->PR = 0; |
Yo_Robot | 3:8d5a9e3cd680 | 82 | |
Yo_Robot | 4:552beeda4722 | 83 | // Calcular el periodo Inicial |
Yo_Robot | 4:552beeda4722 | 84 | uint32_t periodo = ( SystemCoreClock / 400 ); |
Yo_Robot | 3:8d5a9e3cd680 | 85 | |
Yo_Robot | 3:8d5a9e3cd680 | 86 | // Establecer los Registros de Coincidencia |
Yo_Robot | 3:8d5a9e3cd680 | 87 | // ( Match Register ) |
Yo_Robot | 3:8d5a9e3cd680 | 88 | LPC_TIM2->MR2 = periodo; |
Yo_Robot | 4:552beeda4722 | 89 | LPC_TIM2->MR3 = periodo; // Legacy, salidas identicas |
Yo_Robot | 3:8d5a9e3cd680 | 90 | |
Yo_Robot | 4:552beeda4722 | 91 | LPC_TIM2->MCR |= 1 << 7; // Resetear Timer en MR2 |
Yo_Robot | 3:8d5a9e3cd680 | 92 | |
Yo_Robot | 3:8d5a9e3cd680 | 93 | LPC_TIM2->EMR |= 15UL << 8; // Alternar Pin MAT2.2 |
Yo_Robot | 3:8d5a9e3cd680 | 94 | // y MAT2.3 |
Yo_Robot | 3:8d5a9e3cd680 | 95 | |
Yo_Robot | 3:8d5a9e3cd680 | 96 | LPC_PINCON->PINSEL0 |= 15UL << 16; //Activar MAT2.2 |
Yo_Robot | 8:958dfe5052b9 | 97 | // y MAT2.3 como salidas |
Yo_Robot | 8:958dfe5052b9 | 98 | |
Yo_Robot | 6:b4dae934e1ea | 99 | } |
Yo_Robot | 6:b4dae934e1ea | 100 | |
Yo_Robot | 12:c02b08dacc45 | 101 | void ISR_Serial() |
Yo_Robot | 12:c02b08dacc45 | 102 | { |
Yo_Robot | 12:c02b08dacc45 | 103 | int value; // Nuevo Valor |
Yo_Robot | 12:c02b08dacc45 | 104 | char command; // Comando al que aplicar el nuevo valor |
Yo_Robot | 23:2126e38bb48c | 105 | |
Yo_Robot | 12:c02b08dacc45 | 106 | |
Yo_Robot | 19:c26cf8a48986 | 107 | if( isPC ) |
Yo_Robot | 23:2126e38bb48c | 108 | pc.scanf( "%d-%c", &value, &command ); |
Yo_Robot | 19:c26cf8a48986 | 109 | else |
Yo_Robot | 23:2126e38bb48c | 110 | RS_232.scanf( "%d-%c", &value, &command ); |
Yo_Robot | 23:2126e38bb48c | 111 | |
Yo_Robot | 23:2126e38bb48c | 112 | switch( command ) |
Yo_Robot | 23:2126e38bb48c | 113 | { |
Yo_Robot | 19:c26cf8a48986 | 114 | |
Yo_Robot | 29:52932326c45a | 115 | case 'E': // Leer el contador del encoder. |
Yo_Robot | 24:a1d16835201c | 116 | { |
Yo_Robot | 23:2126e38bb48c | 117 | |
Yo_Robot | 27:b8254b76ec57 | 118 | //Leer la posición del encoder |
Yo_Robot | 23:2126e38bb48c | 119 | |
Yo_Robot | 26:dad0b2031173 | 120 | if( isPC ) |
Yo_Robot | 29:52932326c45a | 121 | pc.printf("%d\n\r",encoder.GetPosition()); |
Yo_Robot | 26:dad0b2031173 | 122 | else |
Yo_Robot | 29:52932326c45a | 123 | RS_232.printf("%d\n\r",encoder.GetPosition()); |
Yo_Robot | 23:2126e38bb48c | 124 | break; |
Yo_Robot | 24:a1d16835201c | 125 | } |
Yo_Robot | 27:b8254b76ec57 | 126 | |
Yo_Robot | 23:2126e38bb48c | 127 | case 'H': // Establecer nueva frecuencia |
Yo_Robot | 24:a1d16835201c | 128 | { |
Yo_Robot | 23:2126e38bb48c | 129 | setPTO( value ); |
Yo_Robot | 26:dad0b2031173 | 130 | fq_actual = value; |
Yo_Robot | 32:4483d6c225e5 | 131 | bandera_inicio = 0; //En caso que haya estado yendo a HOME, cancelar |
Yo_Robot | 26:dad0b2031173 | 132 | |
Yo_Robot | 26:dad0b2031173 | 133 | // Envia un OK de comando recibido |
Yo_Robot | 26:dad0b2031173 | 134 | if( isPC ) |
Yo_Robot | 26:dad0b2031173 | 135 | pc.printf("OK\r\n"); |
Yo_Robot | 26:dad0b2031173 | 136 | else |
Yo_Robot | 26:dad0b2031173 | 137 | RS_232.printf("OK\r\n"); |
Yo_Robot | 23:2126e38bb48c | 138 | break; |
Yo_Robot | 24:a1d16835201c | 139 | } |
Yo_Robot | 27:b8254b76ec57 | 140 | |
Yo_Robot | 23:2126e38bb48c | 141 | case 'K': |
Yo_Robot | 24:a1d16835201c | 142 | { |
Yo_Robot | 23:2126e38bb48c | 143 | setPTO( value * 1000 ); |
Yo_Robot | 26:dad0b2031173 | 144 | fq_actual = value; |
Yo_Robot | 32:4483d6c225e5 | 145 | bandera_inicio = 0; //En caso que haya estado yendo a HOME, cancelar |
Yo_Robot | 32:4483d6c225e5 | 146 | |
Yo_Robot | 26:dad0b2031173 | 147 | // Envia un OK de comando recibido |
Yo_Robot | 26:dad0b2031173 | 148 | if( isPC ) |
Yo_Robot | 26:dad0b2031173 | 149 | pc.printf("OK\r\n"); |
Yo_Robot | 26:dad0b2031173 | 150 | else |
Yo_Robot | 26:dad0b2031173 | 151 | RS_232.printf("OK\r\n"); |
Yo_Robot | 26:dad0b2031173 | 152 | |
Yo_Robot | 23:2126e38bb48c | 153 | break; |
Yo_Robot | 24:a1d16835201c | 154 | } |
Yo_Robot | 23:2126e38bb48c | 155 | case 'A': // Cambiar voltaje de salida |
Yo_Robot | 24:a1d16835201c | 156 | { |
Yo_Robot | 23:2126e38bb48c | 157 | aout = (float)( value + 10000.0 ) / 20000.0; |
Yo_Robot | 26:dad0b2031173 | 158 | |
Yo_Robot | 26:dad0b2031173 | 159 | // Envia un OK de comando recibido |
Yo_Robot | 26:dad0b2031173 | 160 | if( isPC ) |
Yo_Robot | 26:dad0b2031173 | 161 | pc.printf("OK\r\n"); |
Yo_Robot | 26:dad0b2031173 | 162 | else |
Yo_Robot | 26:dad0b2031173 | 163 | RS_232.printf("OK\r\n"); |
Yo_Robot | 26:dad0b2031173 | 164 | |
Yo_Robot | 23:2126e38bb48c | 165 | break; |
Yo_Robot | 24:a1d16835201c | 166 | } |
Yo_Robot | 23:2126e38bb48c | 167 | case 'D': // Cambiar la direccion |
Yo_Robot | 24:a1d16835201c | 168 | { |
Yo_Robot | 26:dad0b2031173 | 169 | |
Yo_Robot | 23:2126e38bb48c | 170 | stopTimer2(); |
Yo_Robot | 23:2126e38bb48c | 171 | pin_dir = value; |
Yo_Robot | 23:2126e38bb48c | 172 | wait_us( 2 ); |
Yo_Robot | 32:4483d6c225e5 | 173 | bandera_inicio = 0; //En caso que haya estado yendo a HOME, cancelar |
Yo_Robot | 26:dad0b2031173 | 174 | |
Yo_Robot | 26:dad0b2031173 | 175 | if ( fq_actual != 0 ) |
Yo_Robot | 26:dad0b2031173 | 176 | { |
Yo_Robot | 26:dad0b2031173 | 177 | startTimer2(); |
Yo_Robot | 26:dad0b2031173 | 178 | } |
Yo_Robot | 26:dad0b2031173 | 179 | |
Yo_Robot | 26:dad0b2031173 | 180 | // Envia un OK de comando recibido |
Yo_Robot | 26:dad0b2031173 | 181 | if( isPC ) |
Yo_Robot | 26:dad0b2031173 | 182 | pc.printf("OK\r\n"); |
Yo_Robot | 26:dad0b2031173 | 183 | else |
Yo_Robot | 26:dad0b2031173 | 184 | RS_232.printf("OK\r\n"); |
Yo_Robot | 26:dad0b2031173 | 185 | |
Yo_Robot | 23:2126e38bb48c | 186 | break; |
Yo_Robot | 24:a1d16835201c | 187 | } |
Yo_Robot | 23:2126e38bb48c | 188 | case 'V': //Setear la velocidad de Posicionamiento |
Yo_Robot | 24:a1d16835201c | 189 | { |
Yo_Robot | 23:2126e38bb48c | 190 | fq_posicion = value; |
Yo_Robot | 26:dad0b2031173 | 191 | |
Yo_Robot | 26:dad0b2031173 | 192 | // Envia un OK de comando recibido |
Yo_Robot | 26:dad0b2031173 | 193 | if( isPC ) |
Yo_Robot | 26:dad0b2031173 | 194 | pc.printf("OK\r\n"); |
Yo_Robot | 26:dad0b2031173 | 195 | else |
Yo_Robot | 26:dad0b2031173 | 196 | RS_232.printf("OK\r\n"); |
Yo_Robot | 26:dad0b2031173 | 197 | |
Yo_Robot | 23:2126e38bb48c | 198 | break; |
Yo_Robot | 24:a1d16835201c | 199 | } |
Yo_Robot | 27:b8254b76ec57 | 200 | |
Yo_Robot | 27:b8254b76ec57 | 201 | // Generar un numero definido de pulsos a la velocidad de posicionamiento |
Yo_Robot | 29:52932326c45a | 202 | case 'P': |
Yo_Robot | 24:a1d16835201c | 203 | { |
Yo_Robot | 28:b7ded82ee7da | 204 | float pulsos = value; //Numero de pulsos a generar |
Yo_Robot | 28:b7ded82ee7da | 205 | t_alto = (float)(pulsos / fq_posicion); //Tiempo que debe ser generado el tren de pulsos. |
Yo_Robot | 27:b8254b76ec57 | 206 | |
Yo_Robot | 32:4483d6c225e5 | 207 | bandera_inicio = 0; //En caso que haya estado yendo a HOME, cancelar |
Yo_Robot | 32:4483d6c225e5 | 208 | |
Yo_Robot | 23:2126e38bb48c | 209 | stopTimer2(); //Deten el tren de pulsos |
Yo_Robot | 23:2126e38bb48c | 210 | setPTO( fq_posicion ); //Nueva frecuencia de salida |
Yo_Robot | 26:dad0b2031173 | 211 | |
Yo_Robot | 35:92b0f1b75a51 | 212 | pc.printf("\n\nattach funcion: %f ", t_alto); |
Yo_Robot | 26:dad0b2031173 | 213 | |
Yo_Robot | 35:92b0f1b75a51 | 214 | temporizador.attach( &posicion_ok, t_alto ); |
Yo_Robot | 35:92b0f1b75a51 | 215 | cronometro.start(); |
Yo_Robot | 26:dad0b2031173 | 216 | |
Yo_Robot | 35:92b0f1b75a51 | 217 | startTimer2(); //Inicia el tren de pulsos |
Yo_Robot | 35:92b0f1b75a51 | 218 | // termina cuando el timeout detiene |
Yo_Robot | 35:92b0f1b75a51 | 219 | |
Yo_Robot | 35:92b0f1b75a51 | 220 | pc.printf("\n\nCorriendo "); |
Yo_Robot | 26:dad0b2031173 | 221 | |
Yo_Robot | 23:2126e38bb48c | 222 | break; |
Yo_Robot | 24:a1d16835201c | 223 | } |
Yo_Robot | 27:b8254b76ec57 | 224 | |
Yo_Robot | 27:b8254b76ec57 | 225 | // Generar un numero definido de MILES de pulsos a la velocidad de posicionamiento |
Yo_Robot | 29:52932326c45a | 226 | case 'M': |
Yo_Robot | 27:b8254b76ec57 | 227 | { |
Yo_Robot | 28:b7ded82ee7da | 228 | float pulsos = value * 1000; //Numero de pulsos a generar |
Yo_Robot | 28:b7ded82ee7da | 229 | t_alto = (float)(pulsos / fq_posicion); //Tiempo que debe ser generado el tren de pulsos. |
Yo_Robot | 32:4483d6c225e5 | 230 | bandera_inicio = 0; //En caso que haya estado yendo a HOME, cancelar |
Yo_Robot | 32:4483d6c225e5 | 231 | |
Yo_Robot | 27:b8254b76ec57 | 232 | //DEBUG |
Yo_Robot | 32:4483d6c225e5 | 233 | //pc.printf("Tiempo en timer en seg = %f", t_alto); |
Yo_Robot | 27:b8254b76ec57 | 234 | |
Yo_Robot | 27:b8254b76ec57 | 235 | stopTimer2(); //Deten el tren de pulsos |
Yo_Robot | 27:b8254b76ec57 | 236 | setPTO( fq_posicion ); //Nueva frecuencia de salida |
Yo_Robot | 27:b8254b76ec57 | 237 | startTimer2(); //Inicia el tren de pulsos |
Yo_Robot | 27:b8254b76ec57 | 238 | wait( t_alto ); //Espera hasta llegar a la posicion |
Yo_Robot | 27:b8254b76ec57 | 239 | stopTimer2(); //Posicion alcanzada ALTO. |
Yo_Robot | 27:b8254b76ec57 | 240 | |
Yo_Robot | 27:b8254b76ec57 | 241 | |
Yo_Robot | 27:b8254b76ec57 | 242 | // Envia un OK de comando recibido |
Yo_Robot | 27:b8254b76ec57 | 243 | if( isPC ) |
Yo_Robot | 27:b8254b76ec57 | 244 | pc.printf("OK\r\n"); |
Yo_Robot | 27:b8254b76ec57 | 245 | else |
Yo_Robot | 27:b8254b76ec57 | 246 | RS_232.printf("OK\r\n"); |
Yo_Robot | 27:b8254b76ec57 | 247 | |
Yo_Robot | 27:b8254b76ec57 | 248 | |
Yo_Robot | 27:b8254b76ec57 | 249 | break; |
Yo_Robot | 27:b8254b76ec57 | 250 | } |
Yo_Robot | 27:b8254b76ec57 | 251 | |
Yo_Robot | 27:b8254b76ec57 | 252 | // Generar un numero definido de MILLONES pulsos a la velocidad de posicionamiento |
Yo_Robot | 29:52932326c45a | 253 | case 'N': |
Yo_Robot | 27:b8254b76ec57 | 254 | { |
Yo_Robot | 28:b7ded82ee7da | 255 | float pulsos = value * 1000000; //Numero de pulsos a generar |
Yo_Robot | 28:b7ded82ee7da | 256 | t_alto = (float)(pulsos / fq_posicion); //Tiempo que debe ser generado el tren de pulsos. |
Yo_Robot | 32:4483d6c225e5 | 257 | bandera_inicio = 0; //En caso que haya estado yendo a HOME, cancelar |
Yo_Robot | 32:4483d6c225e5 | 258 | |
Yo_Robot | 27:b8254b76ec57 | 259 | //DEBUG |
Yo_Robot | 32:4483d6c225e5 | 260 | //pc.printf("Tiempo en timer en seg = %f", t_alto); |
Yo_Robot | 27:b8254b76ec57 | 261 | |
Yo_Robot | 27:b8254b76ec57 | 262 | stopTimer2(); //Deten el tren de pulsos |
Yo_Robot | 27:b8254b76ec57 | 263 | setPTO( fq_posicion ); //Nueva frecuencia de salida |
Yo_Robot | 27:b8254b76ec57 | 264 | startTimer2(); //Inicia el tren de pulsos |
Yo_Robot | 27:b8254b76ec57 | 265 | wait( t_alto ); //Espera hasta llegar a la posicion |
Yo_Robot | 27:b8254b76ec57 | 266 | stopTimer2(); //Posicion alcanzada ALTO. |
Yo_Robot | 27:b8254b76ec57 | 267 | |
Yo_Robot | 27:b8254b76ec57 | 268 | |
Yo_Robot | 27:b8254b76ec57 | 269 | // Envia un OK de comando recibido |
Yo_Robot | 27:b8254b76ec57 | 270 | if( isPC ) |
Yo_Robot | 27:b8254b76ec57 | 271 | pc.printf("OK\r\n"); |
Yo_Robot | 27:b8254b76ec57 | 272 | else |
Yo_Robot | 27:b8254b76ec57 | 273 | RS_232.printf("OK\r\n"); |
Yo_Robot | 27:b8254b76ec57 | 274 | |
Yo_Robot | 27:b8254b76ec57 | 275 | |
Yo_Robot | 27:b8254b76ec57 | 276 | break; |
Yo_Robot | 27:b8254b76ec57 | 277 | } |
Yo_Robot | 27:b8254b76ec57 | 278 | |
Yo_Robot | 27:b8254b76ec57 | 279 | |
Yo_Robot | 29:52932326c45a | 280 | case 'R': // Leer la velocidd del encoder en RPM's |
Yo_Robot | 24:a1d16835201c | 281 | { |
Yo_Robot | 23:2126e38bb48c | 282 | if( isPC ) |
Yo_Robot | 33:e6ff02c3e0f5 | 283 | { |
Yo_Robot | 33:e6ff02c3e0f5 | 284 | pc.printf( "%u", encoder.CalculateRPM( encoder.GetVelocityCap(), 360) ); // ultima velocidad leida desde el encoder |
Yo_Robot | 33:e6ff02c3e0f5 | 285 | }else |
Yo_Robot | 33:e6ff02c3e0f5 | 286 | RS_232.printf( "%u",encoder.CalculateRPM( encoder.GetVelocityCap(), 360) ); |
Yo_Robot | 23:2126e38bb48c | 287 | break; |
Yo_Robot | 24:a1d16835201c | 288 | } |
Yo_Robot | 26:dad0b2031173 | 289 | |
Yo_Robot | 23:2126e38bb48c | 290 | case 'Z': //Limpiar contador encoder |
Yo_Robot | 29:52932326c45a | 291 | encoder.Reset(QEI_RESET_POS); |
Yo_Robot | 29:52932326c45a | 292 | encoder.Reset(QEI_RESET_VEL); |
Yo_Robot | 26:dad0b2031173 | 293 | |
Yo_Robot | 26:dad0b2031173 | 294 | // Envia un OK de comando recibido |
Yo_Robot | 26:dad0b2031173 | 295 | if( isPC ) |
Yo_Robot | 26:dad0b2031173 | 296 | pc.printf("OK\r\n"); |
Yo_Robot | 26:dad0b2031173 | 297 | else |
Yo_Robot | 26:dad0b2031173 | 298 | RS_232.printf("OK\r\n"); |
Yo_Robot | 26:dad0b2031173 | 299 | |
Yo_Robot | 23:2126e38bb48c | 300 | break; |
Yo_Robot | 23:2126e38bb48c | 301 | |
Yo_Robot | 23:2126e38bb48c | 302 | case 'S': //Encender el Servo |
Yo_Robot | 26:dad0b2031173 | 303 | |
Yo_Robot | 23:2126e38bb48c | 304 | pin_son = value; |
Yo_Robot | 32:4483d6c225e5 | 305 | bandera_inicio = 0; //En caso que haya estado yendo a HOME, cancelar |
Yo_Robot | 32:4483d6c225e5 | 306 | |
Yo_Robot | 32:4483d6c225e5 | 307 | // Envia un OK de comando recibido |
Yo_Robot | 32:4483d6c225e5 | 308 | if( isPC ) |
Yo_Robot | 32:4483d6c225e5 | 309 | pc.printf("OK\r\n"); |
Yo_Robot | 32:4483d6c225e5 | 310 | else |
Yo_Robot | 32:4483d6c225e5 | 311 | RS_232.printf("OK\r\n"); |
Yo_Robot | 32:4483d6c225e5 | 312 | |
Yo_Robot | 32:4483d6c225e5 | 313 | break; |
Yo_Robot | 32:4483d6c225e5 | 314 | |
Yo_Robot | 32:4483d6c225e5 | 315 | case 'I': //Ir al inicio del recorrido |
Yo_Robot | 32:4483d6c225e5 | 316 | |
Yo_Robot | 32:4483d6c225e5 | 317 | pin_dir = 1; //Mover hacia el motor |
Yo_Robot | 32:4483d6c225e5 | 318 | bandera_inicio = 1; // Informar a ISR_Adv_motor() acerca del Homing |
Yo_Robot | 32:4483d6c225e5 | 319 | setPTO( value ); // a la velocidad seteada |
Yo_Robot | 26:dad0b2031173 | 320 | |
Yo_Robot | 26:dad0b2031173 | 321 | // Envia un OK de comando recibido |
Yo_Robot | 26:dad0b2031173 | 322 | if( isPC ) |
Yo_Robot | 26:dad0b2031173 | 323 | pc.printf("OK\r\n"); |
Yo_Robot | 26:dad0b2031173 | 324 | else |
Yo_Robot | 26:dad0b2031173 | 325 | RS_232.printf("OK\r\n"); |
Yo_Robot | 26:dad0b2031173 | 326 | |
Yo_Robot | 23:2126e38bb48c | 327 | break; |
Yo_Robot | 28:b7ded82ee7da | 328 | |
Yo_Robot | 28:b7ded82ee7da | 329 | default: |
Yo_Robot | 28:b7ded82ee7da | 330 | |
Yo_Robot | 28:b7ded82ee7da | 331 | // EL COMANDO NO SE RECONOCE: No Aplica |
Yo_Robot | 28:b7ded82ee7da | 332 | if( isPC ) |
Yo_Robot | 28:b7ded82ee7da | 333 | pc.printf("NA\r\n"); |
Yo_Robot | 28:b7ded82ee7da | 334 | else |
Yo_Robot | 28:b7ded82ee7da | 335 | RS_232.printf("NA\r\n"); |
Yo_Robot | 28:b7ded82ee7da | 336 | |
Yo_Robot | 28:b7ded82ee7da | 337 | break; |
Yo_Robot | 32:4483d6c225e5 | 338 | |
Yo_Robot | 18:cf1e07d82630 | 339 | } |
Yo_Robot | 23:2126e38bb48c | 340 | |
Yo_Robot | 12:c02b08dacc45 | 341 | } |
Yo_Robot | 12:c02b08dacc45 | 342 | |
Yo_Robot | 12:c02b08dacc45 | 343 | |
Yo_Robot | 12:c02b08dacc45 | 344 | void setPTO( int freq ) |
Yo_Robot | 12:c02b08dacc45 | 345 | { |
Yo_Robot | 12:c02b08dacc45 | 346 | if( freq != 0 ) |
Yo_Robot | 12:c02b08dacc45 | 347 | { |
Yo_Robot | 12:c02b08dacc45 | 348 | LPC_TIM2->TC = 0x00; //Resetear Timer |
Yo_Robot | 12:c02b08dacc45 | 349 | setMR2( getMRvalue( freq ) ); |
Yo_Robot | 12:c02b08dacc45 | 350 | startTimer2(); |
Yo_Robot | 12:c02b08dacc45 | 351 | |
Yo_Robot | 12:c02b08dacc45 | 352 | }else{ |
Yo_Robot | 12:c02b08dacc45 | 353 | |
Yo_Robot | 12:c02b08dacc45 | 354 | stopTimer2(); |
Yo_Robot | 12:c02b08dacc45 | 355 | LPC_TIM2->TC = 0x00; //Resetear Timer |
Yo_Robot | 12:c02b08dacc45 | 356 | } |
Yo_Robot | 12:c02b08dacc45 | 357 | } |
Yo_Robot | 12:c02b08dacc45 | 358 | |
Yo_Robot | 22:d5431fff164b | 359 | |
Yo_Robot | 22:d5431fff164b | 360 | |
Yo_Robot | 22:d5431fff164b | 361 | void ISR_Alarm() |
Yo_Robot | 22:d5431fff164b | 362 | { |
Yo_Robot | 22:d5431fff164b | 363 | pin_son = 0 ; |
Yo_Robot | 22:d5431fff164b | 364 | stopTimer2(); |
Yo_Robot | 22:d5431fff164b | 365 | aout = 0.5 ; |
Yo_Robot | 22:d5431fff164b | 366 | |
Yo_Robot | 22:d5431fff164b | 367 | if(isPC) |
Yo_Robot | 31:7e2cdd547cb2 | 368 | pc.printf( "AL\r\n" ); //ALARMA! solo es AL para que |
Yo_Robot | 31:7e2cdd547cb2 | 369 | //sea conciso con el modo ETH y funcione |
Yo_Robot | 31:7e2cdd547cb2 | 370 | //bien en LabVIEW. |
Yo_Robot | 22:d5431fff164b | 371 | else |
Yo_Robot | 26:dad0b2031173 | 372 | RS_232.printf( "AL\r\n" ); |
Yo_Robot | 22:d5431fff164b | 373 | } |
Yo_Robot | 22:d5431fff164b | 374 | |
Yo_Robot | 30:413d1a6648b5 | 375 | |
Yo_Robot | 30:413d1a6648b5 | 376 | |
Yo_Robot | 30:413d1a6648b5 | 377 | |
Yo_Robot | 30:413d1a6648b5 | 378 | /** @brief: Esta es la rutina que maneja las interrupciones |
Yo_Robot | 30:413d1a6648b5 | 379 | * del sensor óptico, al recibir una ALARMA de proximidad al encoder |
Yo_Robot | 30:413d1a6648b5 | 380 | */ |
Yo_Robot | 31:7e2cdd547cb2 | 381 | void ISR_Alm_encoder() |
Yo_Robot | 31:7e2cdd547cb2 | 382 | { |
Yo_Robot | 31:7e2cdd547cb2 | 383 | if( isPC ) |
Yo_Robot | 32:4483d6c225e5 | 384 | pc.printf("A3\r\n"); |
Yo_Robot | 31:7e2cdd547cb2 | 385 | else |
Yo_Robot | 32:4483d6c225e5 | 386 | RS_232.printf("A3\r\n"); |
Yo_Robot | 31:7e2cdd547cb2 | 387 | } |
Yo_Robot | 30:413d1a6648b5 | 388 | |
Yo_Robot | 30:413d1a6648b5 | 389 | |
Yo_Robot | 30:413d1a6648b5 | 390 | /** @brief: Esta es la rutina que maneja las interrupciones |
Yo_Robot | 30:413d1a6648b5 | 391 | * del sensor óptico, al recibir una ALARMA de proximidad al motor |
Yo_Robot | 30:413d1a6648b5 | 392 | */ |
Yo_Robot | 31:7e2cdd547cb2 | 393 | void ISR_Alm_motor() |
Yo_Robot | 31:7e2cdd547cb2 | 394 | { |
Yo_Robot | 32:4483d6c225e5 | 395 | wait_us( 50 ); |
Yo_Robot | 32:4483d6c225e5 | 396 | if ( limite_4 == 1) |
Yo_Robot | 32:4483d6c225e5 | 397 | { |
Yo_Robot | 32:4483d6c225e5 | 398 | if( isPC ) |
Yo_Robot | 32:4483d6c225e5 | 399 | pc.printf("A0\r\n"); |
Yo_Robot | 32:4483d6c225e5 | 400 | else |
Yo_Robot | 32:4483d6c225e5 | 401 | RS_232.printf("A0\r\n"); |
Yo_Robot | 32:4483d6c225e5 | 402 | } |
Yo_Robot | 31:7e2cdd547cb2 | 403 | } |
Yo_Robot | 30:413d1a6648b5 | 404 | |
Yo_Robot | 30:413d1a6648b5 | 405 | |
Yo_Robot | 30:413d1a6648b5 | 406 | /** @brief: Esta es la rutina que maneja las interrupciones |
Yo_Robot | 30:413d1a6648b5 | 407 | * del sensor óptico, al recibir una advertencia de proximidad al encoder |
Yo_Robot | 30:413d1a6648b5 | 408 | */ |
Yo_Robot | 31:7e2cdd547cb2 | 409 | void ISR_Adv_encoder() |
Yo_Robot | 31:7e2cdd547cb2 | 410 | { |
Yo_Robot | 32:4483d6c225e5 | 411 | wait_ms( 50 ); |
Yo_Robot | 32:4483d6c225e5 | 412 | if ( limite_2 == 1) |
Yo_Robot | 32:4483d6c225e5 | 413 | { |
Yo_Robot | 31:7e2cdd547cb2 | 414 | |
Yo_Robot | 32:4483d6c225e5 | 415 | if( isPC ) |
Yo_Robot | 32:4483d6c225e5 | 416 | pc.printf("A2\r\n"); |
Yo_Robot | 32:4483d6c225e5 | 417 | else |
Yo_Robot | 32:4483d6c225e5 | 418 | RS_232.printf("A2\r\n"); |
Yo_Robot | 32:4483d6c225e5 | 419 | } |
Yo_Robot | 31:7e2cdd547cb2 | 420 | |
Yo_Robot | 31:7e2cdd547cb2 | 421 | } |
Yo_Robot | 30:413d1a6648b5 | 422 | |
Yo_Robot | 30:413d1a6648b5 | 423 | |
Yo_Robot | 30:413d1a6648b5 | 424 | /** @brief: Esta es la rutina que maneja las interrupciones |
Yo_Robot | 30:413d1a6648b5 | 425 | * del sensor óptico, al recibir una advertencia de proximidad al motor |
Yo_Robot | 30:413d1a6648b5 | 426 | */ |
Yo_Robot | 31:7e2cdd547cb2 | 427 | void ISR_Adv_motor() |
Yo_Robot | 31:7e2cdd547cb2 | 428 | { |
Yo_Robot | 32:4483d6c225e5 | 429 | if ( bandera_inicio == 1 ) |
Yo_Robot | 32:4483d6c225e5 | 430 | { |
Yo_Robot | 32:4483d6c225e5 | 431 | setPTO( 0 ); //detener el carro |
Yo_Robot | 32:4483d6c225e5 | 432 | pin_dir = 0; //hacer que se aleje del motor |
Yo_Robot | 32:4483d6c225e5 | 433 | |
Yo_Robot | 32:4483d6c225e5 | 434 | |
Yo_Robot | 32:4483d6c225e5 | 435 | stopTimer2(); //Deten el tren de pulsos |
Yo_Robot | 32:4483d6c225e5 | 436 | setPTO( 20000 ); //Nueva frecuencia de salida |
Yo_Robot | 32:4483d6c225e5 | 437 | startTimer2(); //Inicia el tren de pulsos |
Yo_Robot | 32:4483d6c225e5 | 438 | wait_ms( 100 ); //Espera hasta llegar a la posicion |
Yo_Robot | 32:4483d6c225e5 | 439 | stopTimer2(); //Posicion alcanzada ALTO. |
Yo_Robot | 32:4483d6c225e5 | 440 | |
Yo_Robot | 32:4483d6c225e5 | 441 | setPTO (0); // Detener el carro cuando este en posición valida |
Yo_Robot | 32:4483d6c225e5 | 442 | wait_ms(100); |
Yo_Robot | 32:4483d6c225e5 | 443 | //Encerar el contador de encoder y de velocidad |
Yo_Robot | 32:4483d6c225e5 | 444 | encoder.Reset(QEI_RESET_POS); |
Yo_Robot | 32:4483d6c225e5 | 445 | encoder.Reset(QEI_RESET_VEL); |
Yo_Robot | 32:4483d6c225e5 | 446 | |
Yo_Robot | 32:4483d6c225e5 | 447 | bandera_inicio = 0; //Limpiar la bandera |
Yo_Robot | 32:4483d6c225e5 | 448 | |
Yo_Robot | 32:4483d6c225e5 | 449 | // Envia un IN de proceso terminado |
Yo_Robot | 32:4483d6c225e5 | 450 | if( isPC ) |
Yo_Robot | 32:4483d6c225e5 | 451 | pc.printf("IN\r\n"); |
Yo_Robot | 32:4483d6c225e5 | 452 | else |
Yo_Robot | 32:4483d6c225e5 | 453 | RS_232.printf("IN\r\n"); |
Yo_Robot | 32:4483d6c225e5 | 454 | } |
Yo_Robot | 32:4483d6c225e5 | 455 | |
Yo_Robot | 32:4483d6c225e5 | 456 | else{ |
Yo_Robot | 32:4483d6c225e5 | 457 | |
Yo_Robot | 32:4483d6c225e5 | 458 | if( isPC ) |
Yo_Robot | 32:4483d6c225e5 | 459 | pc.printf("A1\r\n"); |
Yo_Robot | 32:4483d6c225e5 | 460 | else |
Yo_Robot | 32:4483d6c225e5 | 461 | RS_232.printf("A1\r\n"); |
Yo_Robot | 32:4483d6c225e5 | 462 | } |
Yo_Robot | 32:4483d6c225e5 | 463 | |
Yo_Robot | 31:7e2cdd547cb2 | 464 | } |
Yo_Robot | 30:413d1a6648b5 | 465 | |
Yo_Robot | 30:413d1a6648b5 | 466 | |
Yo_Robot | 30:413d1a6648b5 | 467 | |
Yo_Robot | 22:d5431fff164b | 468 | int getMRvalue( int fout ) |
Yo_Robot | 22:d5431fff164b | 469 | { |
Yo_Robot | 22:d5431fff164b | 470 | int toRegister; |
Yo_Robot | 22:d5431fff164b | 471 | |
Yo_Robot | 22:d5431fff164b | 472 | toRegister = (24000000 /(fout*2.0) ) -1; |
Yo_Robot | 22:d5431fff164b | 473 | return toRegister; |
Yo_Robot | 22:d5431fff164b | 474 | } |
Yo_Robot | 22:d5431fff164b | 475 | |
Yo_Robot | 22:d5431fff164b | 476 | |
Yo_Robot | 22:d5431fff164b | 477 | void setMR2( int newValue ) |
Yo_Robot | 22:d5431fff164b | 478 | { |
Yo_Robot | 22:d5431fff164b | 479 | LPC_TIM2->MR2 = newValue; // Las dos salidas son identicas |
Yo_Robot | 22:d5431fff164b | 480 | LPC_TIM2->MR3 = newValue; // Para testear el programa. |
Yo_Robot | 22:d5431fff164b | 481 | } |
Yo_Robot | 22:d5431fff164b | 482 | |
Yo_Robot | 22:d5431fff164b | 483 | |
Yo_Robot | 22:d5431fff164b | 484 | |
Yo_Robot | 22:d5431fff164b | 485 | void startTimer2() |
Yo_Robot | 22:d5431fff164b | 486 | { |
Yo_Robot | 22:d5431fff164b | 487 | // Arrancer el Timer 2 |
Yo_Robot | 22:d5431fff164b | 488 | LPC_TIM2->TCR = 1; |
Yo_Robot | 22:d5431fff164b | 489 | } |
Yo_Robot | 22:d5431fff164b | 490 | |
Yo_Robot | 22:d5431fff164b | 491 | void stopTimer2() |
Yo_Robot | 22:d5431fff164b | 492 | { |
Yo_Robot | 22:d5431fff164b | 493 | // Detener el Timer 2 |
Yo_Robot | 22:d5431fff164b | 494 | LPC_TIM2->TCR = 0x2; |
Yo_Robot | 22:d5431fff164b | 495 | } |
Yo_Robot | 22:d5431fff164b | 496 | |
Yo_Robot | 22:d5431fff164b | 497 | int getBaud() |
Yo_Robot | 22:d5431fff164b | 498 | { |
Yo_Robot | 22:d5431fff164b | 499 | int baudios = 115200; //Valor por defecto |
Yo_Robot | 22:d5431fff164b | 500 | |
Yo_Robot | 22:d5431fff164b | 501 | FILE *fp = fopen("/local/config.mbd", "r"); // Abre el archivo y lo guarda en fp |
Yo_Robot | 22:d5431fff164b | 502 | |
Yo_Robot | 22:d5431fff164b | 503 | if(!fp) // En caso de no encontrarse el archivo |
Yo_Robot | 22:d5431fff164b | 504 | { |
Yo_Robot | 22:d5431fff164b | 505 | printf("\nEl archivo /mbed/config.txt no puede ser abierto!\n"); |
Yo_Robot | 22:d5431fff164b | 506 | printf("Cree un archivo de texto: config.mbd dentro de la unidad Mbed\n"); |
Yo_Robot | 22:d5431fff164b | 507 | printf("que contenga las lineas:\n\n"); |
Yo_Robot | 22:d5431fff164b | 508 | |
Yo_Robot | 22:d5431fff164b | 509 | printf(" 1\n"); |
Yo_Robot | 22:d5431fff164b | 510 | printf(" 2\n"); |
Yo_Robot | 22:d5431fff164b | 511 | printf(" 3\n"); |
Yo_Robot | 22:d5431fff164b | 512 | printf(" 4\n"); |
Yo_Robot | 22:d5431fff164b | 513 | printf(" baudios: 115200\n"); |
Yo_Robot | 22:d5431fff164b | 514 | |
Yo_Robot | 22:d5431fff164b | 515 | printf("Cambie el valor de 115200 por la velocidad a la que desea transmitir:\n"); |
Yo_Robot | 22:d5431fff164b | 516 | printf("luego reinicie el microcontrolador\n"); |
Yo_Robot | 22:d5431fff164b | 517 | exit(1); |
Yo_Robot | 22:d5431fff164b | 518 | |
Yo_Robot | 22:d5431fff164b | 519 | } |
Yo_Robot | 22:d5431fff164b | 520 | else |
Yo_Robot | 22:d5431fff164b | 521 | { |
Yo_Robot | 22:d5431fff164b | 522 | // Cadenas de caracteres desde el Archivo config.mbd |
Yo_Robot | 22:d5431fff164b | 523 | char notstr [04]; // linea vacia |
Yo_Robot | 22:d5431fff164b | 524 | char baud [40]; // valor en baudios |
Yo_Robot | 22:d5431fff164b | 525 | |
Yo_Robot | 22:d5431fff164b | 526 | // Leer linea a linea el archivo |
Yo_Robot | 22:d5431fff164b | 527 | // cuatro primeras lineas no sirven |
Yo_Robot | 22:d5431fff164b | 528 | fgets( notstr, 4, fp ); |
Yo_Robot | 22:d5431fff164b | 529 | fgets( notstr, 4, fp ); |
Yo_Robot | 22:d5431fff164b | 530 | fgets( notstr, 4, fp ); |
Yo_Robot | 22:d5431fff164b | 531 | fgets( notstr, 4, fp ); |
Yo_Robot | 22:d5431fff164b | 532 | fgets( baud, 40, fp ); |
Yo_Robot | 22:d5431fff164b | 533 | fclose(fp); |
Yo_Robot | 23:2126e38bb48c | 534 | |
Yo_Robot | 22:d5431fff164b | 535 | // Extraer los valores numericos |
Yo_Robot | 22:d5431fff164b | 536 | sscanf( baud,"%*s %d",&baudios ); |
Yo_Robot | 22:d5431fff164b | 537 | |
Yo_Robot | 22:d5431fff164b | 538 | |
Yo_Robot | 22:d5431fff164b | 539 | } |
Yo_Robot | 22:d5431fff164b | 540 | |
Yo_Robot | 22:d5431fff164b | 541 | return baudios; |
Yo_Robot | 22:d5431fff164b | 542 | } |
Yo_Robot | 22:d5431fff164b | 543 | |
Yo_Robot | 22:d5431fff164b | 544 | // **** Funciones Liberia Ethernet ***** // |
Yo_Robot | 12:c02b08dacc45 | 545 | void setPTO_eth( char * input, char * output ) |
Yo_Robot | 12:c02b08dacc45 | 546 | { |
Yo_Robot | 12:c02b08dacc45 | 547 | int freq = atoi( input ); |
Yo_Robot | 12:c02b08dacc45 | 548 | |
Yo_Robot | 12:c02b08dacc45 | 549 | if( freq != 0 ){ |
Yo_Robot | 12:c02b08dacc45 | 550 | LPC_TIM2->TC = 0x00; // Resetear Timer |
Yo_Robot | 12:c02b08dacc45 | 551 | setMR2( getMRvalue( freq ) ); // Cambiar frefuencia |
Yo_Robot | 12:c02b08dacc45 | 552 | startTimer2(); // Iniciar Timer |
Yo_Robot | 14:039d070732d5 | 553 | if( pin_alm == 0 ) |
Yo_Robot | 31:7e2cdd547cb2 | 554 | { |
Yo_Robot | 31:7e2cdd547cb2 | 555 | if ( limite_2 == 1 ) // Alarma muy cerca al encoder |
Yo_Robot | 31:7e2cdd547cb2 | 556 | { |
Yo_Robot | 31:7e2cdd547cb2 | 557 | sprintf( output,"A2\r\n" ); |
Yo_Robot | 31:7e2cdd547cb2 | 558 | } |
Yo_Robot | 31:7e2cdd547cb2 | 559 | if ( limite_3 == 1 ) // Alarma muy cerca al encoder |
Yo_Robot | 31:7e2cdd547cb2 | 560 | { |
Yo_Robot | 31:7e2cdd547cb2 | 561 | sprintf( output,"A1\r\n" ); |
Yo_Robot | 31:7e2cdd547cb2 | 562 | } |
Yo_Robot | 31:7e2cdd547cb2 | 563 | if ( limite_1 == 1 || limite_4 == 1 ) // ERROR DE POSICION |
Yo_Robot | 31:7e2cdd547cb2 | 564 | { |
Yo_Robot | 31:7e2cdd547cb2 | 565 | sprintf( output,"A0\r\n" ); |
Yo_Robot | 31:7e2cdd547cb2 | 566 | } |
Yo_Robot | 31:7e2cdd547cb2 | 567 | |
Yo_Robot | 31:7e2cdd547cb2 | 568 | else |
Yo_Robot | 31:7e2cdd547cb2 | 569 | sprintf( output,"OK\r\n" ); |
Yo_Robot | 31:7e2cdd547cb2 | 570 | } |
Yo_Robot | 14:039d070732d5 | 571 | else |
Yo_Robot | 14:039d070732d5 | 572 | sprintf( output,"AL" ); |
Yo_Robot | 12:c02b08dacc45 | 573 | }else{ |
Yo_Robot | 12:c02b08dacc45 | 574 | stopTimer2(); |
Yo_Robot | 12:c02b08dacc45 | 575 | LPC_TIM2->TC = 0x00; // Resetear Timer |
Yo_Robot | 14:039d070732d5 | 576 | if( pin_alm == 0 ) |
Yo_Robot | 31:7e2cdd547cb2 | 577 | { |
Yo_Robot | 31:7e2cdd547cb2 | 578 | if ( limite_2 == 1 ) // Alarma muy cerca al encoder |
Yo_Robot | 31:7e2cdd547cb2 | 579 | { |
Yo_Robot | 31:7e2cdd547cb2 | 580 | sprintf( output,"A2\r\n" ); |
Yo_Robot | 31:7e2cdd547cb2 | 581 | } |
Yo_Robot | 31:7e2cdd547cb2 | 582 | if ( limite_3 == 1 ) // Alarma muy cerca al encoder |
Yo_Robot | 31:7e2cdd547cb2 | 583 | { |
Yo_Robot | 31:7e2cdd547cb2 | 584 | sprintf( output,"A1\r\n" ); |
Yo_Robot | 31:7e2cdd547cb2 | 585 | } |
Yo_Robot | 31:7e2cdd547cb2 | 586 | if ( limite_1 == 1 || limite_4 == 1 ) // ERROR DE POSICION |
Yo_Robot | 31:7e2cdd547cb2 | 587 | { |
Yo_Robot | 31:7e2cdd547cb2 | 588 | sprintf( output,"A0\r\n" ); |
Yo_Robot | 31:7e2cdd547cb2 | 589 | } |
Yo_Robot | 31:7e2cdd547cb2 | 590 | |
Yo_Robot | 31:7e2cdd547cb2 | 591 | else |
Yo_Robot | 31:7e2cdd547cb2 | 592 | sprintf( output,"OK\r\n" ); |
Yo_Robot | 31:7e2cdd547cb2 | 593 | } |
Yo_Robot | 14:039d070732d5 | 594 | else |
Yo_Robot | 14:039d070732d5 | 595 | sprintf( output,"AL" ); |
Yo_Robot | 12:c02b08dacc45 | 596 | } |
Yo_Robot | 12:c02b08dacc45 | 597 | } |
Yo_Robot | 12:c02b08dacc45 | 598 | |
Yo_Robot | 15:a1ffa32ce9d1 | 599 | void setANG_eth( char * input, char * output ) |
Yo_Robot | 15:a1ffa32ce9d1 | 600 | { |
Yo_Robot | 15:a1ffa32ce9d1 | 601 | long int pulsos = atol( input ); //Numero de pulsos a generar |
Yo_Robot | 29:52932326c45a | 602 | float pulsos_f = (float) pulsos; |
Yo_Robot | 29:52932326c45a | 603 | t_alto = pulsos_f / fq_posicion; //Tiempo que debe ser generado el tren de pulsos. |
Yo_Robot | 15:a1ffa32ce9d1 | 604 | |
Yo_Robot | 15:a1ffa32ce9d1 | 605 | stopTimer2(); //Deten el tren de pulsos |
Yo_Robot | 15:a1ffa32ce9d1 | 606 | setPTO( fq_posicion ); //Nueva frecuencia de salida |
Yo_Robot | 15:a1ffa32ce9d1 | 607 | startTimer2(); //Inicia el tren de pulsos |
Yo_Robot | 15:a1ffa32ce9d1 | 608 | wait( t_alto ); //Espera hasta llegar a la posicion |
Yo_Robot | 18:cf1e07d82630 | 609 | stopTimer2(); //Posicion alcanzada ALTO. |
Yo_Robot | 15:a1ffa32ce9d1 | 610 | |
Yo_Robot | 15:a1ffa32ce9d1 | 611 | if( pin_alm == 0 ) |
Yo_Robot | 31:7e2cdd547cb2 | 612 | { |
Yo_Robot | 31:7e2cdd547cb2 | 613 | if ( limite_2 == 1 ) // Alarma muy cerca al encoder |
Yo_Robot | 31:7e2cdd547cb2 | 614 | { |
Yo_Robot | 31:7e2cdd547cb2 | 615 | sprintf( output,"A2\r\n" ); |
Yo_Robot | 31:7e2cdd547cb2 | 616 | } |
Yo_Robot | 31:7e2cdd547cb2 | 617 | if ( limite_3 == 1 ) // Alarma muy cerca al encoder |
Yo_Robot | 31:7e2cdd547cb2 | 618 | { |
Yo_Robot | 31:7e2cdd547cb2 | 619 | sprintf( output,"A1\r\n" ); |
Yo_Robot | 31:7e2cdd547cb2 | 620 | } |
Yo_Robot | 31:7e2cdd547cb2 | 621 | if ( limite_1 == 1 || limite_4 == 1 ) // ERROR DE POSICION |
Yo_Robot | 31:7e2cdd547cb2 | 622 | { |
Yo_Robot | 31:7e2cdd547cb2 | 623 | sprintf( output,"A0\r\n" ); |
Yo_Robot | 31:7e2cdd547cb2 | 624 | } |
Yo_Robot | 31:7e2cdd547cb2 | 625 | |
Yo_Robot | 31:7e2cdd547cb2 | 626 | else |
Yo_Robot | 31:7e2cdd547cb2 | 627 | sprintf( output,"OK\r\n" ); |
Yo_Robot | 31:7e2cdd547cb2 | 628 | } |
Yo_Robot | 15:a1ffa32ce9d1 | 629 | else |
Yo_Robot | 15:a1ffa32ce9d1 | 630 | sprintf( output,"AL" ); |
Yo_Robot | 15:a1ffa32ce9d1 | 631 | |
Yo_Robot | 15:a1ffa32ce9d1 | 632 | } |
Yo_Robot | 21:353b0fe8fc54 | 633 | |
Yo_Robot | 21:353b0fe8fc54 | 634 | |
Yo_Robot | 15:a1ffa32ce9d1 | 635 | void setSPD_eth( char * input, char * output ) |
Yo_Robot | 15:a1ffa32ce9d1 | 636 | { |
Yo_Robot | 15:a1ffa32ce9d1 | 637 | fq_posicion = atoi( input ); |
Yo_Robot | 15:a1ffa32ce9d1 | 638 | // Esta funcion cambia la velocidad con la que se |
Yo_Robot | 15:a1ffa32ce9d1 | 639 | // posicionara el eje del motor en un angulo determinado |
Yo_Robot | 15:a1ffa32ce9d1 | 640 | if( pin_alm == 0 ) |
Yo_Robot | 31:7e2cdd547cb2 | 641 | { |
Yo_Robot | 31:7e2cdd547cb2 | 642 | if ( limite_2 == 1 ) // Alarma muy cerca al encoder |
Yo_Robot | 31:7e2cdd547cb2 | 643 | { |
Yo_Robot | 31:7e2cdd547cb2 | 644 | sprintf( output,"A2\r\n" ); |
Yo_Robot | 31:7e2cdd547cb2 | 645 | } |
Yo_Robot | 31:7e2cdd547cb2 | 646 | if ( limite_3 == 1 ) // Alarma muy cerca al encoder |
Yo_Robot | 31:7e2cdd547cb2 | 647 | { |
Yo_Robot | 31:7e2cdd547cb2 | 648 | sprintf( output,"A1\r\n" ); |
Yo_Robot | 31:7e2cdd547cb2 | 649 | } |
Yo_Robot | 31:7e2cdd547cb2 | 650 | if ( limite_1 == 1 || limite_4 == 1 ) // ERROR DE POSICION |
Yo_Robot | 31:7e2cdd547cb2 | 651 | { |
Yo_Robot | 31:7e2cdd547cb2 | 652 | sprintf( output,"A0\r\n" ); |
Yo_Robot | 31:7e2cdd547cb2 | 653 | } |
Yo_Robot | 31:7e2cdd547cb2 | 654 | |
Yo_Robot | 31:7e2cdd547cb2 | 655 | else |
Yo_Robot | 31:7e2cdd547cb2 | 656 | sprintf( output,"OK\r\n" ); |
Yo_Robot | 31:7e2cdd547cb2 | 657 | } |
Yo_Robot | 15:a1ffa32ce9d1 | 658 | else |
Yo_Robot | 15:a1ffa32ce9d1 | 659 | sprintf( output,"AL" ); |
Yo_Robot | 15:a1ffa32ce9d1 | 660 | |
Yo_Robot | 15:a1ffa32ce9d1 | 661 | } |
Yo_Robot | 21:353b0fe8fc54 | 662 | |
Yo_Robot | 12:c02b08dacc45 | 663 | void setAout_eth( char * input, char * output ) |
Yo_Robot | 12:c02b08dacc45 | 664 | { |
Yo_Robot | 12:c02b08dacc45 | 665 | int vout = atoi( input ); |
Yo_Robot | 14:039d070732d5 | 666 | aout = (float)( vout + 10000 ) / 20000; |
Yo_Robot | 14:039d070732d5 | 667 | if( pin_alm == 0 ) |
Yo_Robot | 31:7e2cdd547cb2 | 668 | { |
Yo_Robot | 31:7e2cdd547cb2 | 669 | if ( limite_2 == 1 ) // Alarma muy cerca al encoder |
Yo_Robot | 31:7e2cdd547cb2 | 670 | { |
Yo_Robot | 31:7e2cdd547cb2 | 671 | sprintf( output,"A2\r\n" ); |
Yo_Robot | 31:7e2cdd547cb2 | 672 | } |
Yo_Robot | 31:7e2cdd547cb2 | 673 | if ( limite_3 == 1 ) // Alarma muy cerca al encoder |
Yo_Robot | 31:7e2cdd547cb2 | 674 | { |
Yo_Robot | 31:7e2cdd547cb2 | 675 | sprintf( output,"A1\r\n" ); |
Yo_Robot | 31:7e2cdd547cb2 | 676 | } |
Yo_Robot | 31:7e2cdd547cb2 | 677 | if ( limite_1 == 1 || limite_4 == 1 ) // ERROR DE POSICION |
Yo_Robot | 31:7e2cdd547cb2 | 678 | { |
Yo_Robot | 31:7e2cdd547cb2 | 679 | sprintf( output,"A0\r\n" ); |
Yo_Robot | 31:7e2cdd547cb2 | 680 | } |
Yo_Robot | 31:7e2cdd547cb2 | 681 | |
Yo_Robot | 31:7e2cdd547cb2 | 682 | else |
Yo_Robot | 31:7e2cdd547cb2 | 683 | sprintf( output,"OK\r\n" ); |
Yo_Robot | 31:7e2cdd547cb2 | 684 | } |
Yo_Robot | 14:039d070732d5 | 685 | else |
Yo_Robot | 14:039d070732d5 | 686 | sprintf( output,"AL" ); |
Yo_Robot | 12:c02b08dacc45 | 687 | } |
Yo_Robot | 12:c02b08dacc45 | 688 | |
Yo_Robot | 12:c02b08dacc45 | 689 | |
Yo_Robot | 12:c02b08dacc45 | 690 | |
Yo_Robot | 12:c02b08dacc45 | 691 | void setDir_eth ( char * input, char * output ) |
Yo_Robot | 12:c02b08dacc45 | 692 | { |
Yo_Robot | 12:c02b08dacc45 | 693 | int value = atoi( input ); |
Yo_Robot | 12:c02b08dacc45 | 694 | |
Yo_Robot | 12:c02b08dacc45 | 695 | pin_dir = value; |
Yo_Robot | 12:c02b08dacc45 | 696 | |
Yo_Robot | 14:039d070732d5 | 697 | if( pin_alm == 0 ) |
Yo_Robot | 31:7e2cdd547cb2 | 698 | { |
Yo_Robot | 31:7e2cdd547cb2 | 699 | if ( limite_2 == 1 ) // Alarma muy cerca al encoder |
Yo_Robot | 31:7e2cdd547cb2 | 700 | { |
Yo_Robot | 31:7e2cdd547cb2 | 701 | sprintf( output,"A2\r\n" ); |
Yo_Robot | 31:7e2cdd547cb2 | 702 | } |
Yo_Robot | 31:7e2cdd547cb2 | 703 | if ( limite_3 == 1 ) // Alarma muy cerca al encoder |
Yo_Robot | 31:7e2cdd547cb2 | 704 | { |
Yo_Robot | 31:7e2cdd547cb2 | 705 | sprintf( output,"A1\r\n" ); |
Yo_Robot | 31:7e2cdd547cb2 | 706 | } |
Yo_Robot | 31:7e2cdd547cb2 | 707 | if ( limite_1 == 1 || limite_4 == 1 ) // ERROR DE POSICION |
Yo_Robot | 31:7e2cdd547cb2 | 708 | { |
Yo_Robot | 31:7e2cdd547cb2 | 709 | sprintf( output,"A0\r\n" ); |
Yo_Robot | 31:7e2cdd547cb2 | 710 | } |
Yo_Robot | 31:7e2cdd547cb2 | 711 | |
Yo_Robot | 31:7e2cdd547cb2 | 712 | else |
Yo_Robot | 31:7e2cdd547cb2 | 713 | sprintf( output,"OK\r\n" ); |
Yo_Robot | 31:7e2cdd547cb2 | 714 | } |
Yo_Robot | 12:c02b08dacc45 | 715 | else |
Yo_Robot | 14:039d070732d5 | 716 | sprintf( output,"AL" ); |
Yo_Robot | 12:c02b08dacc45 | 717 | } |
Yo_Robot | 12:c02b08dacc45 | 718 | |
Yo_Robot | 12:c02b08dacc45 | 719 | |
Yo_Robot | 12:c02b08dacc45 | 720 | |
Yo_Robot | 12:c02b08dacc45 | 721 | void setSON_eth ( char * input, char * output ) |
Yo_Robot | 12:c02b08dacc45 | 722 | { |
Yo_Robot | 12:c02b08dacc45 | 723 | int value = atoi( input ); |
Yo_Robot | 12:c02b08dacc45 | 724 | |
Yo_Robot | 12:c02b08dacc45 | 725 | pin_son = value; |
Yo_Robot | 12:c02b08dacc45 | 726 | |
Yo_Robot | 14:039d070732d5 | 727 | if( pin_alm == 0 ) |
Yo_Robot | 31:7e2cdd547cb2 | 728 | { |
Yo_Robot | 31:7e2cdd547cb2 | 729 | if ( limite_2 == 1 ) // Alarma muy cerca al encoder |
Yo_Robot | 31:7e2cdd547cb2 | 730 | { |
Yo_Robot | 31:7e2cdd547cb2 | 731 | sprintf( output,"A2\r\n" ); |
Yo_Robot | 31:7e2cdd547cb2 | 732 | } |
Yo_Robot | 31:7e2cdd547cb2 | 733 | if ( limite_3 == 1 ) // Alarma muy cerca al encoder |
Yo_Robot | 31:7e2cdd547cb2 | 734 | { |
Yo_Robot | 31:7e2cdd547cb2 | 735 | sprintf( output,"A1\r\n" ); |
Yo_Robot | 31:7e2cdd547cb2 | 736 | } |
Yo_Robot | 31:7e2cdd547cb2 | 737 | if ( limite_1 == 1 || limite_4 == 1 ) // ERROR DE POSICION |
Yo_Robot | 31:7e2cdd547cb2 | 738 | { |
Yo_Robot | 31:7e2cdd547cb2 | 739 | sprintf( output,"A0\r\n" ); |
Yo_Robot | 31:7e2cdd547cb2 | 740 | } |
Yo_Robot | 31:7e2cdd547cb2 | 741 | |
Yo_Robot | 31:7e2cdd547cb2 | 742 | else |
Yo_Robot | 31:7e2cdd547cb2 | 743 | sprintf( output,"OK\r\n" ); |
Yo_Robot | 31:7e2cdd547cb2 | 744 | } |
Yo_Robot | 12:c02b08dacc45 | 745 | else |
Yo_Robot | 14:039d070732d5 | 746 | sprintf( output,"AL" ); |
Yo_Robot | 12:c02b08dacc45 | 747 | |
Yo_Robot | 12:c02b08dacc45 | 748 | } |
Yo_Robot | 12:c02b08dacc45 | 749 | |
Yo_Robot | 12:c02b08dacc45 | 750 | |
Yo_Robot | 22:d5431fff164b | 751 | void getENC_eth( char * input, char * output ) |
Yo_Robot | 20:4b154134ab20 | 752 | { |
Yo_Robot | 22:d5431fff164b | 753 | if( pin_alm == 0 ) |
Yo_Robot | 31:7e2cdd547cb2 | 754 | { |
Yo_Robot | 31:7e2cdd547cb2 | 755 | if ( limite_2 == 1 ) // Alarma muy cerca al encoder |
Yo_Robot | 31:7e2cdd547cb2 | 756 | { |
Yo_Robot | 31:7e2cdd547cb2 | 757 | sprintf( output,"A2\r\n" ); |
Yo_Robot | 31:7e2cdd547cb2 | 758 | } |
Yo_Robot | 31:7e2cdd547cb2 | 759 | if ( limite_3 == 1 ) // Alarma muy cerca al encoder |
Yo_Robot | 31:7e2cdd547cb2 | 760 | { |
Yo_Robot | 31:7e2cdd547cb2 | 761 | sprintf( output,"A1\r\n" ); |
Yo_Robot | 31:7e2cdd547cb2 | 762 | } |
Yo_Robot | 31:7e2cdd547cb2 | 763 | if ( limite_1 == 1 || limite_4 == 1 ) // ERROR DE POSICION |
Yo_Robot | 31:7e2cdd547cb2 | 764 | { |
Yo_Robot | 31:7e2cdd547cb2 | 765 | sprintf( output,"A0\r\n" ); |
Yo_Robot | 31:7e2cdd547cb2 | 766 | } |
Yo_Robot | 31:7e2cdd547cb2 | 767 | |
Yo_Robot | 31:7e2cdd547cb2 | 768 | else |
Yo_Robot | 31:7e2cdd547cb2 | 769 | sprintf( output,"OK\r\n" ); |
Yo_Robot | 31:7e2cdd547cb2 | 770 | } |
Yo_Robot | 22:d5431fff164b | 771 | else |
Yo_Robot | 22:d5431fff164b | 772 | sprintf( output,"AL" ); |
Yo_Robot | 22:d5431fff164b | 773 | } |
Yo_Robot | 21:353b0fe8fc54 | 774 | |
Yo_Robot | 21:353b0fe8fc54 | 775 | |
Yo_Robot | 22:d5431fff164b | 776 | void setENC_eth( char * input, char * output ) |
Yo_Robot | 22:d5431fff164b | 777 | { |
Yo_Robot | 29:52932326c45a | 778 | encoder.Reset(QEI_RESET_POS); |
Yo_Robot | 29:52932326c45a | 779 | encoder.Reset(QEI_RESET_VEL); |
Yo_Robot | 22:d5431fff164b | 780 | |
Yo_Robot | 22:d5431fff164b | 781 | if( pin_alm == 0 ) |
Yo_Robot | 31:7e2cdd547cb2 | 782 | { |
Yo_Robot | 31:7e2cdd547cb2 | 783 | if ( limite_2 == 1 ) // Alarma muy cerca al encoder |
Yo_Robot | 31:7e2cdd547cb2 | 784 | { |
Yo_Robot | 31:7e2cdd547cb2 | 785 | sprintf( output,"A2\r\n" ); |
Yo_Robot | 31:7e2cdd547cb2 | 786 | } |
Yo_Robot | 31:7e2cdd547cb2 | 787 | if ( limite_3 == 1 ) // Alarma muy cerca al encoder |
Yo_Robot | 31:7e2cdd547cb2 | 788 | { |
Yo_Robot | 31:7e2cdd547cb2 | 789 | sprintf( output,"A1\r\n" ); |
Yo_Robot | 31:7e2cdd547cb2 | 790 | } |
Yo_Robot | 31:7e2cdd547cb2 | 791 | if ( limite_1 == 1 || limite_4 == 1 ) // ERROR DE POSICION |
Yo_Robot | 31:7e2cdd547cb2 | 792 | { |
Yo_Robot | 31:7e2cdd547cb2 | 793 | sprintf( output,"A0\r\n" ); |
Yo_Robot | 31:7e2cdd547cb2 | 794 | } |
Yo_Robot | 31:7e2cdd547cb2 | 795 | |
Yo_Robot | 31:7e2cdd547cb2 | 796 | else |
Yo_Robot | 31:7e2cdd547cb2 | 797 | sprintf( output,"OK\r\n" ); |
Yo_Robot | 31:7e2cdd547cb2 | 798 | } |
Yo_Robot | 22:d5431fff164b | 799 | else |
Yo_Robot | 22:d5431fff164b | 800 | sprintf( output,"AL" ); |
Yo_Robot | 22:d5431fff164b | 801 | |
Yo_Robot | 20:4b154134ab20 | 802 | } |
Yo_Robot | 12:c02b08dacc45 | 803 | |
Yo_Robot | 23:2126e38bb48c | 804 | void getRPM_eth( char * input, char * output ) |
Yo_Robot | 23:2126e38bb48c | 805 | { |
Yo_Robot | 29:52932326c45a | 806 | int rpm; |
Yo_Robot | 23:2126e38bb48c | 807 | |
Yo_Robot | 29:52932326c45a | 808 | rpm = encoder.CalculateRPM( encoder.GetVelocityCap(), 360); // ultima velocidad leida desde el encoder |
Yo_Robot | 29:52932326c45a | 809 | // numero de revoluciones por vuelta del encoder |
Yo_Robot | 29:52932326c45a | 810 | |
Yo_Robot | 23:2126e38bb48c | 811 | |
Yo_Robot | 23:2126e38bb48c | 812 | if( pin_alm == 0 ) |
Yo_Robot | 31:7e2cdd547cb2 | 813 | { |
Yo_Robot | 31:7e2cdd547cb2 | 814 | if ( limite_2 == 1 ) // Alarma muy cerca al encoder |
Yo_Robot | 31:7e2cdd547cb2 | 815 | { |
Yo_Robot | 31:7e2cdd547cb2 | 816 | sprintf( output,"A2\r\n" ); |
Yo_Robot | 31:7e2cdd547cb2 | 817 | } |
Yo_Robot | 31:7e2cdd547cb2 | 818 | if ( limite_3 == 1 ) // Alarma muy cerca al encoder |
Yo_Robot | 31:7e2cdd547cb2 | 819 | { |
Yo_Robot | 31:7e2cdd547cb2 | 820 | sprintf( output,"A1\r\n" ); |
Yo_Robot | 31:7e2cdd547cb2 | 821 | } |
Yo_Robot | 31:7e2cdd547cb2 | 822 | if ( limite_1 == 1 || limite_4 == 1 ) // ERROR DE POSICION |
Yo_Robot | 31:7e2cdd547cb2 | 823 | { |
Yo_Robot | 31:7e2cdd547cb2 | 824 | sprintf( output,"A0\r\n" ); |
Yo_Robot | 31:7e2cdd547cb2 | 825 | } |
Yo_Robot | 31:7e2cdd547cb2 | 826 | |
Yo_Robot | 31:7e2cdd547cb2 | 827 | else |
Yo_Robot | 31:7e2cdd547cb2 | 828 | sprintf( output,"OK\r\n" ); |
Yo_Robot | 31:7e2cdd547cb2 | 829 | } |
Yo_Robot | 23:2126e38bb48c | 830 | else |
Yo_Robot | 29:52932326c45a | 831 | sprintf( output,"AL" ); |
Yo_Robot | 23:2126e38bb48c | 832 | |
Yo_Robot | 23:2126e38bb48c | 833 | |
Yo_Robot | 23:2126e38bb48c | 834 | } |
Yo_Robot | 23:2126e38bb48c | 835 | |
Yo_Robot | 21:353b0fe8fc54 | 836 | |
Yo_Robot | 32:4483d6c225e5 | 837 | void setHOME_eth( char * input, char * output ) |
Yo_Robot | 32:4483d6c225e5 | 838 | { |
Yo_Robot | 32:4483d6c225e5 | 839 | int value = atoi( input ); |
Yo_Robot | 32:4483d6c225e5 | 840 | pin_dir = 1; //Mover hacia el motor |
Yo_Robot | 32:4483d6c225e5 | 841 | bandera_inicio = 1; // Informar a ISR_Adv_motor() acerca del Homing |
Yo_Robot | 32:4483d6c225e5 | 842 | setPTO( value * 1000 ); // a la velocidad seteada en KiloHertzios |
Yo_Robot | 32:4483d6c225e5 | 843 | |
Yo_Robot | 32:4483d6c225e5 | 844 | if( pin_alm == 0 ) |
Yo_Robot | 32:4483d6c225e5 | 845 | { |
Yo_Robot | 32:4483d6c225e5 | 846 | if ( limite_2 == 1 ) // Alarma muy cerca al encoder |
Yo_Robot | 32:4483d6c225e5 | 847 | { |
Yo_Robot | 32:4483d6c225e5 | 848 | sprintf( output,"A2\r\n" ); |
Yo_Robot | 32:4483d6c225e5 | 849 | } |
Yo_Robot | 32:4483d6c225e5 | 850 | |
Yo_Robot | 32:4483d6c225e5 | 851 | if ( limite_3 == 1 ) // Alarma muy cerca al encoder |
Yo_Robot | 32:4483d6c225e5 | 852 | { |
Yo_Robot | 32:4483d6c225e5 | 853 | if (bandera_inicio == 1) |
Yo_Robot | 32:4483d6c225e5 | 854 | { |
Yo_Robot | 32:4483d6c225e5 | 855 | sprintf( output,"IN\r\n" ); |
Yo_Robot | 32:4483d6c225e5 | 856 | } |
Yo_Robot | 32:4483d6c225e5 | 857 | sprintf( output,"A1\r\n" ); |
Yo_Robot | 32:4483d6c225e5 | 858 | } |
Yo_Robot | 32:4483d6c225e5 | 859 | if ( limite_1 == 1 || limite_4 == 1 ) // ERROR DE POSICION |
Yo_Robot | 32:4483d6c225e5 | 860 | { |
Yo_Robot | 32:4483d6c225e5 | 861 | sprintf( output,"A0\r\n" ); |
Yo_Robot | 32:4483d6c225e5 | 862 | } |
Yo_Robot | 32:4483d6c225e5 | 863 | |
Yo_Robot | 32:4483d6c225e5 | 864 | else |
Yo_Robot | 32:4483d6c225e5 | 865 | sprintf( output,"OK\r\n" ); |
Yo_Robot | 32:4483d6c225e5 | 866 | } |
Yo_Robot | 32:4483d6c225e5 | 867 | else |
Yo_Robot | 32:4483d6c225e5 | 868 | sprintf( output,"AL" ); |
Yo_Robot | 32:4483d6c225e5 | 869 | |
Yo_Robot | 32:4483d6c225e5 | 870 | |
Yo_Robot | 32:4483d6c225e5 | 871 | } |
Yo_Robot | 32:4483d6c225e5 | 872 | |
Yo_Robot | 34:bdf918bc9b59 | 873 | void getALM_eth ( char * input, char * output ) |
Yo_Robot | 34:bdf918bc9b59 | 874 | { |
Yo_Robot | 34:bdf918bc9b59 | 875 | |
Yo_Robot | 34:bdf918bc9b59 | 876 | if( pin_alm == 0 ) |
Yo_Robot | 34:bdf918bc9b59 | 877 | { |
Yo_Robot | 34:bdf918bc9b59 | 878 | if ( limite_2 == 1 ) // Alarma muy cerca al encoder |
Yo_Robot | 34:bdf918bc9b59 | 879 | { |
Yo_Robot | 34:bdf918bc9b59 | 880 | sprintf( output,"A2\r\n" ); |
Yo_Robot | 34:bdf918bc9b59 | 881 | } |
Yo_Robot | 34:bdf918bc9b59 | 882 | if ( limite_3 == 1 ) // Alarma muy cerca al encoder |
Yo_Robot | 34:bdf918bc9b59 | 883 | { |
Yo_Robot | 34:bdf918bc9b59 | 884 | sprintf( output,"A1\r\n" ); |
Yo_Robot | 34:bdf918bc9b59 | 885 | } |
Yo_Robot | 34:bdf918bc9b59 | 886 | if ( limite_1 == 1 || limite_4 == 1 ) // ERROR DE POSICION |
Yo_Robot | 34:bdf918bc9b59 | 887 | { |
Yo_Robot | 34:bdf918bc9b59 | 888 | sprintf( output,"A0\r\n" ); |
Yo_Robot | 34:bdf918bc9b59 | 889 | } |
Yo_Robot | 34:bdf918bc9b59 | 890 | |
Yo_Robot | 34:bdf918bc9b59 | 891 | else |
Yo_Robot | 34:bdf918bc9b59 | 892 | sprintf( output,"OK\r\n" ); |
Yo_Robot | 34:bdf918bc9b59 | 893 | } |
Yo_Robot | 34:bdf918bc9b59 | 894 | else |
Yo_Robot | 34:bdf918bc9b59 | 895 | sprintf( output,"AL" ); |
Yo_Robot | 34:bdf918bc9b59 | 896 | |
Yo_Robot | 34:bdf918bc9b59 | 897 | } |
Yo_Robot | 22:d5431fff164b | 898 | |
Yo_Robot | 22:d5431fff164b | 899 | |
Yo_Robot | 4:552beeda4722 | 900 | /* LEGACY FUNCTIONS |
Yo_Robot | 4:552beeda4722 | 901 | * |
Yo_Robot | 20:4b154134ab20 | 902 | * El siguiente codigo no es utilizado por el |
Yo_Robot | 20:4b154134ab20 | 903 | * programa. Sin embargo pueden servir como |
Yo_Robot | 20:4b154134ab20 | 904 | * futuras referencias. |
Yo_Robot | 4:552beeda4722 | 905 | */ |
Yo_Robot | 4:552beeda4722 | 906 | void setMR3( int newValue ) |
Yo_Robot | 4:552beeda4722 | 907 | { |
Yo_Robot | 4:552beeda4722 | 908 | LPC_TIM2->MR3 = newValue; |
Yo_Robot | 4:552beeda4722 | 909 | } |
Yo_Robot | 4:552beeda4722 | 910 | |
Yo_Robot | 4:552beeda4722 | 911 | |
Yo_Robot | 4:552beeda4722 | 912 | void setPrescaler( int newValue) |
Yo_Robot | 4:552beeda4722 | 913 | { |
Yo_Robot | 4:552beeda4722 | 914 | LPC_TIM2->PR = newValue; |
Yo_Robot | 4:552beeda4722 | 915 | } |
Yo_Robot | 20:4b154134ab20 | 916 | |
Yo_Robot | 20:4b154134ab20 | 917 | |
Yo_Robot | 20:4b154134ab20 | 918 | EthernetNetIf configurarEthernet() |
Yo_Robot | 20:4b154134ab20 | 919 | { |
Yo_Robot | 20:4b154134ab20 | 920 | //____________ *** ARCHIVO DE CONFIGURACION ***_______________________ // |
Yo_Robot | 20:4b154134ab20 | 921 | |
Yo_Robot | 20:4b154134ab20 | 922 | printf("\n *** CONFIGURACION ETHERNET DE MBED ***\n"); |
Yo_Robot | 20:4b154134ab20 | 923 | printf("Leyendo archivo de configuracion...\n\n"); |
Yo_Robot | 20:4b154134ab20 | 924 | |
Yo_Robot | 20:4b154134ab20 | 925 | FILE *fp = fopen("/local/config.txt", "r"); // Abre el archivo y lo guarda en fp |
Yo_Robot | 20:4b154134ab20 | 926 | |
Yo_Robot | 20:4b154134ab20 | 927 | if(!fp) // En caso de no encontrarse el archivo |
Yo_Robot | 20:4b154134ab20 | 928 | { |
Yo_Robot | 20:4b154134ab20 | 929 | printf("\nEl archivo /mbed/config.txt no puede ser abierto!\n"); |
Yo_Robot | 20:4b154134ab20 | 930 | exit(1); |
Yo_Robot | 20:4b154134ab20 | 931 | |
Yo_Robot | 20:4b154134ab20 | 932 | } |
Yo_Robot | 20:4b154134ab20 | 933 | else |
Yo_Robot | 20:4b154134ab20 | 934 | { |
Yo_Robot | 20:4b154134ab20 | 935 | // Cadenas de caracteres desde el Archivo config.txt |
Yo_Robot | 20:4b154134ab20 | 936 | char isDHCP [15]; //Modo Automatico o Manual |
Yo_Robot | 20:4b154134ab20 | 937 | char empty [2]; // Linea vacia |
Yo_Robot | 20:4b154134ab20 | 938 | char ip [40]; // Direccion IP |
Yo_Robot | 20:4b154134ab20 | 939 | char mask [40]; // Mascara de Subred |
Yo_Robot | 20:4b154134ab20 | 940 | char gate [40]; // Puerta de enlace |
Yo_Robot | 20:4b154134ab20 | 941 | char dns [40]; // Direccion DNS |
Yo_Robot | 20:4b154134ab20 | 942 | |
Yo_Robot | 20:4b154134ab20 | 943 | // Valores 'int' para agregar a la configuracion manual |
Yo_Robot | 20:4b154134ab20 | 944 | int DHCP; |
Yo_Robot | 20:4b154134ab20 | 945 | int n_ip[4]; |
Yo_Robot | 20:4b154134ab20 | 946 | int n_mask[4]; |
Yo_Robot | 20:4b154134ab20 | 947 | int n_gate[4]; |
Yo_Robot | 20:4b154134ab20 | 948 | int n_dns[4]; |
Yo_Robot | 20:4b154134ab20 | 949 | |
Yo_Robot | 20:4b154134ab20 | 950 | |
Yo_Robot | 20:4b154134ab20 | 951 | // Leer linea a linea el archivo |
Yo_Robot | 20:4b154134ab20 | 952 | fgets( isDHCP, 15, fp ); |
Yo_Robot | 20:4b154134ab20 | 953 | fgets( empty, 2, fp ); |
Yo_Robot | 20:4b154134ab20 | 954 | fgets( ip, 40, fp ); |
Yo_Robot | 20:4b154134ab20 | 955 | fgets( mask, 40, fp ); |
Yo_Robot | 20:4b154134ab20 | 956 | fgets( gate, 40, fp ); |
Yo_Robot | 20:4b154134ab20 | 957 | fgets( dns, 40, fp ); |
Yo_Robot | 20:4b154134ab20 | 958 | |
Yo_Robot | 20:4b154134ab20 | 959 | printf("Cerrando archivo...\n"); |
Yo_Robot | 20:4b154134ab20 | 960 | fclose(fp); |
Yo_Robot | 20:4b154134ab20 | 961 | |
Yo_Robot | 20:4b154134ab20 | 962 | // Extraer los valores numericos |
Yo_Robot | 20:4b154134ab20 | 963 | sscanf( isDHCP,"%*s %d",&DHCP ); |
Yo_Robot | 20:4b154134ab20 | 964 | |
Yo_Robot | 20:4b154134ab20 | 965 | sscanf( ip,"%*s %d.%d.%d.%d",&n_ip[0],&n_ip[1],&n_ip[2],&n_ip[3] ); |
Yo_Robot | 20:4b154134ab20 | 966 | sscanf( mask,"%*s %d.%d.%d.%d",&n_mask[0],&n_mask[1],&n_mask[2],&n_mask[3] ); |
Yo_Robot | 20:4b154134ab20 | 967 | sscanf( gate,"%*s %d.%d.%d.%d",&n_gate[0],&n_gate[1],&n_gate[2],&n_gate[3] ); |
Yo_Robot | 20:4b154134ab20 | 968 | sscanf( dns,"%*s %d.%d.%d.%d",&n_dns[0],&n_dns[1],&n_dns[2],&n_dns[3] ); |
Yo_Robot | 20:4b154134ab20 | 969 | |
Yo_Robot | 20:4b154134ab20 | 970 | |
Yo_Robot | 20:4b154134ab20 | 971 | if(DHCP) //En caso de modo DHCP |
Yo_Robot | 20:4b154134ab20 | 972 | { |
Yo_Robot | 20:4b154134ab20 | 973 | printf("\n Configurar red de manera automatica\n"); |
Yo_Robot | 20:4b154134ab20 | 974 | EthernetNetIf eth; //Crea la interfaz |
Yo_Robot | 20:4b154134ab20 | 975 | EthernetErr ethErr = eth.setup(); |
Yo_Robot | 20:4b154134ab20 | 976 | if( ethErr ) |
Yo_Robot | 20:4b154134ab20 | 977 | { |
Yo_Robot | 20:4b154134ab20 | 978 | printf( "Error %d en la configuracion\n", ethErr ); |
Yo_Robot | 20:4b154134ab20 | 979 | exit(1); |
Yo_Robot | 20:4b154134ab20 | 980 | } |
Yo_Robot | 20:4b154134ab20 | 981 | |
Yo_Robot | 20:4b154134ab20 | 982 | printf("Configuracion Correcta\n\n"); |
Yo_Robot | 20:4b154134ab20 | 983 | return eth; |
Yo_Robot | 20:4b154134ab20 | 984 | } |
Yo_Robot | 20:4b154134ab20 | 985 | else |
Yo_Robot | 20:4b154134ab20 | 986 | { |
Yo_Robot | 20:4b154134ab20 | 987 | sscanf( ip,"%*s %d.%d.%d.%d",&n_ip[0], &n_ip[1], &n_ip[2], &n_ip[3] ); |
Yo_Robot | 20:4b154134ab20 | 988 | sscanf( mask,"%*s %d.%d.%d.%d",&n_mask[0],&n_mask[1],&n_mask[2],&n_mask[3] ); |
Yo_Robot | 20:4b154134ab20 | 989 | sscanf( gate,"%*s %d.%d.%d.%d",&n_gate[0],&n_gate[1],&n_gate[2],&n_gate[3] ); |
Yo_Robot | 20:4b154134ab20 | 990 | sscanf( dns,"%*s %d.%d.%d.%d",&n_dns[0], &n_dns[1], &n_dns[2], &n_dns[3] ); |
Yo_Robot | 20:4b154134ab20 | 991 | printf(" %s %s %s %s %s\n " , isDHCP, ip, mask, gate, dns ); |
Yo_Robot | 20:4b154134ab20 | 992 | |
Yo_Robot | 20:4b154134ab20 | 993 | printf("\n Configurar red de manera manual\n"); |
Yo_Robot | 20:4b154134ab20 | 994 | EthernetNetIf eth( |
Yo_Robot | 20:4b154134ab20 | 995 | IpAddr( n_ip[0], n_ip[1], n_ip[2], n_ip[3]), //IP Address |
Yo_Robot | 20:4b154134ab20 | 996 | IpAddr( n_mask[0],n_mask[1],n_mask[2],n_mask[3]), //Network Mask |
Yo_Robot | 20:4b154134ab20 | 997 | IpAddr( n_gate[0],n_gate[1],n_gate[2],n_gate[3]), //Gateway |
Yo_Robot | 20:4b154134ab20 | 998 | IpAddr( n_dns[0], n_dns[1], n_dns[2], n_dns[3] ) //DNS |
Yo_Robot | 20:4b154134ab20 | 999 | ); |
Yo_Robot | 20:4b154134ab20 | 1000 | |
Yo_Robot | 20:4b154134ab20 | 1001 | EthernetErr ethErr = eth.setup(); |
Yo_Robot | 20:4b154134ab20 | 1002 | if( ethErr ) |
Yo_Robot | 20:4b154134ab20 | 1003 | { |
Yo_Robot | 20:4b154134ab20 | 1004 | printf( "Error %d en la configuracion\n", ethErr ); |
Yo_Robot | 20:4b154134ab20 | 1005 | exit(1); |
Yo_Robot | 20:4b154134ab20 | 1006 | } |
Yo_Robot | 20:4b154134ab20 | 1007 | |
Yo_Robot | 20:4b154134ab20 | 1008 | printf("Configuracion Correcta\n\n"); |
Yo_Robot | 20:4b154134ab20 | 1009 | return eth; |
Yo_Robot | 20:4b154134ab20 | 1010 | } |
Yo_Robot | 20:4b154134ab20 | 1011 | |
Yo_Robot | 20:4b154134ab20 | 1012 | } |
Yo_Robot | 20:4b154134ab20 | 1013 | |
Yo_Robot | 20:4b154134ab20 | 1014 | EthernetNetIf eth; |
Yo_Robot | 20:4b154134ab20 | 1015 | return eth; |
Yo_Robot | 20:4b154134ab20 | 1016 | } |