Giovani Cardona / Mbed 2 deprecated G_PID3_Bluetooth

PID - Bluetooth

Dependencies:   mbed

main.cpp@0:53194b344442, 2019-06-13 (annotated)

Committer:
Giovani_Cardona
Date:
Thu Jun 13 21:20:06 2019 +0000
Revision:
0:53194b344442
PID - Bluetooth

Who changed what in which revision?

UserRevisionLine numberNew 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 }