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, 1000);
00014 
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 // Códigos movimiento del cursor:
00027 int C1=0x0F;
00028 int C2=0x18; // desplaza izquierda
00029 int C3=0x1A; // desplaza derecha
00030 int C4=0x0C; // quito cursor bajo
00031 
00032 int cambio = 0, diferencia = 0;
00033 float pid, o, ai, ad, ap, med, err;
00034 float err_v;
00035 int spnum = 0, kinum = 0, kpnum = 0 ,kdnum = 0, pos = 1;
00036 int j,k; 
00037 
00038 int main() {
00039     // Se asigna baudrate y se configura el puerto serie de la USART
00040     GSM.baud(9600);   
00041     GSM.format(8,Serial::None,1);
00042     
00043     lcd.locate(0,1);
00044     lcd.printf("**Control PID**");
00045     wait(3);
00046     
00047     lcd.cls();              // Borrar Pantalla
00048     lcd.writeCommand(C1);   // Escribimos un comando segun el manual del modulo LCD
00049 
00050     lcd.locate(8,0);
00051     lcd.printf("Kp=%d", kpnum);
00052     lcd.locate(0,1);
00053     lcd.printf("Ki=%d", kinum);
00054     lcd.locate(8,1);
00055     lcd.printf("Kd=%d", kdnum);
00056     lcd.locate(0,0);
00057     lcd.printf("Sp=%d", spnum);
00058 
00059     while(true) {
00060 
00061         diferencia=encoder.getPulses() - cambio;
00062         cambio=encoder.getPulses();
00063         
00064         if (diferencia != 0) {
00065             switch (pos) {
00066                 case 1:
00067                     spnum += diferencia;
00068                     
00069                     if (spnum >= 999)
00070                         spnum = 999;
00071                     else if (spnum < 0)
00072                         spnum = 0;
00073                         
00074                     lcd.locate(3,0);
00075                     lcd.printf("    ");
00076                     lcd.locate(3,0);
00077                     lcd.printf("%d", spnum);
00078                     break;
00079                 case 2:
00080                     kpnum += diferencia;
00081                     
00082                     if (kpnum >= 999)
00083                         kpnum = 999;
00084                     else if (kpnum < 0)
00085                         kpnum = 0;
00086                     lcd.locate(11,0);
00087                     lcd.printf("    ");
00088                     lcd.locate(11,0);
00089                     lcd.printf("%d", kpnum);
00090                     break;
00091                 case 3:
00092                     kinum += diferencia;
00093                     
00094                     if (kinum >= 999)
00095                         kinum = 999;
00096                     else if (kinum < 0)
00097                         kinum = 0;
00098 
00099                     lcd.locate(3,1);
00100                     lcd.printf("    ");
00101                     lcd.locate(3,1);
00102                     lcd.printf("%d", kinum);
00103                     break;
00104                 case 4:
00105                     kdnum += diferencia;
00106                     
00107                     if (kdnum >= 999)
00108                         kdnum = 999;
00109                     else if (kdnum < 0)
00110                         kdnum = 0;
00111 
00112                     lcd.locate(11,1);
00113                     lcd.printf("    ");
00114                     lcd.locate(11,1);
00115                     lcd.printf("%d", kdnum);
00116                     break;
00117                 default:
00118                     break;
00119             } // Fin switch
00120         } // Fin if
00121 
00122         if (!button3) { // Cambia la posición de ingreso de parámetros
00123         
00124             led3 = !led3;
00125             
00126             switch(pos++) {
00127                 case 4:
00128                     pos = 1;
00129                     lcd.locate(3,0);
00130                     lcd.printf("%d", spnum);
00131                     break;
00132                 case 1:
00133                     lcd.locate(11,0);
00134                     lcd.printf("%d", kpnum);
00135                     break;
00136                 case 2:
00137                     lcd.locate(3,1);
00138                     lcd.printf("%d", kinum);
00139                     break;
00140                 case 3:
00141                     lcd.locate(11,1);
00142                     lcd.printf("%d", kdnum);
00143                     break;
00144                 default:
00145                     break;
00146             } // Fin switch
00147             wait(0.25);
00148         } // Fin if
00149               
00150 
00151         if (!button4) {
00152             break;     //sale del bucle si pisan suiche4
00153         }
00154         wait(0.1);        
00155     }
00156 
00157 
00158 // Transición
00159     lcd.writeCommand(C4); // Escribimos un comando segun el manual del módulo LCD para quitar cursor bajo
00160     lcd.cls(); // Borra la pantalla
00161     lcd.printf("   GUARDADOS!");
00162     wait(1);
00163     lcd.cls();
00164     lcd.printf("  *** PID ***");
00165     wait(1);
00166 
00167     lcd.cls();
00168     lcd.printf("e=%3.0f",err);
00169     lcd.locate(8,0);
00170     lcd.printf("y=%3.0f",med);
00171     lcd.locate(0,1);
00172     lcd.printf("r=%3.0f",spnum);
00173     lcd.locate(8,1);
00174     lcd.printf("u=%3.0f",pid);
00175     wait(1);
00176 
00177 int referencia;
00178 int salida;
00179 
00180 // CICLO PRINCIPAL CONTROLADOR PID
00181 loop:   med = y.read()*999;
00182         err = spnum - med;      // Se calcula el error
00183         ap = kpnum*err*0.01f;   // Se calcula la acción proporcional
00184         ai += kinum*err*0.01f;  // Cálculo de la integral del error
00185         ad = kdnum*(err - err_v)*0.01f; // Cálculo de la acción derivativa
00186         pid = ap + ai + ad;
00187         
00188         referencia = (spnum / 999) * 255;
00189         salida = med / 999 * 743 + 256;
00190         
00191         if (referencia < 256) {        // Debo generar dos casos a APP inventor solo me recibe hex asi: 0xhhhh (4 cifras)    
00192             GSM.putc(0);         // si el numero es hasta 255 se le ponen dos ceros adelante a la secuencia de bits
00193             GSM.putc(referencia);       // luego la cifra menos significativa
00194         }
00195                 /*  
00196         
00197         if (salida > 255){         // pero si es mayor a 255 las cifras deben ser convertidas a un hex de dos bytes de la siguiente forma   
00198            j = salida/256;         // calculo la cifra mas significativa
00199            k = salida - j*256;     // calculo la cifra menos significativa
00200            GSM.putc(j);         // las envio a la usart para que se las ponga al modulo bluetooth y la lleve al android
00201            GSM.putc(k);         // mas significativa primero, menos despues si no no funciona!!! y con la orden PUTC solo asi le envia binarios
00202        }
00203        
00204  
00205         GSM.putc('r');          // Se envía el set-point al módulo Bluetooth.
00206         if (spnum < 256) {    
00207            GSM.putc(0);
00208            GSM.putc(spnum);
00209         }
00210         if (spnum > 255){  
00211            j = spnum/256;
00212            k = spnum - j*256;
00213            GSM.putc(j);
00214            GSM.putc(k);
00215        }
00216        */
00217        
00218         // Se verifica que pid sea positivo:
00219         if (pid <= 0)
00220             pid = 0;
00221         else if (pid > 999)
00222             pid = 999;
00223 
00224        
00225         // Se muestran las variables en LCD:
00226         lcd.locate(3,0);
00227         lcd.printf("    ");
00228         lcd.locate(3,0);
00229         lcd.printf("%3.0f",err);
00230         lcd.locate(11,0);
00231         lcd.printf("   ");
00232         lcd.locate(11,0);
00233         lcd.printf("%3.0f",med);
00234         lcd.locate(3,1);
00235         lcd.printf("   ");
00236         lcd.locate(3,1);
00237         lcd.printf("%d",spnum);
00238         lcd.locate(11,1);
00239         lcd.printf("   ");
00240         lcd.locate(11,1);
00241         lcd.printf("%3.0f",pid);
00242            
00243         // Normalización de la salida y actualización de variables:
00244         err_v = err;
00245         o = pid/999;
00246         u.write(o);
00247         
00248         // Se repite el ciclo:
00249         wait_ms(300);
00250         goto loop;
00251 }