main.cpp

00001 #include "mbed.h"
00002 #include <stdio.h>
00003 #include <string.h>
00004 #include <stdlib.h>
00005 
00006 #define BT_RX PTE1
00007 #define BT_TX PTE0
00008 
00009 /*Alvaro Javier Vargas Miranda
00010 Guillermo Luna Rozo
00011 Santiago Yepes Montoya
00012 Juan David Cordoba Giraldo
00013 Susana Buriticá Mejía*/
00014 
00015 DigitalOut lRed(LED1);
00016 DigitalOut lGrn(LED2);
00017 DigitalOut lBlu(LED3);
00018 
00019 AnalogIn AnInput(PTB0);
00020 AnalogOut  AnOut(PTE30);
00021 PwmOut pwmOut(PTE20);
00022 
00023 Serial PC(USBTX, USBRX); // tx, rx
00024 Serial BT(BT_TX, BT_RX); // Serial para el Bluetooth
00025 
00026 /* Variables Globales */
00027 // Parametros del PID sintonizado
00028 float cKp=1.432f;
00029 float cKi=25.95f;
00030 float cKd=0.0f;
00031 float cRf=1.0f;
00032 float out,ai,ad,ap,med,err,err_v,uPID;
00033 int Ts=250;
00034 bool continuo = true;
00035 
00036 char buffer[5]; //Buffer para almacenar los msjs del usuario
00037 char dbg[100]="";
00038 
00039 void cleanBuffer(char *buffer)
00040 {
00041     for(int i=0; i< 5; i++) {
00042         buffer[i]='\0';
00043     }
00044 }
00045 
00046 void printMenuBT(void)
00047 {
00048     char Menu1[] =   "|********** CONTROLADOR PID ***********|\n";
00049     char Menu2[] =   "|*                                    *|\n";
00050     char Menu3[] =   "|( P ) -- Ingresar parametros del PID *|\n";
00051     char Menu4[] =   "|( I ) -- PID con DCA                 *|\n";
00052     char Menu5[] =   "|( W ) -- PID con PWM                 *|\n";
00053     char Menu6[] =   "|( S ) -- Apagar la salida            *|\n";
00054     char Menu7[] =   "|*                                    *|\n";
00055     char Menu8[] =   "|**************************************|\n";
00056 
00057     //Impresion del Menu
00058 
00059     BT.printf("\n%s",Menu1);
00060     BT.printf("%s",Menu2);
00061     BT.printf("%s",Menu3);
00062     BT.printf("%s",Menu4);
00063     BT.printf("%s",Menu5);
00064     BT.printf("%s",Menu6);
00065     BT.printf("%s",Menu7);
00066     BT.printf("%s",Menu8);
00067 }
00068 
00069 void enterPIDParameters(void)
00070 {   
00071     bool ready=false;
00072     float kp,ki,kd;
00073     float ref;
00074     char ans[5];
00075 
00076     BT.printf("Ingrese los parametros del controlador.\n\r");
00077     while(ready == false) 
00078     {
00079         BT.printf("PID valores actuales\nKp=%1.4f ki=%1.4f kd=%1.4f Ref=%1.4f\n",cKp,cKi,cKd,cRf);
00080         BT.printf("Kp >> ");
00081         BT.scanf("%f",&kp);
00082         BT.printf("Ki >> ");
00083         BT.scanf("%f",&ki);
00084         BT.printf("Kd >> ");
00085         BT.scanf("%f",&kd);
00086         BT.printf("Ref [0 - 3.3]>> ");
00087         BT.scanf("%f",&ref);
00088 
00089         BT.printf("Nuevos Valores\nKp=%1.4f Ki=%1.4f Kd=%1.4f Ref=%1.4f\n",kp,ki,kd,ref);
00090         BT.printf("Desea continuar? (S/n), (Q) para salir ");
00091         BT.scanf("%s",ans);
00092         PC.printf("Llego %s \n",ans);
00093         
00094         if(strncmp(ans,"S",1)==0 || strncmp(ans,"s",1)==0 || strncmp(ans,"q",1)==0 ||strncmp(ans,"Q",1)==0) {
00095             PC.printf("Entre\n");
00096             ready = true;
00097         }
00098     }
00099     if(strncmp(ans,"S",1)==0 || strncmp(ans,"s",1)==0){
00100         cKp = kp;
00101         cKi = ki;
00102         cKd = kd;
00103         cRf = ref;
00104         lRed=0;
00105         lGrn=0;
00106         BT.printf("PID valores\nKp=%1.2f ki=%1.2f kd=%1.2f Ref=%1.2f\n",cKp,cKi,cKd,cRf);
00107     }
00108     
00109 }
00110 
00111 // CICLO PRINCIPAL CONTROLADOR PID
00112 
00113 void controlPID(){
00114     med = AnInput.read()*999;   //Medicion de la salida
00115     err = cRf*(999/3.3f) - med; //calculo del error
00116     ap = err*cKp*0.01f;         //Calculo accion proporcional
00117     ai = ai+(err*cKi*0.01f);    //Calculo accion integral
00118     ad = (err-err_v)*(cKd)*0.01f;//Calculo   de la accion derivativa 
00119     uPID=ap+ai+ad;              //Calculo de la accion de control
00120     if(uPID<=0){uPID=0;}        //Saturador inferior
00121     if(uPID>=999){uPID=999;}    //Saturador superior
00122     err_v = err;                //Actualizacion de error
00123     out=uPID*(3.3f/999);        //Conversion de voltaje a accion de control
00124     AnOut.write(out);           //Escritura de accion de control
00125     BT.printf("Ref=%fv Error=%fv Ucontrol=%fv\n\r",cRf,err/999,out);
00126 }
00127 
00128 void controlPIDpwm(){
00129     med = AnInput.read()*999;   //Medicion de la salida
00130     err = cRf*(999/3.3f) - med; //calculo del error
00131     ap = err*cKp*0.01f;         //Calculo accion proporcional
00132     ai = ai+(err*cKi*0.01f);    //Calculo accion integral
00133     ad = (err-err_v)*(cKd)*0.01f;//Calculo   de la accion derivativa 
00134     uPID=ap+ai+ad;              //Calculo de la accion de control
00135     if(uPID<=0){uPID=0;}        //Saturador inferior
00136     if(uPID>=999){uPID=999;}    //Saturador superior
00137     err_v = err;                //Actualizacion de error
00138     out=uPID*(3.3f/999);        //Conversion de voltaje a accion de control
00139     pwmOut.write(out);           //Escritura de accion de control
00140     BT.printf("Ref=%fv Error=%fv Ucontrol=%fv\n\r",cRf,err/999,out);
00141 }
00142 
00143 
00144 void LeerBuffer(char *buffer)
00145 {
00146     int i= 0;
00147     while (BT.readable()) {
00148         char c = BT.getc();
00149         if (c == '\r' || c == '\n' || c == '\t') c = '$';//si se envia fin de linea o de caracter inserta $
00150         buffer[i++] = c;//mete al bufer el caracter leido
00151     }
00152 }
00153 
00154 
00155 void executePIDControl()
00156 {
00157     PC.printf("Entre al PID\n");
00158     //Capturamos los parametros del controlador
00159     bool run=true;
00160     out=0;ai=0;ad=0;ap=0;med=0;err=0;err_v=0;uPID=0;
00161     while(run == true) {
00162         lRed=1;
00163         lGrn=0;
00164         if(continuo){controlPID();}
00165         else{controlPIDpwm();}
00166         wait_ms(Ts);
00167         LeerBuffer(buffer);
00168         if(buffer[0]=='d' || buffer[0]=='D' || buffer[1]=='d' || buffer[1]=='D') {
00169             run = false;
00170         }
00171         cleanBuffer(buffer);
00172     }
00173 }
00174 
00175 
00176 int main()
00177 {
00178     BT.baud(9600);
00179     AnOut.write(0.0f);
00180     pwmOut.write(0.0f);
00181     lRed = 1;
00182     lBlu = 1;
00183     for(int i = 0; i<3; i++) {
00184         /* Parpadeo de LEDS para indicar el inicio*/
00185         lGrn = 0;
00186         wait(0.25);
00187         lGrn = 1;
00188         wait(0.25);
00189     }
00190     PC.printf("\nProgram Start!\n");
00191 
00192     /*Led en naranja para indicar que esta listo para recibir parametros*/
00193     lRed = 0;
00194     lGrn = 1;
00195     enterPIDParameters();
00196 
00197     while(1) {
00198         printMenuBT();
00199         BT.printf("\nIngrese una opcion >> ");
00200         BT.scanf("%s",buffer);
00201         PC.printf("\n Se recibio: %s \n",buffer);
00202 
00203         char msg[10];
00204         int mIdx = 0;
00205         for (int m = 0; m < 10; m++) {
00206             if (buffer[m] != '\n' && buffer[m] != '\r' && buffer[m] != ' ') {
00207                 msg[mIdx] = buffer[m]; //Guardar los caracteres validos en el mensaje
00208                 mIdx++;  //Corremos el indice del array de chars
00209             }
00210         }
00211         
00212         PC.printf("Msg[1] es <%s> \n",msg);
00213         if(strncmp(msg,"P",1)==0 || strncmp(msg,"p",1)==0){
00214             enterPIDParameters();
00215             BT.printf("Se han ingreso los parametros del PID\n"); 
00216         }
00217         else if(strncmp(msg,"i",1)==0 || strncmp(msg,"I",1)==0){
00218             PC.printf("Llame al PID continuo\n");
00219             continuo=true;
00220             pwmOut.write(0.0f);
00221             executePIDControl();
00222             lRed=0;
00223             lGrn=0;
00224             //BT.printf("Se han ingreso los parametros del PID\n"); 
00225         }
00226         else if(strncmp(msg,"w",1)==0 || strncmp(msg,"W",1)==0){
00227             PC.printf("Llame al PID pwm\n");
00228             continuo=false;
00229             AnOut.write(0.0f);
00230             executePIDControl();
00231             lRed=0;
00232             lGrn=0;
00233             //BT.printf("Se han ingreso los parametros del PID\n"); 
00234         }
00235         else if(strncmp(msg,"s",1)==0 || strncmp(msg,"S",1)==0){
00236             PC.printf("Salida en Bajo\n");
00237             lRed=0;
00238             lGrn=1;
00239             pwmOut.write(0.0f);
00240             AnOut.write(0.0f);
00241 
00242             //BT.printf("Se han ingreso los parametros del PID\n"); 
00243         }
00244         else{
00245             BT.printf("El parametro ingresado no es correcto\n");   
00246         }
00247     }
00248 }