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