Laura Duque / Mbed 2 deprecated apppidfull

App PID

Dependencies:   mbed

main.cpp@0:379fd298c42a, 2019-07-24 (annotated)

Committer:
Cam53
Date:
Wed Jul 24 18:50:59 2019 +0000
Revision:
0:379fd298c42a
App controlador PID

Who changed what in which revision?

UserRevisionLine numberNew contents of line
Cam53 0:379fd298c42a 1 //PROGRAMA PARA CONTROLAR EL VOLTAJE DE UN CAPACITOR EN UN CIRCUITO RC DESDE DISPOSITIVO ANDROID
Cam53 0:379fd298c42a 2 //ENVIAR LOS VALORES SEPARADOS POR COMAS
Cam53 0:379fd298c42a 3 #include "mbed.h"
Cam53 0:379fd298c42a 4 #include "stdio.h"
Cam53 0:379fd298c42a 5 #include "string.h"
Cam53 0:379fd298c42a 6 #include "stdlib.h"
Cam53 0:379fd298c42a 7
Cam53 0:379fd298c42a 8 //Variables y parametros del control PID
Cam53 0:379fd298c42a 9 float Kp=0,Ki=0,Kd=0,x=0,anchop=0;
Cam53 0:379fd298c42a 10 float Sp=0;//set point
Cam53 0:379fd298c42a 11 float ai,ek,ad,ap,err_v,o,yT,uk=0,Ts=0.01;
Cam53 0:379fd298c42a 12 float duty=0,med=0,an=0;
Cam53 0:379fd298c42a 13
Cam53 0:379fd298c42a 14
Cam53 0:379fd298c42a 15 AnalogIn Vc(PTB0);//voltaje condensador
Cam53 0:379fd298c42a 16 PwmOut uc(PTE20); //accion de control
Cam53 0:379fd298c42a 17 AnalogOut u(PTE30);
Cam53 0:379fd298c42a 18 Serial MR_ROBOT(PTE0,PTE1); //puertos del xDM para el modulo BLUETOOTH
Cam53 0:379fd298c42a 19 Serial pc(USBTX,USBRX); //puertos del PC
Cam53 0:379fd298c42a 20 char buffer[60];// TAMAÑO DEL BUFER
Cam53 0:379fd298c42a 21 Timer t; //VALOR DEL TIEMPO
Cam53 0:379fd298c42a 22 int count;
Cam53 0:379fd298c42a 23 int i = 0;
Cam53 0:379fd298c42a 24 int c=0;
Cam53 0:379fd298c42a 25 int num,ii,j,k;
Cam53 0:379fd298c42a 26 char r[]="";
Cam53 0:379fd298c42a 27 DigitalOut LedRojo(LED1);
Cam53 0:379fd298c42a 28 DigitalOut LedVerde(LED2);
Cam53 0:379fd298c42a 29 DigitalOut LedAzul(LED3);
Cam53 0:379fd298c42a 30
Cam53 0:379fd298c42a 31 int readBuffer(char *buffer,int count) //esta funcion lee un bufer de datos
Cam53 0:379fd298c42a 32 {
Cam53 0:379fd298c42a 33 int i=0;
Cam53 0:379fd298c42a 34 t.start(); //CUENTA EL TIEMPO DE CONEXION E INICIA
Cam53 0:379fd298c42a 35 while(1) {
Cam53 0:379fd298c42a 36 while (MR_ROBOT.readable()) {
Cam53 0:379fd298c42a 37 char c = MR_ROBOT.getc();
Cam53 0:379fd298c42a 38 if (c == '\r' || c == '\n') c = '$';//si se envia fin de linea o de caracter inserta $
Cam53 0:379fd298c42a 39 buffer[i++] = c;//mete al bufer el caracter leido
Cam53 0:379fd298c42a 40 if(i > count)break;//sale del loop si ya detecto terminacion
Cam53 0:379fd298c42a 41 }
Cam53 0:379fd298c42a 42 if(i > count)break;
Cam53 0:379fd298c42a 43 if(t.read() > 1) { //MAS DE UN SEGUNDO DE ESPERA SE SALE Y REINICA EL RELOJ Y SE SALE
Cam53 0:379fd298c42a 44 t.stop();
Cam53 0:379fd298c42a 45 t.reset();
Cam53 0:379fd298c42a 46 break;
Cam53 0:379fd298c42a 47 }
Cam53 0:379fd298c42a 48 }
Cam53 0:379fd298c42a 49 return 0;
Cam53 0:379fd298c42a 50 }
Cam53 0:379fd298c42a 51
Cam53 0:379fd298c42a 52 void cleanBuffer(char *buffer, int count) //esta funcion limpia el bufer
Cam53 0:379fd298c42a 53 {
Cam53 0:379fd298c42a 54 for(int i=0; i < count; i++) {
Cam53 0:379fd298c42a 55 buffer[i] = '\0';
Cam53 0:379fd298c42a 56 }
Cam53 0:379fd298c42a 57 }
Cam53 0:379fd298c42a 58 int main() {
Cam53 0:379fd298c42a 59
Cam53 0:379fd298c42a 60 LedVerde=1;
Cam53 0:379fd298c42a 61 LedRojo=1;
Cam53 0:379fd298c42a 62 LedAzul=1;
Cam53 0:379fd298c42a 63 LedRojo=0;
Cam53 0:379fd298c42a 64 wait(2); //PRENDE EL LED ROJO POR 2 SEGUNDOS
Cam53 0:379fd298c42a 65 LedRojo=1;
Cam53 0:379fd298c42a 66 MR_ROBOT.baud(9600);
Cam53 0:379fd298c42a 67 MR_ROBOT.format(8,Serial::None,1);
Cam53 0:379fd298c42a 68
Cam53 0:379fd298c42a 69
Cam53 0:379fd298c42a 70 pc.printf("Enviar los valores Kp,Ki,Kd,Sp \n");
Cam53 0:379fd298c42a 71
Cam53 0:379fd298c42a 72
Cam53 0:379fd298c42a 73 DEF_CONST:
Cam53 0:379fd298c42a 74
Cam53 0:379fd298c42a 75
Cam53 0:379fd298c42a 76 if (MR_ROBOT.readable()) {
Cam53 0:379fd298c42a 77 readBuffer(buffer,30);
Cam53 0:379fd298c42a 78 pc.printf("Enviar los valores Kp,Ki,Kd,Sp \n");
Cam53 0:379fd298c42a 79 sscanf( buffer, "%f, %f, %f, %f, %f, %f",&x, &Kp, &Ki, &Kd, &Sp, &anchop);
Cam53 0:379fd298c42a 80 pc.printf("%.1f, %.1f, %.1f, %.1f, %.1f, %.1f \n",x,Kp,Ki,Kd,Sp,anchop);
Cam53 0:379fd298c42a 81 }
Cam53 0:379fd298c42a 82
Cam53 0:379fd298c42a 83 if(Sp==0){
Cam53 0:379fd298c42a 84 goto DEF_CONST;}
Cam53 0:379fd298c42a 85 if(x == 1){
Cam53 0:379fd298c42a 86 goto PID;
Cam53 0:379fd298c42a 87 }if(x == 2){
Cam53 0:379fd298c42a 88 goto PWM;
Cam53 0:379fd298c42a 89 }
Cam53 0:379fd298c42a 90 if(x == 3){
Cam53 0:379fd298c42a 91 goto ANALOG;
Cam53 0:379fd298c42a 92 }
Cam53 0:379fd298c42a 93 if(x==4){
Cam53 0:379fd298c42a 94 goto DEF_CONST;}
Cam53 0:379fd298c42a 95
Cam53 0:379fd298c42a 96
Cam53 0:379fd298c42a 97
Cam53 0:379fd298c42a 98 PID:
Cam53 0:379fd298c42a 99
Cam53 0:379fd298c42a 100
Cam53 0:379fd298c42a 101 while(1){
Cam53 0:379fd298c42a 102
Cam53 0:379fd298c42a 103
Cam53 0:379fd298c42a 104
Cam53 0:379fd298c42a 105 yT=Vc.read()*3.3;
Cam53 0:379fd298c42a 106 ek=Sp-yT;
Cam53 0:379fd298c42a 107
Cam53 0:379fd298c42a 108 ap = Kp*ek*0.01f; //se calcula la accion proporcinal
Cam53 0:379fd298c42a 109 ai =(Ki*ek*0.01f)+ai; //calculo de la integral del error
Cam53 0:379fd298c42a 110 ad = Kd*(ek-err_v)*0.01f; //calculo de la accion derivativa
Cam53 0:379fd298c42a 111 uk = (ap+ai+ad);
Cam53 0:379fd298c42a 112
Cam53 0:379fd298c42a 113 // se verifica que pid sea positivo **************
Cam53 0:379fd298c42a 114 if(uk<=0)
Cam53 0:379fd298c42a 115 {
Cam53 0:379fd298c42a 116 uk=0;
Cam53 0:379fd298c42a 117 }
Cam53 0:379fd298c42a 118
Cam53 0:379fd298c42a 119 // se verifica que pid sea menor o igual la valor maximo *******
Cam53 0:379fd298c42a 120 if (uk > 3.3)
Cam53 0:379fd298c42a 121 {
Cam53 0:379fd298c42a 122 uk=3.3;
Cam53 0:379fd298c42a 123 }
Cam53 0:379fd298c42a 124 //Normalizacion de la salida
Cam53 0:379fd298c42a 125 // se actualizan las variables ***************
Cam53 0:379fd298c42a 126 err_v = ek;
Cam53 0:379fd298c42a 127 o = uk/3.3;
Cam53 0:379fd298c42a 128 u.write(o);
Cam53 0:379fd298c42a 129 // se envia el valor pid a puerto analogico de salida (D/A) ******
Cam53 0:379fd298c42a 130
Cam53 0:379fd298c42a 131
Cam53 0:379fd298c42a 132 //Mostrando error y salida actual
Cam53 0:379fd298c42a 133 // wait_ms(500);
Cam53 0:379fd298c42a 134
Cam53 0:379fd298c42a 135 pc.printf("Error= %.2f\t",ek);
Cam53 0:379fd298c42a 136 pc.printf("Set Point= %.1f\t",Sp);
Cam53 0:379fd298c42a 137 pc.printf("Voltaje Actual= %.1f\n",yT);
Cam53 0:379fd298c42a 138
Cam53 0:379fd298c42a 139
Cam53 0:379fd298c42a 140
Cam53 0:379fd298c42a 141 //leo puerto analogico
Cam53 0:379fd298c42a 142 // wait_ms(500);
Cam53 0:379fd298c42a 143 num = yT*303; //agrando el numero de cero a mil
Cam53 0:379fd298c42a 144 if(num<256){ //debo generar dos casos a APP inventor solo me recibe hex asi: 0xhhhh (4 cixas)
Cam53 0:379fd298c42a 145 MR_ROBOT.putc(0); //si el numero es hasta 255 se le ponen dos ceros adelante a la secuencia de bits
Cam53 0:379fd298c42a 146 MR_ROBOT.putc(ii); //luego la cixa menos significativa
Cam53 0:379fd298c42a 147 }
Cam53 0:379fd298c42a 148 if(num>255){ //pero si es mayor a 255 las cixas deben ser convertidas a un hex de dos bytes de la siguiente forma
Cam53 0:379fd298c42a 149 j=num/256; //calculo la cixa mas significativa
Cam53 0:379fd298c42a 150 k=num-j*256; //calculo la cixa menos significativa
Cam53 0:379fd298c42a 151 MR_ROBOT.putc(j); //las envio a la usart para que se las ponga al modulo bluetooth y la lleve al android
Cam53 0:379fd298c42a 152 MR_ROBOT.putc(k); //mas significativa primero, menos despues si no no funciona!!! y con la orden PUTC solo asi le envia binarios
Cam53 0:379fd298c42a 153 }
Cam53 0:379fd298c42a 154
Cam53 0:379fd298c42a 155 readBuffer(buffer,30);
Cam53 0:379fd298c42a 156 sscanf( buffer, "%f, %f, %f, %f, %f, %f",&x, &Kp, &Ki, &Kd, &Sp,&anchop);
Cam53 0:379fd298c42a 157
Cam53 0:379fd298c42a 158 if(x == 4){
Cam53 0:379fd298c42a 159 goto DEF_CONST;
Cam53 0:379fd298c42a 160 }
Cam53 0:379fd298c42a 161
Cam53 0:379fd298c42a 162 }
Cam53 0:379fd298c42a 163
Cam53 0:379fd298c42a 164 PWM:
Cam53 0:379fd298c42a 165 while(1){
Cam53 0:379fd298c42a 166 yT=Vc.read()*3.3;
Cam53 0:379fd298c42a 167
Cam53 0:379fd298c42a 168 duty=(anchop/4);
Cam53 0:379fd298c42a 169 uc.period_ms(200);
Cam53 0:379fd298c42a 170 uc.pulsewidth_ms(duty);
Cam53 0:379fd298c42a 171
Cam53 0:379fd298c42a 172 //wait_ms(500);
Cam53 0:379fd298c42a 173
Cam53 0:379fd298c42a 174 pc.printf("Ancho de pulso PWM(T=2000ms)= %.1f\t",duty);
Cam53 0:379fd298c42a 175 pc.printf("Voltaje Actual= %.1f\n",yT);
Cam53 0:379fd298c42a 176
Cam53 0:379fd298c42a 177 // se repite el ciclo
Cam53 0:379fd298c42a 178 //wait_ms(500);
Cam53 0:379fd298c42a 179
Cam53 0:379fd298c42a 180 num = yT*303;
Cam53 0:379fd298c42a 181 if(num<256){ //debo generar dos casos a APP inventor solo me recibe hex asi: 0xhhhh (4 cixas)
Cam53 0:379fd298c42a 182 MR_ROBOT.putc(0); //si el numero es hasta 255 se le ponen dos ceros adelante a la secuencia de bits
Cam53 0:379fd298c42a 183 MR_ROBOT.putc(ii); //luego la cixa menos significativa
Cam53 0:379fd298c42a 184 }
Cam53 0:379fd298c42a 185 if(num>255){ //pero si es mayor a 255 las cixas deben ser convertidas a un hex de dos bytes de la siguiente forma
Cam53 0:379fd298c42a 186 j=num/256; //calculo la cixa mas significativa
Cam53 0:379fd298c42a 187 k=num-j*256; //calculo la cixa menos significativa
Cam53 0:379fd298c42a 188 MR_ROBOT.putc(j); //las envio a la usart para que se las ponga al modulo bluetooth y la lleve al android
Cam53 0:379fd298c42a 189 MR_ROBOT.putc(k); //mas significativa primero, menos despues si no no funciona!!! y con la orden PUTC solo asi le envia binarios
Cam53 0:379fd298c42a 190 }
Cam53 0:379fd298c42a 191
Cam53 0:379fd298c42a 192 readBuffer(buffer,30);
Cam53 0:379fd298c42a 193 sscanf( buffer, "%f, %f, %f, %f, %f, %f",&x, &Kp, &Ki, &Kd, &Sp,&anchop);
Cam53 0:379fd298c42a 194
Cam53 0:379fd298c42a 195 if(x == 4){
Cam53 0:379fd298c42a 196 goto DEF_CONST;
Cam53 0:379fd298c42a 197 }
Cam53 0:379fd298c42a 198
Cam53 0:379fd298c42a 199 }
Cam53 0:379fd298c42a 200
Cam53 0:379fd298c42a 201 ANALOG:
Cam53 0:379fd298c42a 202 while(1){
Cam53 0:379fd298c42a 203 yT=Vc.read()*3.3;
Cam53 0:379fd298c42a 204 o = Sp/3.3;
Cam53 0:379fd298c42a 205 u.write(o);
Cam53 0:379fd298c42a 206 // se envia el valor pid a puerto analogico de salida (D/A) **
Cam53 0:379fd298c42a 207
Cam53 0:379fd298c42a 208 // se repite el ciclo
Cam53 0:379fd298c42a 209 //wait_ms(500);
Cam53 0:379fd298c42a 210
Cam53 0:379fd298c42a 211 //Mostrando error y salida actual
Cam53 0:379fd298c42a 212
Cam53 0:379fd298c42a 213 pc.printf("Set Point= %.1f\t",Sp);
Cam53 0:379fd298c42a 214 pc.printf("Voltaje Actual= %.1f\n",yT);
Cam53 0:379fd298c42a 215
Cam53 0:379fd298c42a 216 // se repite el ciclo
Cam53 0:379fd298c42a 217 //wait_ms(500);
Cam53 0:379fd298c42a 218
Cam53 0:379fd298c42a 219 num = yT*303;
Cam53 0:379fd298c42a 220 if(num<256){ //debo generar dos casos a APP inventor solo me recibe hex asi: 0xhhhh (4 cixas)
Cam53 0:379fd298c42a 221 MR_ROBOT.putc(0); //si el numero es hasta 255 se le ponen dos ceros adelante a la secuencia de bits
Cam53 0:379fd298c42a 222 MR_ROBOT.putc(ii); //luego la cixa menos significativa
Cam53 0:379fd298c42a 223 }
Cam53 0:379fd298c42a 224 if(num>255){ //pero si es mayor a 255 las cixas deben ser convertidas a un hex de dos bytes de la siguiente forma
Cam53 0:379fd298c42a 225 j=num/256; //calculo la cixa mas significativa
Cam53 0:379fd298c42a 226 k=num-j*256; //calculo la cixa menos significativa
Cam53 0:379fd298c42a 227 MR_ROBOT.putc(j); //las envio a la usart para que se las ponga al modulo bluetooth y la lleve al android
Cam53 0:379fd298c42a 228 MR_ROBOT.putc(k); //mas significativa primero, menos despues si no no funciona!!! y con la orden PUTC solo asi le envia binarios
Cam53 0:379fd298c42a 229 }
Cam53 0:379fd298c42a 230
Cam53 0:379fd298c42a 231 readBuffer(buffer,30);
Cam53 0:379fd298c42a 232 sscanf( buffer, "%f, %f, %f, %f, %f, %f",&x, &Kp, &Ki, &Kd, &Sp,&anchop);
Cam53 0:379fd298c42a 233
Cam53 0:379fd298c42a 234 if(x == 4){
Cam53 0:379fd298c42a 235 goto DEF_CONST;
Cam53 0:379fd298c42a 236 }
Cam53 0:379fd298c42a 237
Cam53 0:379fd298c42a 238
Cam53 0:379fd298c42a 239
Cam53 0:379fd298c42a 240 }
Cam53 0:379fd298c42a 241
Cam53 0:379fd298c42a 242 }