David Cordoba
/
P2_ControlPID_Proc20191
Entrega numero 2 del curso procesadores 2019-2
main.cpp
- Committer:
- diavad
- Date:
- 2019-06-14
- Revision:
- 0:55be7cc4ef10
File content as of revision 0:55be7cc4ef10:
#include "mbed.h" #include <stdio.h> #include <string.h> #include <stdlib.h> #define BT_RX PTE1 #define BT_TX PTE0 /*Alvaro Javier Vargas Miranda Guillermo Luna Rozo Santiago Yepes Montoya Juan David Cordoba Giraldo Susana Buriticá Mejía*/ DigitalOut lRed(LED1); DigitalOut lGrn(LED2); DigitalOut lBlu(LED3); AnalogIn AnInput(PTB0); AnalogOut AnOut(PTE30); PwmOut pwmOut(PTE20); Serial PC(USBTX, USBRX); // tx, rx Serial BT(BT_TX, BT_RX); // Serial para el Bluetooth /* Variables Globales */ // Parametros del PID sintonizado float cKp=1.432f; float cKi=25.95f; float cKd=0.0f; float cRf=1.0f; float out,ai,ad,ap,med,err,err_v,uPID; int Ts=250; bool continuo = true; char buffer[5]; //Buffer para almacenar los msjs del usuario char dbg[100]=""; void cleanBuffer(char *buffer) { for(int i=0; i< 5; i++) { buffer[i]='\0'; } } void printMenuBT(void) { char Menu1[] = "|********** CONTROLADOR PID ***********|\n"; char Menu2[] = "|* *|\n"; char Menu3[] = "|( P ) -- Ingresar parametros del PID *|\n"; char Menu4[] = "|( I ) -- PID con DCA *|\n"; char Menu5[] = "|( W ) -- PID con PWM *|\n"; char Menu6[] = "|( S ) -- Apagar la salida *|\n"; char Menu7[] = "|* *|\n"; char Menu8[] = "|**************************************|\n"; //Impresion del Menu BT.printf("\n%s",Menu1); BT.printf("%s",Menu2); BT.printf("%s",Menu3); BT.printf("%s",Menu4); BT.printf("%s",Menu5); BT.printf("%s",Menu6); BT.printf("%s",Menu7); BT.printf("%s",Menu8); } void enterPIDParameters(void) { bool ready=false; float kp,ki,kd; float ref; char ans[5]; BT.printf("Ingrese los parametros del controlador.\n\r"); while(ready == false) { BT.printf("PID valores actuales\nKp=%1.4f ki=%1.4f kd=%1.4f Ref=%1.4f\n",cKp,cKi,cKd,cRf); BT.printf("Kp >> "); BT.scanf("%f",&kp); BT.printf("Ki >> "); BT.scanf("%f",&ki); BT.printf("Kd >> "); BT.scanf("%f",&kd); BT.printf("Ref [0 - 3.3]>> "); BT.scanf("%f",&ref); BT.printf("Nuevos Valores\nKp=%1.4f Ki=%1.4f Kd=%1.4f Ref=%1.4f\n",kp,ki,kd,ref); BT.printf("Desea continuar? (S/n), (Q) para salir "); BT.scanf("%s",ans); PC.printf("Llego %s \n",ans); if(strncmp(ans,"S",1)==0 || strncmp(ans,"s",1)==0 || strncmp(ans,"q",1)==0 ||strncmp(ans,"Q",1)==0) { PC.printf("Entre\n"); ready = true; } } if(strncmp(ans,"S",1)==0 || strncmp(ans,"s",1)==0){ cKp = kp; cKi = ki; cKd = kd; cRf = ref; lRed=0; lGrn=0; BT.printf("PID valores\nKp=%1.2f ki=%1.2f kd=%1.2f Ref=%1.2f\n",cKp,cKi,cKd,cRf); } } // CICLO PRINCIPAL CONTROLADOR PID void controlPID(){ med = AnInput.read()*999; //Medicion de la salida err = cRf*(999/3.3f) - med; //calculo del error ap = err*cKp*0.01f; //Calculo accion proporcional ai = ai+(err*cKi*0.01f); //Calculo accion integral ad = (err-err_v)*(cKd)*0.01f;//Calculo de la accion derivativa uPID=ap+ai+ad; //Calculo de la accion de control if(uPID<=0){uPID=0;} //Saturador inferior if(uPID>=999){uPID=999;} //Saturador superior err_v = err; //Actualizacion de error out=uPID*(3.3f/999); //Conversion de voltaje a accion de control AnOut.write(out); //Escritura de accion de control BT.printf("Ref=%fv Error=%fv Ucontrol=%fv\n\r",cRf,err/999,out); } void controlPIDpwm(){ med = AnInput.read()*999; //Medicion de la salida err = cRf*(999/3.3f) - med; //calculo del error ap = err*cKp*0.01f; //Calculo accion proporcional ai = ai+(err*cKi*0.01f); //Calculo accion integral ad = (err-err_v)*(cKd)*0.01f;//Calculo de la accion derivativa uPID=ap+ai+ad; //Calculo de la accion de control if(uPID<=0){uPID=0;} //Saturador inferior if(uPID>=999){uPID=999;} //Saturador superior err_v = err; //Actualizacion de error out=uPID*(3.3f/999); //Conversion de voltaje a accion de control pwmOut.write(out); //Escritura de accion de control BT.printf("Ref=%fv Error=%fv Ucontrol=%fv\n\r",cRf,err/999,out); } void LeerBuffer(char *buffer) { int i= 0; while (BT.readable()) { char c = BT.getc(); if (c == '\r' || c == '\n' || c == '\t') c = '$';//si se envia fin de linea o de caracter inserta $ buffer[i++] = c;//mete al bufer el caracter leido } } void executePIDControl() { PC.printf("Entre al PID\n"); //Capturamos los parametros del controlador bool run=true; out=0;ai=0;ad=0;ap=0;med=0;err=0;err_v=0;uPID=0; while(run == true) { lRed=1; lGrn=0; if(continuo){controlPID();} else{controlPIDpwm();} wait_ms(Ts); LeerBuffer(buffer); if(buffer[0]=='d' || buffer[0]=='D' || buffer[1]=='d' || buffer[1]=='D') { run = false; } cleanBuffer(buffer); } } int main() { BT.baud(9600); AnOut.write(0.0f); pwmOut.write(0.0f); lRed = 1; lBlu = 1; for(int i = 0; i<3; i++) { /* Parpadeo de LEDS para indicar el inicio*/ lGrn = 0; wait(0.25); lGrn = 1; wait(0.25); } PC.printf("\nProgram Start!\n"); /*Led en naranja para indicar que esta listo para recibir parametros*/ lRed = 0; lGrn = 1; enterPIDParameters(); while(1) { printMenuBT(); BT.printf("\nIngrese una opcion >> "); BT.scanf("%s",buffer); PC.printf("\n Se recibio: %s \n",buffer); char msg[10]; int mIdx = 0; for (int m = 0; m < 10; m++) { if (buffer[m] != '\n' && buffer[m] != '\r' && buffer[m] != ' ') { msg[mIdx] = buffer[m]; //Guardar los caracteres validos en el mensaje mIdx++; //Corremos el indice del array de chars } } PC.printf("Msg[1] es <%s> \n",msg); if(strncmp(msg,"P",1)==0 || strncmp(msg,"p",1)==0){ enterPIDParameters(); BT.printf("Se han ingreso los parametros del PID\n"); } else if(strncmp(msg,"i",1)==0 || strncmp(msg,"I",1)==0){ PC.printf("Llame al PID continuo\n"); continuo=true; pwmOut.write(0.0f); executePIDControl(); lRed=0; lGrn=0; //BT.printf("Se han ingreso los parametros del PID\n"); } else if(strncmp(msg,"w",1)==0 || strncmp(msg,"W",1)==0){ PC.printf("Llame al PID pwm\n"); continuo=false; AnOut.write(0.0f); executePIDControl(); lRed=0; lGrn=0; //BT.printf("Se han ingreso los parametros del PID\n"); } else if(strncmp(msg,"s",1)==0 || strncmp(msg,"S",1)==0){ PC.printf("Salida en Bajo\n"); lRed=0; lGrn=1; pwmOut.write(0.0f); AnOut.write(0.0f); //BT.printf("Se han ingreso los parametros del PID\n"); } else{ BT.printf("El parametro ingresado no es correcto\n"); } } }