Ernesto Palacios / Mbed 2 deprecated DeslizadorUTN

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@31:7e2cdd547cb2, 2014-05-03 (annotated)

Committer:
Yo_Robot
Date:
Sat May 03 00:14:17 2014 +0000
Revision:
31:7e2cdd547cb2
Parent:
30:413d1a6648b5
Child:
32:4483d6c225e5
ARREGLAR ALARMA:;    - Mensaje de tiempo calculado con -P; ; Falta agregar nueva Funcion:  Home;    Regresa el carro hasta 0; ; Encoder Ok: Resoluci?n 720 ppr

Who changed what in which revision?

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