Daniel Uran / Mbed 2 deprecated TAREA-PID_APLICACION

PID con android

Dependencies:   mbed

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

Committer:
dsuranr
Date:
Wed Jul 24 19:02:20 2019 +0000
Revision:
0:d890d56973d1
Child:
1:e076d163a730
Practica App

Who changed what in which revision?

UserRevisionLine numberNew contents of line
dsuranr 0:d890d56973d1 1 #include "mbed.h"
dsuranr 0:d890d56973d1 2 #include "stdio.h"
dsuranr 0:d890d56973d1 3 #include "string.h"
dsuranr 0:d890d56973d1 4 #include "stdlib.h"
dsuranr 0:d890d56973d1 5
dsuranr 0:d890d56973d1 6
dsuranr 0:d890d56973d1 7 Serial GSM(PTE0,PTE1); //puertos del FRDM para el modem
dsuranr 0:d890d56973d1 8 Serial pc(USBTX,USBRX); //puertos del PC
dsuranr 0:d890d56973d1 9
dsuranr 0:d890d56973d1 10 float pid,o,ai,ad,ap,med,err,pwm;
dsuranr 0:d890d56973d1 11 float err_v;
dsuranr 0:d890d56973d1 12 float KPNII,KDNII,kdnum=0;
dsuranr 0:d890d56973d1 13 float kpnum=0,kinum=0,ok=0;
dsuranr 0:d890d56973d1 14 float spnum=0.0;
dsuranr 0:d890d56973d1 15 float AP=1;
dsuranr 0:d890d56973d1 16 float h=0;
dsuranr 0:d890d56973d1 17 char P;
dsuranr 0:d890d56973d1 18
dsuranr 0:d890d56973d1 19 char buffer[21];// TAMAÑO DEL BUFER
dsuranr 0:d890d56973d1 20 char bufferB[5];
dsuranr 0:d890d56973d1 21 char KPC[4],KDC[4];
dsuranr 0:d890d56973d1 22 char KPCII[4],KDCII[4];
dsuranr 0:d890d56973d1 23 char *cadena = "";
dsuranr 0:d890d56973d1 24 char r[]="";
dsuranr 0:d890d56973d1 25
dsuranr 0:d890d56973d1 26 Timer t; //VALOR DEL TIEMPO
dsuranr 0:d890d56973d1 27 int count;
dsuranr 0:d890d56973d1 28 int i = 0;
dsuranr 0:d890d56973d1 29 int c=0;
dsuranr 0:d890d56973d1 30
dsuranr 0:d890d56973d1 31
dsuranr 0:d890d56973d1 32 DigitalOut LedRojo(LED1);
dsuranr 0:d890d56973d1 33 DigitalOut LedVerde(LED2);
dsuranr 0:d890d56973d1 34 DigitalOut LedAzul(LED3);
dsuranr 0:d890d56973d1 35 //PwmOut led(PTA13);
dsuranr 0:d890d56973d1 36
dsuranr 0:d890d56973d1 37 AnalogIn y(PTB3);//entrada analoga
dsuranr 0:d890d56973d1 38
dsuranr 0:d890d56973d1 39 //PwmOut u(PTE30);
dsuranr 0:d890d56973d1 40 PwmOut led(PTA13);
dsuranr 0:d890d56973d1 41
dsuranr 0:d890d56973d1 42
dsuranr 0:d890d56973d1 43
dsuranr 0:d890d56973d1 44
dsuranr 0:d890d56973d1 45 int readBuffer(char *buffer,int count) //esta funcion lee un bufer de datos
dsuranr 0:d890d56973d1 46 {
dsuranr 0:d890d56973d1 47 int i=0;
dsuranr 0:d890d56973d1 48 t.start(); //CUENTA EL TIEMPO DE CONEXION E INICIA
dsuranr 0:d890d56973d1 49 while(1) {
dsuranr 0:d890d56973d1 50 while (GSM.readable()) {
dsuranr 0:d890d56973d1 51 char c = GSM.getc();
dsuranr 0:d890d56973d1 52
dsuranr 0:d890d56973d1 53 if (c == '\r' || c == '\n') c = '$';//si se envia fin de linea o de caracter inserta $
dsuranr 0:d890d56973d1 54 buffer[i++] = c;//mete al bufer el caracter leido
dsuranr 0:d890d56973d1 55 if(i > count)break;//sale del loop si ya detecto terminacion
dsuranr 0:d890d56973d1 56 }
dsuranr 0:d890d56973d1 57 if(i > count)break;
dsuranr 0:d890d56973d1 58 if(t.read() > 1) { //MAS DE UN SEGUNDO DE ESPERA SE SALE Y REINICA EL RELOJ Y SE SALE
dsuranr 0:d890d56973d1 59 t.stop();
dsuranr 0:d890d56973d1 60 t.reset();
dsuranr 0:d890d56973d1 61 break;
dsuranr 0:d890d56973d1 62 }
dsuranr 0:d890d56973d1 63 }
dsuranr 0:d890d56973d1 64 return 0;
dsuranr 0:d890d56973d1 65 }
dsuranr 0:d890d56973d1 66
dsuranr 0:d890d56973d1 67 void cleanBuffer(char *buffer, int count) //esta funcion limpia el bufer
dsuranr 0:d890d56973d1 68 {
dsuranr 0:d890d56973d1 69 for(int i=0; i < count; i++) {
dsuranr 0:d890d56973d1 70 buffer[i] = '\0';
dsuranr 0:d890d56973d1 71 }
dsuranr 0:d890d56973d1 72 }
dsuranr 0:d890d56973d1 73
dsuranr 0:d890d56973d1 74
dsuranr 0:d890d56973d1 75 int main(void)
dsuranr 0:d890d56973d1 76 {
dsuranr 0:d890d56973d1 77 LL:
dsuranr 0:d890d56973d1 78 AP=0;
dsuranr 0:d890d56973d1 79 LedVerde=1;
dsuranr 0:d890d56973d1 80 LedRojo=1;
dsuranr 0:d890d56973d1 81 LedAzul=1;
dsuranr 0:d890d56973d1 82 LedVerde=0;
dsuranr 0:d890d56973d1 83 wait(2); //PRENDE EL LED VERDE POR 2 SEGUNDOS
dsuranr 0:d890d56973d1 84 LedVerde=1;
dsuranr 0:d890d56973d1 85
dsuranr 0:d890d56973d1 86 led.period(0.005f); // 4 second period
dsuranr 0:d890d56973d1 87 led.write(0.40f); // 50% duty cycle, relative to period
dsuranr 0:d890d56973d1 88
dsuranr 0:d890d56973d1 89 pc.printf("SI VOLVI");
dsuranr 0:d890d56973d1 90 while(1) {
dsuranr 0:d890d56973d1 91
dsuranr 0:d890d56973d1 92 /////////////////////////////////////////////////////////////////////////////////////////////////////////////
dsuranr 0:d890d56973d1 93 LedAzul=0;
dsuranr 0:d890d56973d1 94 /*pc.printf(" \n");
dsuranr 0:d890d56973d1 95 pc.printf("CONTROLADOR PID (pc) \n");
dsuranr 0:d890d56973d1 96 pc.printf("Ingrese el parametro a modificar KP , KD, KI, SP(SetPoint) ) \n" );
dsuranr 0:d890d56973d1 97 pc.printf("Escriba M para mirar los parametros del PID \n");
dsuranr 0:d890d56973d1 98 pc.printf("Escriba OK para Iniciar PID \n");
dsuranr 0:d890d56973d1 99 pc.printf(" \n");
dsuranr 0:d890d56973d1 100 ///////////////////////////////////////////////////////////////////////////////////////////////////
dsuranr 0:d890d56973d1 101 */
dsuranr 0:d890d56973d1 102 pc:
dsuranr 0:d890d56973d1 103 if (GSM.readable()) {
dsuranr 0:d890d56973d1 104 readBuffer(buffer,25);
dsuranr 0:d890d56973d1 105
dsuranr 0:d890d56973d1 106
dsuranr 0:d890d56973d1 107
dsuranr 0:d890d56973d1 108 GSM.scanf("%f, %f, %f , %f , %f, %f, %f ", &spnum, &kpnum, &kdnum,&kinum,&AP,&pwm,&h);
dsuranr 0:d890d56973d1 109 //GSM.scanf("%f ", &spnum);
dsuranr 0:d890d56973d1 110 // pc.printf("buffer= %C %c\n\r ",buffer);//imprime el cero y el uno
dsuranr 0:d890d56973d1 111 pc.printf("buffer= %s ",buffer);//imprime el cero y el uno
dsuranr 0:d890d56973d1 112 // pc.printf("buffer= %d , %d , %d ",buffer[0]+buffer[1]+buffer[2]);//imprime el cero y el uno
dsuranr 0:d890d56973d1 113
dsuranr 0:d890d56973d1 114 pc.printf("\n SP = %f \n", spnum);
dsuranr 0:d890d56973d1 115 pc.printf("\n KP = %f \n", kpnum);
dsuranr 0:d890d56973d1 116 pc.printf("\n KD = %f \n", kdnum);
dsuranr 0:d890d56973d1 117 pc.printf("\n Ki = %f \n", kinum);
dsuranr 0:d890d56973d1 118 pc.printf("\n AP = %f \n", AP);
dsuranr 0:d890d56973d1 119 pc.printf("\n pwm = %f \n", pwm);
dsuranr 0:d890d56973d1 120 pc.printf("\n h = %f \n", h);
dsuranr 0:d890d56973d1 121 //cleanBuffer(buffer,15);
dsuranr 0:d890d56973d1 122
dsuranr 0:d890d56973d1 123
dsuranr 0:d890d56973d1 124 pc.printf("ESTOY ACA");
dsuranr 0:d890d56973d1 125 //if(ok==1){
dsuranr 0:d890d56973d1 126 lop1:
dsuranr 0:d890d56973d1 127 //cleanBuffer(buffer,15);
dsuranr 0:d890d56973d1 128 LedAzul=1;
dsuranr 0:d890d56973d1 129 wait(1);
dsuranr 0:d890d56973d1 130 LedAzul=0;
dsuranr 0:d890d56973d1 131
dsuranr 0:d890d56973d1 132
dsuranr 0:d890d56973d1 133 med = y.read()*3.3;
dsuranr 0:d890d56973d1 134 err = (spnum-med); //se calcula el error
dsuranr 0:d890d56973d1 135 ap = kpnum*err*0.01f; //se calcula la accion proporcinal
dsuranr 0:d890d56973d1 136 ai =(kinum*err*0.01f)+ai; //calculo de la integral del error
dsuranr 0:d890d56973d1 137 ad = kdnum*(err-err_v)*0.01f; //calculo de la accion derivativa
dsuranr 0:d890d56973d1 138 pid = (ap+ai+ad);
dsuranr 0:d890d56973d1 139
dsuranr 0:d890d56973d1 140
dsuranr 0:d890d56973d1 141 // se verifica que pid sea positivo **************************************
dsuranr 0:d890d56973d1 142 if(pid<=0) {
dsuranr 0:d890d56973d1 143 pid=0;
dsuranr 0:d890d56973d1 144
dsuranr 0:d890d56973d1 145 }
dsuranr 0:d890d56973d1 146
dsuranr 0:d890d56973d1 147 // se verifica que pid sea menor o igual la valor maximo *****************
dsuranr 0:d890d56973d1 148 if (pid > 3.3) {
dsuranr 0:d890d56973d1 149 pid=3.3;
dsuranr 0:d890d56973d1 150
dsuranr 0:d890d56973d1 151 }
dsuranr 0:d890d56973d1 152
dsuranr 0:d890d56973d1 153
dsuranr 0:d890d56973d1 154
dsuranr 0:d890d56973d1 155
dsuranr 0:d890d56973d1 156 pc.printf("ERROR= %0.01f ",err);
dsuranr 0:d890d56973d1 157 pc.printf(" ");
dsuranr 0:d890d56973d1 158 pc.printf("MEDICION= %0.01f ",med);
dsuranr 0:d890d56973d1 159 pc.printf(" ");
dsuranr 0:d890d56973d1 160 pc.printf("SETPOINT= %0.01f ",spnum);
dsuranr 0:d890d56973d1 161 pc.printf(" ");
dsuranr 0:d890d56973d1 162 pc.printf("PID= %0.01f \r\n ",pid);
dsuranr 0:d890d56973d1 163 pc.printf(" ");
dsuranr 0:d890d56973d1 164 pc.printf("pwm= %0.01f ",pwm);
dsuranr 0:d890d56973d1 165 pc.printf(" ");
dsuranr 0:d890d56973d1 166
dsuranr 0:d890d56973d1 167
dsuranr 0:d890d56973d1 168 //Normalizacion de la salida
dsuranr 0:d890d56973d1 169 // se actualizan las variables *******************************************
dsuranr 0:d890d56973d1 170 err_v = err;
dsuranr 0:d890d56973d1 171 o = pid/3.3;
dsuranr 0:d890d56973d1 172
dsuranr 0:d890d56973d1 173
dsuranr 0:d890d56973d1 174 if(AP==1) {
dsuranr 0:d890d56973d1 175 AnalogOut u(PTE30);
dsuranr 0:d890d56973d1 176 u.write(o);
dsuranr 0:d890d56973d1 177 pc.printf("analogo");
dsuranr 0:d890d56973d1 178
dsuranr 0:d890d56973d1 179 } else if(AP==2) {
dsuranr 0:d890d56973d1 180 PwmOut u1(PTE20);
dsuranr 0:d890d56973d1 181 u1.write(pwm);
dsuranr 0:d890d56973d1 182 pc.printf("digital");
dsuranr 0:d890d56973d1 183
dsuranr 0:d890d56973d1 184 }
dsuranr 0:d890d56973d1 185
dsuranr 0:d890d56973d1 186
dsuranr 0:d890d56973d1 187 int med2=med*100;
dsuranr 0:d890d56973d1 188
dsuranr 0:d890d56973d1 189 int p, q, r;
dsuranr 0:d890d56973d1 190 if(med2<3.3) { //debo generar dos casos a APP inventor solo me recibe hex asi: 0xhhhh (4 cifras)
dsuranr 0:d890d56973d1 191 GSM.putc(0); //si el numero es hasta 255 se le ponen dos ceros adelante a la secuencia de bits
dsuranr 0:d890d56973d1 192 GSM.putc(p); //luego la cifra menos significativa
dsuranr 0:d890d56973d1 193 }
dsuranr 0:d890d56973d1 194 if(med2>3.3) { //pero si es mayor a 255 las cifras deben ser convertidas a un hex de dos bytes de la siguiente forma
dsuranr 0:d890d56973d1 195 q=med2/3.3; //calculo la cifra mas significativa
dsuranr 0:d890d56973d1 196 r=med2-q*3.3; //calculo la cifra menos significativa
dsuranr 0:d890d56973d1 197 GSM.putc(q); //las envio a la usart para que se las ponga al modulo bluetooth y la lleve al android
dsuranr 0:d890d56973d1 198 GSM.putc(r); //mas significativa primero, menos despues si no no funciona!!! y con la orden PUTC solo asi le envia binarios
dsuranr 0:d890d56973d1 199 }
dsuranr 0:d890d56973d1 200
dsuranr 0:d890d56973d1 201 //goto lop1;
dsuranr 0:d890d56973d1 202
dsuranr 0:d890d56973d1 203 if (GSM.readable()) {
dsuranr 0:d890d56973d1 204 readBuffer(buffer,25);
dsuranr 0:d890d56973d1 205 if (strncmp(buffer, "F", 1) == 0) {
dsuranr 0:d890d56973d1 206
dsuranr 0:d890d56973d1 207 ok=0;
dsuranr 0:d890d56973d1 208 goto LL;
dsuranr 0:d890d56973d1 209
dsuranr 0:d890d56973d1 210 //goto pc;
dsuranr 0:d890d56973d1 211 //cleanBufferB(bufferB,5);
dsuranr 0:d890d56973d1 212 //} else if(strncmp(buffer, "P", 1) == 0) {
dsuranr 0:d890d56973d1 213 } else if(h==1) {
dsuranr 0:d890d56973d1 214 // pc.printf("%s buffer", buffer);
dsuranr 0:d890d56973d1 215 //GSM.scanf(" %f, %f , %f , %f, %f ,%f,", &spnum, &kpnum, &kdnum,&kinum,&AP,&pwm);
dsuranr 0:d890d56973d1 216
dsuranr 0:d890d56973d1 217 pw:
dsuranr 0:d890d56973d1 218
dsuranr 0:d890d56973d1 219
dsuranr 0:d890d56973d1 220 if(AP==1) {
dsuranr 0:d890d56973d1 221 AnalogOut u(PTE30);
dsuranr 0:d890d56973d1 222 u.write(o);
dsuranr 0:d890d56973d1 223 pc.printf("analogo");
dsuranr 0:d890d56973d1 224
dsuranr 0:d890d56973d1 225 } else if(AP==2) {
dsuranr 0:d890d56973d1 226 PwmOut u1(PTE20);
dsuranr 0:d890d56973d1 227 u1.write(pwm);
dsuranr 0:d890d56973d1 228 pc.printf("digital");
dsuranr 0:d890d56973d1 229
dsuranr 0:d890d56973d1 230 }
dsuranr 0:d890d56973d1 231
dsuranr 0:d890d56973d1 232
dsuranr 0:d890d56973d1 233 //int med2=med*100;
dsuranr 0:d890d56973d1 234 float med2=med;
dsuranr 0:d890d56973d1 235 float med3;
dsuranr 0:d890d56973d1 236 // med3 = y.read();
dsuranr 0:d890d56973d1 237 med3=pwm;
dsuranr 0:d890d56973d1 238 int p, q, r;
dsuranr 0:d890d56973d1 239 if(med3<3.3) { //debo generar dos casos a APP inventor solo me recibe hex asi: 0xhhhh (4 cifras)
dsuranr 0:d890d56973d1 240 GSM.putc(0); //si el numero es hasta 255 se le ponen dos ceros adelante a la secuencia de bits
dsuranr 0:d890d56973d1 241 GSM.putc(p); //luego la cifra menos significativa
dsuranr 0:d890d56973d1 242 }
dsuranr 0:d890d56973d1 243 if(med3>3.3) { //pero si es mayor a 255 las cifras deben ser convertidas a un hex de dos bytes de la siguiente forma
dsuranr 0:d890d56973d1 244 //q=med2/3.3; //calculo la cifra mas significativa
dsuranr 0:d890d56973d1 245 //r=med2-q*3.3; //calculo la cifra menos significativa
dsuranr 0:d890d56973d1 246 q=med3 ; //calculo la cifra mas significativa
dsuranr 0:d890d56973d1 247 r=med3-q*3.3;
dsuranr 0:d890d56973d1 248 GSM.putc(q); //las envio a la usart para que se las ponga al modulo bluetooth y la lleve al android
dsuranr 0:d890d56973d1 249 GSM.putc(r); //mas significativa primero, menos despues si no no funciona!!! y con la orden PUTC solo asi le envia binarios
dsuranr 0:d890d56973d1 250 }
dsuranr 0:d890d56973d1 251
dsuranr 0:d890d56973d1 252
dsuranr 0:d890d56973d1 253 pc.printf("MEDICION= %0.01f \n",med3);
dsuranr 0:d890d56973d1 254 pc.printf(" ");
dsuranr 0:d890d56973d1 255 pc.printf("pwm= %0.01f \n",pwm);
dsuranr 0:d890d56973d1 256 pc.printf(" ");
dsuranr 0:d890d56973d1 257 pc.printf("buffer= %s ",buffer);//imprime el cero y el uno
dsuranr 0:d890d56973d1 258 wait_ms(600);
dsuranr 0:d890d56973d1 259
dsuranr 0:d890d56973d1 260 if (GSM.readable()) {
dsuranr 0:d890d56973d1 261 readBuffer(buffer,25);
dsuranr 0:d890d56973d1 262 if (strncmp(buffer, "F", 1) == 0) {
dsuranr 0:d890d56973d1 263
dsuranr 0:d890d56973d1 264 ok=0;
dsuranr 0:d890d56973d1 265 goto LL;
dsuranr 0:d890d56973d1 266
dsuranr 0:d890d56973d1 267
dsuranr 0:d890d56973d1 268 }
dsuranr 0:d890d56973d1 269 }
dsuranr 0:d890d56973d1 270
dsuranr 0:d890d56973d1 271
dsuranr 0:d890d56973d1 272
dsuranr 0:d890d56973d1 273 //}
dsuranr 0:d890d56973d1 274 goto pw;
dsuranr 0:d890d56973d1 275 wait_ms(300);
dsuranr 0:d890d56973d1 276
dsuranr 0:d890d56973d1 277 } else {
dsuranr 0:d890d56973d1 278 wait_ms(300);
dsuranr 0:d890d56973d1 279 goto lop1;
dsuranr 0:d890d56973d1 280 }
dsuranr 0:d890d56973d1 281 } else {
dsuranr 0:d890d56973d1 282 wait_ms(300);
dsuranr 0:d890d56973d1 283 goto lop1;
dsuranr 0:d890d56973d1 284 }
dsuranr 0:d890d56973d1 285
dsuranr 0:d890d56973d1 286 // se envia el valor pid a puerto analogico de salida (D/A) **************
dsuranr 0:d890d56973d1 287
dsuranr 0:d890d56973d1 288 // se repite el ciclo
dsuranr 0:d890d56973d1 289
dsuranr 0:d890d56973d1 290
dsuranr 0:d890d56973d1 291 // } // DEL IF OK
dsuranr 0:d890d56973d1 292 goto pc;
dsuranr 0:d890d56973d1 293 } // del if DE LOS DATOS
dsuranr 0:d890d56973d1 294 //goto pc;
dsuranr 0:d890d56973d1 295
dsuranr 0:d890d56973d1 296
dsuranr 0:d890d56973d1 297
dsuranr 0:d890d56973d1 298 // cleanBuffer(buffer,10);
dsuranr 0:d890d56973d1 299
dsuranr 0:d890d56973d1 300
dsuranr 0:d890d56973d1 301 } //WHILE
dsuranr 0:d890d56973d1 302
dsuranr 0:d890d56973d1 303
dsuranr 0:d890d56973d1 304
dsuranr 0:d890d56973d1 305 }//INT