main.cpp

00001 /*
00002  * Código por: Laura Álvila
00003  *             Daniela López
00004  *             Nicolás Villegas            
00005  */
00006  
00007 #include "mbed.h"
00008 #include "QEI.h"
00009 #include "TextLCD.h"
00010 
00011 Serial GSM(PTE0,PTE1);  // Módulo Bluetooh
00012 TextLCD lcd(PTB10, PTB11, PTE2, PTE3, PTE4, PTE5); // rs, e, d4-d7
00013 QEI encoder (PTA13, PTD5, NC, 10);
00014 Timer t;
00015 
00016 AnalogIn y(PTC2);   // Entrada análoga: Salida del sistema
00017 AnalogOut u(PTE30); // Salida análoga: Señal de control
00018 
00019 DigitalOut led1(LED1);
00020 DigitalOut led2(LED2);
00021 DigitalOut led3(LED3);
00022 
00023 DigitalIn button3(PTC16); // Botón del encoder
00024 DigitalIn button4(PTC17); // Pulsador
00025 
00026 int C1=0x0F;
00027 int C2=0x18;
00028 int C3=0x1A;
00029 int C4=0x0C;
00030 
00031 int cambio = 0, diferencia = 0;
00032 float pid, o, ai, ad, ap, med, err, setpoint;
00033 float err_v;
00034 int spnum = 0, kinum = 0, kpnum = 0 ,kdnum = 0, pos = 1;
00035 int j,k; 
00036 long t_pid = 0, t_btn = 0;
00037 
00038 int main() {
00039     
00040     // Se asigna baudrate y se configura el puerto serie de la USART
00041     
00042     GSM.baud(9600);   
00043     GSM.format(8,Serial::None,1);
00044     t.start();
00045     
00046     
00047     // Animación de bienvenida:
00048     
00049     for (int i = 3; i < 15; ++i) {
00050         lcd.cls();
00051         lcd.locate(0,0);
00052         lcd.printf("**Control PID**");
00053         wait_ms(1000/i);
00054         
00055         ++i;
00056         lcd.cls();
00057         lcd.locate(1,0);
00058         lcd.printf("**Control PID**");
00059         wait_ms(1000/i);
00060         
00061         ++i;
00062         lcd.cls();
00063         lcd.locate(1,1);
00064         lcd.printf("**Control PID**");
00065         wait_ms(1000/i);
00066         
00067         ++i;
00068         lcd.cls();
00069         lcd.locate(0,1);
00070         lcd.printf("**Control PID**");
00071         wait_ms(1000/i);
00072     }
00073 
00074 init:
00075     
00076     lcd.cls();
00077     lcd.writeCommand(C1);
00078       
00079     lcd.locate(8,0);
00080     lcd.printf("Ki=%d", kinum);
00081     
00082     lcd.locate(0,1);
00083     lcd.printf("Kd=%d", kdnum);
00084     
00085     lcd.locate(8,1);
00086     lcd.printf("**PID**");
00087     
00088     lcd.locate(0,0);
00089     lcd.printf("Kp=%d", kpnum);
00090     
00091     bool state = 1; // Esta variable determina si se están modificando las constantes del controlador o el Set-Point.
00092     
00093     while(true) {
00094 
00095         diferencia=(encoder.getPulses() - cambio)*3;
00096         cambio=encoder.getPulses();
00097         
00098         if (diferencia != 0 && state) {
00099             switch (pos) {
00100                 case 1:
00101                     kpnum += diferencia;
00102                     
00103                     if (kpnum >= 999)
00104                         kpnum = 999;
00105                     else if (kpnum < 0)
00106                         kpnum = 0;
00107                         
00108                     lcd.locate(3,0);
00109                     lcd.printf("    ");
00110                     lcd.locate(3,0);
00111                     lcd.printf("%d", kpnum);
00112                     break;
00113                 case 2:
00114                     kinum += diferencia;
00115                     
00116                     if (kinum >= 999)
00117                         kinum = 999;
00118                     else if (kinum < 0)
00119                         kinum = 0;
00120 
00121                     lcd.locate(11,0);
00122                     lcd.printf("    ");
00123                     lcd.locate(11,0);
00124                     lcd.printf("%d", kinum);
00125                     break;
00126                 case 3:
00127                     kdnum += diferencia;
00128                     
00129                     if (kdnum >= 999)
00130                         kdnum = 999;
00131                     else if (kdnum < 0)
00132                         kdnum = 0;
00133 
00134                     lcd.locate(3,1);
00135                     lcd.printf("    ");
00136                     lcd.locate(3,1);
00137                     lcd.printf("%d", kdnum);
00138                     break;
00139             }
00140         }
00141         
00142         if (diferencia != 0 && !state) {
00143             
00144             spnum += diferencia;
00145               
00146             if (spnum >= 999) spnum = 999;
00147             else if (spnum < 0) spnum = 0;
00148                 
00149             lcd.locate(10,0);
00150             lcd.printf("   ");
00151             lcd.locate(10,0);
00152             lcd.printf("%d", spnum);
00153         }
00154             
00155 
00156         if (!button3) { // Cambia la posición de ingreso de parámetros
00157             led2 = !led2;
00158             switch(pos++) {
00159                 case 3:
00160                     pos = 1;
00161                     lcd.locate(3,0);
00162                     lcd.printf("%d", kpnum);
00163                     break;
00164                 case 1:
00165                     lcd.locate(11,0);
00166                     lcd.printf("%d", kinum);
00167                     break;
00168                 case 2:
00169                     lcd.locate(3,1);
00170                     lcd.printf("%d", kdnum);
00171                     break;
00172             }
00173             wait(0.2);
00174         }
00175 
00176         if (!button4 && state) {
00177             lcd.cls();
00178             lcd.locate(8,1);
00179             lcd.printf("**PID**");
00180             lcd.locate(0,0);
00181             lcd.printf("Set-Point=");
00182             lcd.locate(10,0);
00183             lcd.printf("%d", spnum);
00184             wait(0.2);
00185             state = 0;
00186         }
00187         else if (!button4 && !state) {
00188             wait(0.2);
00189             break;
00190         }
00191         wait(0.1);        
00192     }
00193     
00194 
00195  // Transición
00196     lcd.writeCommand(C4); // Escribimos un comando segun el manual del módulo LCD para quitar cursor bajo
00197     lcd.cls();
00198     lcd.locate(0,0); lcd.printf("y=");
00199     lcd.locate(8,0); lcd.printf("e=");
00200     lcd.locate(0,1); lcd.printf("r=");
00201     lcd.locate(8,1); lcd.printf("u=");
00202 
00203 loop:   
00204 
00205     if (t.read_ms() - t_pid > 10) {
00206         
00207         med = y.read()*999;
00208         err = spnum - med;              // Se calcula el error
00209         ap = kpnum*err*0.01f;           // Se calcula la acción proporcional    
00210         ai += kinum*err*0.01f;          // Cálculo de la integral del error
00211         ad = kdnum*(err - err_v)*0.01f; // Cálculo de la acción derivativa
00212         pid = ap + ai + ad;
00213            
00214         if (pid <= 0) pid = 0;
00215         if (pid > 999) pid = 999;
00216         
00217         setpoint = spnum;
00218         GSM.printf("#%3.0f%3.0f&", med, setpoint);   // Se envía una cadena de caracteres por el puerto serial. Se agregan identificadores #...& que denotan el comienzo y la terminación de la misma.
00219                                                      // Esto se hace con el fin de evitar errores en la transmisión. Estos identificadores se usan en la aplicación de AppInventor. 
00220         lcd.locate(3,0);  lcd.printf("%3.0f ", med);
00221         lcd.locate(3,1);  lcd.printf("%3d ", spnum);
00222         lcd.locate(11,0); lcd.printf("%3.0f ", err);
00223         lcd.locate(11,1); lcd.printf("%3.0f ", pid);
00224 
00225         err_v = err;
00226         o = pid/999;
00227         u.write(o);
00228         
00229         t_pid = t.read_ms();
00230     }
00231     else {
00232         
00233         diferencia=(encoder.getPulses() - cambio)*6;  // Es posible cambiar el Set-Point cuando el controlador ya está funcionando. 
00234         cambio=encoder.getPulses();
00235         
00236         if (diferencia != 0) {
00237             spnum += diferencia;
00238             if (spnum >= 999) spnum = 999;
00239             else if (spnum < 0) spnum = 0;
00240         }
00241             
00242     }
00243     
00244     if (!button4) { // Si se presiona el pulsador se vuelve al menú inicial para configurar de nuevo el controlador.
00245         u.write(0);
00246         med = 0;
00247         err = 0;
00248         pid = 0;
00249         
00250         goto init;
00251     }
00252     else
00253         goto loop;
00254         
00255 }