Giovani Cardona
/
G_PID3
Controlador PID con comunicación Bluetooth, ingreso de parámetros por terminal.
main.cpp@0:87552d9a8512, 2019-06-12 (annotated)
- Committer:
- Giovani_Cardona
- Date:
- Wed Jun 12 20:06:42 2019 +0000
- Revision:
- 0:87552d9a8512
-
Who changed what in which revision?
User | Revision | Line number | New contents of line |
---|---|---|---|
Giovani_Cardona | 0:87552d9a8512 | 1 | #include "mbed.h" |
Giovani_Cardona | 0:87552d9a8512 | 2 | #include "string.h" |
Giovani_Cardona | 0:87552d9a8512 | 3 | |
Giovani_Cardona | 0:87552d9a8512 | 4 | AnalogIn y(PTB3);//entrada analoga |
Giovani_Cardona | 0:87552d9a8512 | 5 | AnalogOut u(PTE30);//salida analoga OJO solo se le pueden drenar 1.5mA en circuitos use un Buffer |
Giovani_Cardona | 0:87552d9a8512 | 6 | //si se ignora esto se arruina la FRDMKL25Z |
Giovani_Cardona | 0:87552d9a8512 | 7 | Serial pc(USBTX, USBRX, "pc"); |
Giovani_Cardona | 0:87552d9a8512 | 8 | Serial HC06(PTE0,PTE1); |
Giovani_Cardona | 0:87552d9a8512 | 9 | |
Giovani_Cardona | 0:87552d9a8512 | 10 | char buffer[128]; |
Giovani_Cardona | 0:87552d9a8512 | 11 | int readptr = 0; |
Giovani_Cardona | 0:87552d9a8512 | 12 | int Kp, Ki, Kd, Sp; |
Giovani_Cardona | 0:87552d9a8512 | 13 | float pid,o,ai,ad,ap,med,err; |
Giovani_Cardona | 0:87552d9a8512 | 14 | float err_v; |
Giovani_Cardona | 0:87552d9a8512 | 15 | int checker1, checker2, checker3, checker4; |
Giovani_Cardona | 0:87552d9a8512 | 16 | |
Giovani_Cardona | 0:87552d9a8512 | 17 | int main(){ |
Giovani_Cardona | 0:87552d9a8512 | 18 | char c; |
Giovani_Cardona | 0:87552d9a8512 | 19 | |
Giovani_Cardona | 0:87552d9a8512 | 20 | // Bluetooth |
Giovani_Cardona | 0:87552d9a8512 | 21 | HC06.baud(9600); |
Giovani_Cardona | 0:87552d9a8512 | 22 | HC06.printf("CONTROLADOR PID BLUETOOTH\n"); |
Giovani_Cardona | 0:87552d9a8512 | 23 | |
Giovani_Cardona | 0:87552d9a8512 | 24 | // Interfaz para el pc |
Giovani_Cardona | 0:87552d9a8512 | 25 | pc.printf("***********************************************\n"); |
Giovani_Cardona | 0:87552d9a8512 | 26 | pc.printf("CONTROLADOR PID \n"); |
Giovani_Cardona | 0:87552d9a8512 | 27 | pc.printf("***********************************************\n"); |
Giovani_Cardona | 0:87552d9a8512 | 28 | pc.printf("* Menu para los parametros del controlador PID: \n\n"); |
Giovani_Cardona | 0:87552d9a8512 | 29 | pc.printf("* Kp - Ingresar valor de la consotante proporcional \n"); |
Giovani_Cardona | 0:87552d9a8512 | 30 | pc.printf("* Ki - Ingresar valor de la consotante integral \n"); |
Giovani_Cardona | 0:87552d9a8512 | 31 | pc.printf("* Kd - Ingresar valor de la consotante derivativa \n"); |
Giovani_Cardona | 0:87552d9a8512 | 32 | pc.printf("* Sp - Ingresar valor del Set-Point \n\n"); |
Giovani_Cardona | 0:87552d9a8512 | 33 | |
Giovani_Cardona | 0:87552d9a8512 | 34 | while(1){ |
Giovani_Cardona | 0:87552d9a8512 | 35 | |
Giovani_Cardona | 0:87552d9a8512 | 36 | // Analizamos lo que ingresa el usuario |
Giovani_Cardona | 0:87552d9a8512 | 37 | while( (c = pc.getc()) != '\n') { |
Giovani_Cardona | 0:87552d9a8512 | 38 | buffer[readptr++] = c; |
Giovani_Cardona | 0:87552d9a8512 | 39 | } |
Giovani_Cardona | 0:87552d9a8512 | 40 | buffer[readptr++] = 0; |
Giovani_Cardona | 0:87552d9a8512 | 41 | |
Giovani_Cardona | 0:87552d9a8512 | 42 | //pc.printf("buffer= %s\n\r ",buffer); //imprime el bufer |
Giovani_Cardona | 0:87552d9a8512 | 43 | //pc.printf("buffer= %c %c\n\r ",buffer[0],buffer[1]);//imprime el cero y el uno |
Giovani_Cardona | 0:87552d9a8512 | 44 | |
Giovani_Cardona | 0:87552d9a8512 | 45 | ////////////////////////////////////////// |
Giovani_Cardona | 0:87552d9a8512 | 46 | |
Giovani_Cardona | 0:87552d9a8512 | 47 | if (strncmp(buffer, "Kp", 2) == 0) { |
Giovani_Cardona | 0:87552d9a8512 | 48 | //Se lee el valor para Kp |
Giovani_Cardona | 0:87552d9a8512 | 49 | pc.printf("Ingrese el valor para Kp: "); |
Giovani_Cardona | 0:87552d9a8512 | 50 | pc.scanf("%d", &Kp); |
Giovani_Cardona | 0:87552d9a8512 | 51 | checker1=1; |
Giovani_Cardona | 0:87552d9a8512 | 52 | } |
Giovani_Cardona | 0:87552d9a8512 | 53 | else if (strncmp(buffer, "Ki", 2) == 0) { |
Giovani_Cardona | 0:87552d9a8512 | 54 | //Se lee el valor para Ki |
Giovani_Cardona | 0:87552d9a8512 | 55 | pc.printf("Ingrese el valor para Ki: "); |
Giovani_Cardona | 0:87552d9a8512 | 56 | pc.scanf("%d", &Ki); |
Giovani_Cardona | 0:87552d9a8512 | 57 | checker2=1; |
Giovani_Cardona | 0:87552d9a8512 | 58 | } |
Giovani_Cardona | 0:87552d9a8512 | 59 | else if (strncmp(buffer, "Kd", 2) == 0) { |
Giovani_Cardona | 0:87552d9a8512 | 60 | //Se lee el valor para Kd |
Giovani_Cardona | 0:87552d9a8512 | 61 | pc.printf("Ingrese el valor para Kd: "); |
Giovani_Cardona | 0:87552d9a8512 | 62 | pc.scanf("%d", &Kd); |
Giovani_Cardona | 0:87552d9a8512 | 63 | checker3=1; |
Giovani_Cardona | 0:87552d9a8512 | 64 | } |
Giovani_Cardona | 0:87552d9a8512 | 65 | else if (strncmp(buffer, "Sp", 2) == 0) { |
Giovani_Cardona | 0:87552d9a8512 | 66 | //Se lee el valor para Sp |
Giovani_Cardona | 0:87552d9a8512 | 67 | pc.printf("Ingrese el valor para Sp: "); |
Giovani_Cardona | 0:87552d9a8512 | 68 | pc.scanf("%d", &Sp); |
Giovani_Cardona | 0:87552d9a8512 | 69 | if(Sp<=0){Sp=0;} |
Giovani_Cardona | 0:87552d9a8512 | 70 | if(Sp > 999){Sp=999;} |
Giovani_Cardona | 0:87552d9a8512 | 71 | checker4=1; |
Giovani_Cardona | 0:87552d9a8512 | 72 | } |
Giovani_Cardona | 0:87552d9a8512 | 73 | else { |
Giovani_Cardona | 0:87552d9a8512 | 74 | pc.printf("Syntax error \n"); |
Giovani_Cardona | 0:87552d9a8512 | 75 | } |
Giovani_Cardona | 0:87552d9a8512 | 76 | readptr = 0; |
Giovani_Cardona | 0:87552d9a8512 | 77 | |
Giovani_Cardona | 0:87552d9a8512 | 78 | if (checker1 == 1 && checker2 == 1 && checker3 == 1 && checker4 == 1 ){ |
Giovani_Cardona | 0:87552d9a8512 | 79 | pc.printf("Parametros listos para iniciar \n"); |
Giovani_Cardona | 0:87552d9a8512 | 80 | pc.printf("*************************************\n"); |
Giovani_Cardona | 0:87552d9a8512 | 81 | pc.printf("Iniciar ahora (Si/No) \n"); |
Giovani_Cardona | 0:87552d9a8512 | 82 | |
Giovani_Cardona | 0:87552d9a8512 | 83 | //Se lee la elección del usuario |
Giovani_Cardona | 0:87552d9a8512 | 84 | while(1){ |
Giovani_Cardona | 0:87552d9a8512 | 85 | while( (c = pc.getc()) != '\n') { |
Giovani_Cardona | 0:87552d9a8512 | 86 | buffer[readptr++] = c; |
Giovani_Cardona | 0:87552d9a8512 | 87 | } |
Giovani_Cardona | 0:87552d9a8512 | 88 | buffer[readptr++] = 0; |
Giovani_Cardona | 0:87552d9a8512 | 89 | |
Giovani_Cardona | 0:87552d9a8512 | 90 | if (strncmp(buffer, "Si", 2) == 0){ |
Giovani_Cardona | 0:87552d9a8512 | 91 | pc.printf("* Ingrese 'P' si desea cambiar el Set Ponit \n\n"); |
Giovani_Cardona | 0:87552d9a8512 | 92 | pc.printf("Iniciando... \n\n"); |
Giovani_Cardona | 0:87552d9a8512 | 93 | wait_ms(1000); |
Giovani_Cardona | 0:87552d9a8512 | 94 | break; |
Giovani_Cardona | 0:87552d9a8512 | 95 | } |
Giovani_Cardona | 0:87552d9a8512 | 96 | else{ |
Giovani_Cardona | 0:87552d9a8512 | 97 | pc.printf("Iniciar ahora (Si/No) \n"); |
Giovani_Cardona | 0:87552d9a8512 | 98 | } |
Giovani_Cardona | 0:87552d9a8512 | 99 | readptr = 0; |
Giovani_Cardona | 0:87552d9a8512 | 100 | } |
Giovani_Cardona | 0:87552d9a8512 | 101 | break; |
Giovani_Cardona | 0:87552d9a8512 | 102 | }else{ |
Giovani_Cardona | 0:87552d9a8512 | 103 | pc.printf("*************************************\n"); |
Giovani_Cardona | 0:87552d9a8512 | 104 | pc.printf("Ingrese los valores restantes \n"); |
Giovani_Cardona | 0:87552d9a8512 | 105 | pc.printf("*************************************\n"); |
Giovani_Cardona | 0:87552d9a8512 | 106 | } |
Giovani_Cardona | 0:87552d9a8512 | 107 | pc.printf("\n"); |
Giovani_Cardona | 0:87552d9a8512 | 108 | } |
Giovani_Cardona | 0:87552d9a8512 | 109 | |
Giovani_Cardona | 0:87552d9a8512 | 110 | // CICLO PRINCIPAL CONTROLADOR PID |
Giovani_Cardona | 0:87552d9a8512 | 111 | |
Giovani_Cardona | 0:87552d9a8512 | 112 | lop1: |
Giovani_Cardona | 0:87552d9a8512 | 113 | med = y.read()*999; |
Giovani_Cardona | 0:87552d9a8512 | 114 | err = (Sp-med); //se calcula el error |
Giovani_Cardona | 0:87552d9a8512 | 115 | ap = Kp*err*0.01f; //se calcula la accion proporcinal |
Giovani_Cardona | 0:87552d9a8512 | 116 | ai =(Ki*err*0.01f)+ai; //calculo de la integral del error |
Giovani_Cardona | 0:87552d9a8512 | 117 | ad = Kd*(err-err_v)*0.01f; //calculo de la accion derivativa |
Giovani_Cardona | 0:87552d9a8512 | 118 | pid = (ap+ai+ad); |
Giovani_Cardona | 0:87552d9a8512 | 119 | // se verifica que pid sea positivo ************************************** |
Giovani_Cardona | 0:87552d9a8512 | 120 | if(pid<=0) |
Giovani_Cardona | 0:87552d9a8512 | 121 | { |
Giovani_Cardona | 0:87552d9a8512 | 122 | pid=0; |
Giovani_Cardona | 0:87552d9a8512 | 123 | } |
Giovani_Cardona | 0:87552d9a8512 | 124 | |
Giovani_Cardona | 0:87552d9a8512 | 125 | // se verifica que pid sea menor o igual al valor maximo ***************** |
Giovani_Cardona | 0:87552d9a8512 | 126 | if (pid > 999) |
Giovani_Cardona | 0:87552d9a8512 | 127 | { |
Giovani_Cardona | 0:87552d9a8512 | 128 | pid=999; |
Giovani_Cardona | 0:87552d9a8512 | 129 | } |
Giovani_Cardona | 0:87552d9a8512 | 130 | |
Giovani_Cardona | 0:87552d9a8512 | 131 | //Normalizacion de la salida |
Giovani_Cardona | 0:87552d9a8512 | 132 | // se actualizan las variables ******************************************* |
Giovani_Cardona | 0:87552d9a8512 | 133 | err_v = err; |
Giovani_Cardona | 0:87552d9a8512 | 134 | o = pid/999; |
Giovani_Cardona | 0:87552d9a8512 | 135 | u.write(o); // se envia el valor pid a puerto analogico de salida (D/A) ************** |
Giovani_Cardona | 0:87552d9a8512 | 136 | |
Giovani_Cardona | 0:87552d9a8512 | 137 | pc.printf("SetPoint\tEntrada\t\tError\t\tSalida\n"); |
Giovani_Cardona | 0:87552d9a8512 | 138 | pc.printf("%d\t\t%0.2f\t\t%0.2f\t\t%0.2f\n",Sp,med,err,pid); |
Giovani_Cardona | 0:87552d9a8512 | 139 | pc.printf("%0.2fV\t\t%0.2fV\t\t%0.2fV\t\t%0.2fV\n\n",((Sp*3.3)/1000),((med*3.3)/1000),((err*3.3)/1000),((pid*3.3)/1000)); |
Giovani_Cardona | 0:87552d9a8512 | 140 | |
Giovani_Cardona | 0:87552d9a8512 | 141 | HC06.printf("SetPoint: %d %0.2fV\n\r",Sp,((Sp*3.3)/1000)); |
Giovani_Cardona | 0:87552d9a8512 | 142 | HC06.printf("Entrada: %0.2f %0.2fV\n\r",med,((med*3.3)/1000)); |
Giovani_Cardona | 0:87552d9a8512 | 143 | HC06.printf("Error: %0.2f %0.2fV\n\r",err,((err*3.3)/1000)); |
Giovani_Cardona | 0:87552d9a8512 | 144 | HC06.printf("Salida: %0.2f %0.2fV\n\r",pid,((pid*3.3)/1000)); |
Giovani_Cardona | 0:87552d9a8512 | 145 | HC06.printf("\n\r"); |
Giovani_Cardona | 0:87552d9a8512 | 146 | |
Giovani_Cardona | 0:87552d9a8512 | 147 | ////-----------------------Nuevo Setpoint------------------------------------------- |
Giovani_Cardona | 0:87552d9a8512 | 148 | if(pc.readable()){ |
Giovani_Cardona | 0:87552d9a8512 | 149 | if ((c = pc.getc()) == 'P') { |
Giovani_Cardona | 0:87552d9a8512 | 150 | buffer[readptr++] = c; |
Giovani_Cardona | 0:87552d9a8512 | 151 | } |
Giovani_Cardona | 0:87552d9a8512 | 152 | buffer[readptr++] = 0; |
Giovani_Cardona | 0:87552d9a8512 | 153 | //pc.printf("buffer= %s\n\r ",buffer); //imprime el bufer |
Giovani_Cardona | 0:87552d9a8512 | 154 | //pc.printf("buffer= %c %c\n\r ",buffer[0],buffer[1]);//imprime el cero y el uno |
Giovani_Cardona | 0:87552d9a8512 | 155 | } |
Giovani_Cardona | 0:87552d9a8512 | 156 | if (strncmp(buffer, "P", 1) == 0){ |
Giovani_Cardona | 0:87552d9a8512 | 157 | buffer[0]='\0'; |
Giovani_Cardona | 0:87552d9a8512 | 158 | pc.printf("* Ingrese el nuevo SetPoint: "); |
Giovani_Cardona | 0:87552d9a8512 | 159 | pc.scanf("%d", &Sp); |
Giovani_Cardona | 0:87552d9a8512 | 160 | if(Sp<=0){Sp=0;} |
Giovani_Cardona | 0:87552d9a8512 | 161 | if(Sp > 999){Sp=999;} |
Giovani_Cardona | 0:87552d9a8512 | 162 | pc.printf("Iniciando... \n\n"); |
Giovani_Cardona | 0:87552d9a8512 | 163 | wait_ms(1000); |
Giovani_Cardona | 0:87552d9a8512 | 164 | } |
Giovani_Cardona | 0:87552d9a8512 | 165 | |
Giovani_Cardona | 0:87552d9a8512 | 166 | readptr = 0; |
Giovani_Cardona | 0:87552d9a8512 | 167 | /////------------------------------------------------------------------ |
Giovani_Cardona | 0:87552d9a8512 | 168 | |
Giovani_Cardona | 0:87552d9a8512 | 169 | // se repite el ciclo |
Giovani_Cardona | 0:87552d9a8512 | 170 | wait_ms(500); |
Giovani_Cardona | 0:87552d9a8512 | 171 | goto lop1; |
Giovani_Cardona | 0:87552d9a8512 | 172 | } |