![](/media/cache/img/default_profile.jpg.50x50_q85.jpg)
Aplicación para un controlador PID con Interfaz en AppInventor
main.cpp@1:608758dfdcfd, 2019-07-24 (annotated)
- Committer:
- Giovani_Cardona
- Date:
- Wed Jul 24 18:58:25 2019 +0000
- Revision:
- 1:608758dfdcfd
- Parent:
- 0:53194b344442
; ;
Who changed what in which revision?
User | Revision | Line number | New contents of line |
---|---|---|---|
Giovani_Cardona | 1:608758dfdcfd | 1 | /*Práctica 3 - Procesadores 2019-1 |
Giovani_Cardona | 1:608758dfdcfd | 2 | Giovani Cardona Sánchez |
Giovani_Cardona | 1:608758dfdcfd | 3 | Mateo Valencia Diaz |
Giovani_Cardona | 1:608758dfdcfd | 4 | Verónica Ríos Vargas |
Giovani_Cardona | 1:608758dfdcfd | 5 | Juan Esteban Rodriguez Oquendo |
Giovani_Cardona | 1:608758dfdcfd | 6 | Juan Camilo Pérez Estrada |
Giovani_Cardona | 1:608758dfdcfd | 7 | */ |
Giovani_Cardona | 1:608758dfdcfd | 8 | |
Giovani_Cardona | 0:53194b344442 | 9 | #include "mbed.h" |
Giovani_Cardona | 0:53194b344442 | 10 | #include "string.h" |
Giovani_Cardona | 0:53194b344442 | 11 | |
Giovani_Cardona | 0:53194b344442 | 12 | AnalogIn y(PTB3);//entrada analoga |
Giovani_Cardona | 1:608758dfdcfd | 13 | AnalogOut u(PTE30);//salida analoga |
Giovani_Cardona | 1:608758dfdcfd | 14 | Serial pc(USBTX, USBRX, "pc"); |
Giovani_Cardona | 0:53194b344442 | 15 | Serial HC06(PTE0,PTE1); |
Giovani_Cardona | 1:608758dfdcfd | 16 | PwmOut out(PTC3); |
Giovani_Cardona | 0:53194b344442 | 17 | |
Giovani_Cardona | 1:608758dfdcfd | 18 | |
Giovani_Cardona | 1:608758dfdcfd | 19 | char buffer[30]; |
Giovani_Cardona | 0:53194b344442 | 20 | char c; |
Giovani_Cardona | 0:53194b344442 | 21 | int readptr = 0; |
Giovani_Cardona | 0:53194b344442 | 22 | int Kp, Ki, Kd, Sp; |
Giovani_Cardona | 1:608758dfdcfd | 23 | float pid,o,ai,ad,ap,med,err,err_v,d; |
Giovani_Cardona | 1:608758dfdcfd | 24 | |
Giovani_Cardona | 1:608758dfdcfd | 25 | Timer t; //VALOR DEL TIEMPO |
Giovani_Cardona | 1:608758dfdcfd | 26 | Timer t2; |
Giovani_Cardona | 1:608758dfdcfd | 27 | int count; |
Giovani_Cardona | 1:608758dfdcfd | 28 | int j,k; |
Giovani_Cardona | 1:608758dfdcfd | 29 | int mode = -1, val; |
Giovani_Cardona | 1:608758dfdcfd | 30 | char masc,b; |
Giovani_Cardona | 1:608758dfdcfd | 31 | int duty; |
Giovani_Cardona | 0:53194b344442 | 32 | |
Giovani_Cardona | 1:608758dfdcfd | 33 | //------------------------------Funciones----------------------------------------------// |
Giovani_Cardona | 1:608758dfdcfd | 34 | int readBuffer(char *buffer,int count) //esta funcion lee un bufer de datos |
Giovani_Cardona | 1:608758dfdcfd | 35 | { |
Giovani_Cardona | 1:608758dfdcfd | 36 | int i=0; |
Giovani_Cardona | 1:608758dfdcfd | 37 | t.start(); //CUENTA EL TIEMPO DE CONEXION E INICIA |
Giovani_Cardona | 1:608758dfdcfd | 38 | while(1) { |
Giovani_Cardona | 1:608758dfdcfd | 39 | while (HC06.readable()) { |
Giovani_Cardona | 1:608758dfdcfd | 40 | char c = HC06.getc(); |
Giovani_Cardona | 1:608758dfdcfd | 41 | if (c == '\r' || c == '\n') c = '$';//si se envia fin de linea o de caracter inserta $ |
Giovani_Cardona | 1:608758dfdcfd | 42 | buffer[i++] = c;//mete al bufer el caracter leido |
Giovani_Cardona | 1:608758dfdcfd | 43 | if(i > count)break;//sale del loop si ya detecto terminacion |
Giovani_Cardona | 1:608758dfdcfd | 44 | //pc.printf("c: %c",c); |
Giovani_Cardona | 1:608758dfdcfd | 45 | } |
Giovani_Cardona | 1:608758dfdcfd | 46 | if(i > count)break; |
Giovani_Cardona | 1:608758dfdcfd | 47 | |
Giovani_Cardona | 1:608758dfdcfd | 48 | if(t.read() > 1) { //MAS DE UN SEGUNDO DE ESPERA SE SALE Y REINICA EL RELOJ Y SE SALE |
Giovani_Cardona | 1:608758dfdcfd | 49 | t.stop(); |
Giovani_Cardona | 1:608758dfdcfd | 50 | t.reset(); |
Giovani_Cardona | 1:608758dfdcfd | 51 | //pc.printf("\ni: %d",i); |
Giovani_Cardona | 1:608758dfdcfd | 52 | //pc.printf("\ncount: %d",count); |
Giovani_Cardona | 1:608758dfdcfd | 53 | //pc.printf("\nc: %c",c); |
Giovani_Cardona | 1:608758dfdcfd | 54 | break; |
Giovani_Cardona | 1:608758dfdcfd | 55 | } |
Giovani_Cardona | 0:53194b344442 | 56 | } |
Giovani_Cardona | 1:608758dfdcfd | 57 | return 0; |
Giovani_Cardona | 0:53194b344442 | 58 | } |
Giovani_Cardona | 0:53194b344442 | 59 | |
Giovani_Cardona | 1:608758dfdcfd | 60 | //-----// |
Giovani_Cardona | 0:53194b344442 | 61 | void cleanBuffer(char *buffer, int count) //esta funcion limpia el bufer |
Giovani_Cardona | 0:53194b344442 | 62 | { |
Giovani_Cardona | 0:53194b344442 | 63 | for(int i=0; i < count; i++) { |
Giovani_Cardona | 0:53194b344442 | 64 | buffer[i] = '\0'; |
Giovani_Cardona | 0:53194b344442 | 65 | } |
Giovani_Cardona | 0:53194b344442 | 66 | } |
Giovani_Cardona | 0:53194b344442 | 67 | |
Giovani_Cardona | 1:608758dfdcfd | 68 | //-----// |
Giovani_Cardona | 1:608758dfdcfd | 69 | void PID(int Sp, int Kp, int Ki, int Kd ) |
Giovani_Cardona | 1:608758dfdcfd | 70 | { |
Giovani_Cardona | 1:608758dfdcfd | 71 | // CICLO PRINCIPAL CONTROLADOR PID |
Giovani_Cardona | 1:608758dfdcfd | 72 | med = y.read()*999; |
Giovani_Cardona | 1:608758dfdcfd | 73 | err = (Sp-med); //se calcula el error |
Giovani_Cardona | 1:608758dfdcfd | 74 | ap = Kp*err*0.01f; //se calcula la accion proporcinal |
Giovani_Cardona | 1:608758dfdcfd | 75 | ai =(Ki*err*0.01f)+ai; //calculo de la integral del error |
Giovani_Cardona | 1:608758dfdcfd | 76 | ad = Kd*(err-err_v)*0.01f; //calculo de la accion derivativa |
Giovani_Cardona | 1:608758dfdcfd | 77 | pid = (ap+ai+ad); |
Giovani_Cardona | 1:608758dfdcfd | 78 | // se verifica que pid sea positivo ************************************** |
Giovani_Cardona | 1:608758dfdcfd | 79 | if(pid<=0){ |
Giovani_Cardona | 1:608758dfdcfd | 80 | pid=0; |
Giovani_Cardona | 1:608758dfdcfd | 81 | } |
Giovani_Cardona | 1:608758dfdcfd | 82 | // se verifica que pid sea menor o igual al valor maximo ***************** |
Giovani_Cardona | 1:608758dfdcfd | 83 | if (pid > 999){ |
Giovani_Cardona | 1:608758dfdcfd | 84 | pid=999; |
Giovani_Cardona | 1:608758dfdcfd | 85 | } |
Giovani_Cardona | 1:608758dfdcfd | 86 | |
Giovani_Cardona | 1:608758dfdcfd | 87 | //Normalizacion de la salida |
Giovani_Cardona | 1:608758dfdcfd | 88 | // se actualizan las variables ******************************************* |
Giovani_Cardona | 1:608758dfdcfd | 89 | err_v = err; |
Giovani_Cardona | 1:608758dfdcfd | 90 | o = pid/999; |
Giovani_Cardona | 1:608758dfdcfd | 91 | u.write(o); // se envia el valor pid a puerto analogico de salida (D/A) ************** |
Giovani_Cardona | 1:608758dfdcfd | 92 | |
Giovani_Cardona | 1:608758dfdcfd | 93 | if(med<256){ //debo generar dos casos a APP inventor solo me recibe hex asi: 0xhhhh (4 cifras) |
Giovani_Cardona | 1:608758dfdcfd | 94 | HC06.putc(0); //si el numero es hasta 255 se le ponen dos ceros adelante a la secuencia de bits |
Giovani_Cardona | 1:608758dfdcfd | 95 | HC06.putc(med); //luego la cifra menos significativa |
Giovani_Cardona | 1:608758dfdcfd | 96 | } |
Giovani_Cardona | 1:608758dfdcfd | 97 | if(med>255){ //pero si es mayor a 255 las cifras deben ser convertidas a un hex de dos bytes de la siguiente forma |
Giovani_Cardona | 1:608758dfdcfd | 98 | j=med/256; //calculo la cifra mas significativa |
Giovani_Cardona | 1:608758dfdcfd | 99 | k=med-j*256; //calculo la cifra menos significativa |
Giovani_Cardona | 1:608758dfdcfd | 100 | HC06.putc(j); //las envio a la usart para que se las ponga al modulo bluetooth y la lleve al android |
Giovani_Cardona | 1:608758dfdcfd | 101 | HC06.putc(k); //mas significativa primero, menos despues si no no funciona!!! y con la orden PUTC solo asi le envia binarios |
Giovani_Cardona | 1:608758dfdcfd | 102 | } |
Giovani_Cardona | 1:608758dfdcfd | 103 | //pc.printf("SetPoint\tEntrada\t\tError\t\tSalida\n"); |
Giovani_Cardona | 1:608758dfdcfd | 104 | //pc.printf("%d\t\t%0.2f\t\t%0.2f\t\t%0.2f\n",Sp,med,err,pid); |
Giovani_Cardona | 1:608758dfdcfd | 105 | //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 | 1:608758dfdcfd | 106 | // se repite el ciclo |
Giovani_Cardona | 1:608758dfdcfd | 107 | //wait_ms(100); |
Giovani_Cardona | 1:608758dfdcfd | 108 | } |
Giovani_Cardona | 1:608758dfdcfd | 109 | //-----------------------------------------------------------------------------------// |
Giovani_Cardona | 1:608758dfdcfd | 110 | |
Giovani_Cardona | 0:53194b344442 | 111 | int main(){ |
Giovani_Cardona | 0:53194b344442 | 112 | |
Giovani_Cardona | 1:608758dfdcfd | 113 | out.period_ms(200); |
Giovani_Cardona | 0:53194b344442 | 114 | // Bluetooth |
Giovani_Cardona | 0:53194b344442 | 115 | HC06.baud(9600); |
Giovani_Cardona | 1:608758dfdcfd | 116 | HC06.format(8,Serial::None,1); |
Giovani_Cardona | 0:53194b344442 | 117 | |
Giovani_Cardona | 1:608758dfdcfd | 118 | while(1){ |
Giovani_Cardona | 1:608758dfdcfd | 119 | if (HC06.readable()) { |
Giovani_Cardona | 1:608758dfdcfd | 120 | cleanBuffer(buffer,30); |
Giovani_Cardona | 1:608758dfdcfd | 121 | readBuffer(buffer,30); |
Giovani_Cardona | 1:608758dfdcfd | 122 | sscanf(buffer,"%c %d %d %d %d",&masc,&Sp,&Kp,&Ki,&Kd); |
Giovani_Cardona | 1:608758dfdcfd | 123 | pc.printf("%c %d %d %d %d\n",masc,Sp,Kp,Ki,Kd); |
Giovani_Cardona | 1:608758dfdcfd | 124 | |
Giovani_Cardona | 1:608758dfdcfd | 125 | pc.printf("buffer= %s\n\r ",buffer); //imprime el bufer |
Giovani_Cardona | 1:608758dfdcfd | 126 | //pc.printf("buffer= %c %c %c\n\r ",buffer[0],buffer[1],buffer[2]);//imprime el cero y el uno |
Giovani_Cardona | 1:608758dfdcfd | 127 | |
Giovani_Cardona | 1:608758dfdcfd | 128 | t2.start(); |
Giovani_Cardona | 1:608758dfdcfd | 129 | cleanBuffer(buffer,30); |
Giovani_Cardona | 1:608758dfdcfd | 130 | if (masc == 'A'){ |
Giovani_Cardona | 1:608758dfdcfd | 131 | //sscanf(buffer,"%c %d %d %d %d",&masc,&Sp, &Kp, &Ki, &Kd); |
Giovani_Cardona | 1:608758dfdcfd | 132 | //pc.printf("%c %d %d %d %d\n",masc,Sp,Kp,Ki,Kd); |
Giovani_Cardona | 1:608758dfdcfd | 133 | if(Sp<=0){Sp=0;} |
Giovani_Cardona | 1:608758dfdcfd | 134 | if(Sp > 999){Sp=999;} |
Giovani_Cardona | 1:608758dfdcfd | 135 | mode = 0; |
Giovani_Cardona | 1:608758dfdcfd | 136 | } |
Giovani_Cardona | 1:608758dfdcfd | 137 | else if (masc == 'B'){ |
Giovani_Cardona | 1:608758dfdcfd | 138 | //sscanf(buffer,"%c %d %d",&masc,&mode,&val); |
Giovani_Cardona | 1:608758dfdcfd | 139 | //pc.printf("%c %d %d\n",masc,mode,val); |
Giovani_Cardona | 1:608758dfdcfd | 140 | mode = Sp; |
Giovani_Cardona | 1:608758dfdcfd | 141 | val = Kp; |
Giovani_Cardona | 1:608758dfdcfd | 142 | } |
Giovani_Cardona | 1:608758dfdcfd | 143 | } |
Giovani_Cardona | 0:53194b344442 | 144 | |
Giovani_Cardona | 1:608758dfdcfd | 145 | if (mode == 0){ |
Giovani_Cardona | 1:608758dfdcfd | 146 | if(t2.read_ms() > 300){ |
Giovani_Cardona | 1:608758dfdcfd | 147 | PID(Sp, Kp, Ki, Kd); |
Giovani_Cardona | 1:608758dfdcfd | 148 | t2.reset(); |
Giovani_Cardona | 1:608758dfdcfd | 149 | } |
Giovani_Cardona | 1:608758dfdcfd | 150 | } |
Giovani_Cardona | 1:608758dfdcfd | 151 | else if(mode == 1){ |
Giovani_Cardona | 1:608758dfdcfd | 152 | if(t2.read_ms() > 300){ |
Giovani_Cardona | 1:608758dfdcfd | 153 | med = y.read()*999; |
Giovani_Cardona | 1:608758dfdcfd | 154 | duty = (val/4); |
Giovani_Cardona | 1:608758dfdcfd | 155 | //pc.printf("\nduty: %d",duty); |
Giovani_Cardona | 1:608758dfdcfd | 156 | out.pulsewidth_ms(duty); |
Giovani_Cardona | 0:53194b344442 | 157 | |
Giovani_Cardona | 1:608758dfdcfd | 158 | if(med<256){ //debo generar dos casos a APP inventor solo me recibe hex asi: 0xhhhh (4 cifras) |
Giovani_Cardona | 1:608758dfdcfd | 159 | HC06.putc(0); //si el numero es hasta 255 se le ponen dos ceros adelante a la secuencia de bits |
Giovani_Cardona | 1:608758dfdcfd | 160 | HC06.putc(med); //luego la cifra menos significativa |
Giovani_Cardona | 0:53194b344442 | 161 | } |
Giovani_Cardona | 1:608758dfdcfd | 162 | if(med>255){ //pero si es mayor a 255 las cifras deben ser convertidas a un hex de dos bytes de la siguiente forma |
Giovani_Cardona | 1:608758dfdcfd | 163 | j=med/256; //calculo la cifra mas significativa |
Giovani_Cardona | 1:608758dfdcfd | 164 | k=med-j*256; //calculo la cifra menos significativa |
Giovani_Cardona | 1:608758dfdcfd | 165 | HC06.putc(j); //las envio a la usart para que se las ponga al modulo bluetooth y la lleve al android |
Giovani_Cardona | 1:608758dfdcfd | 166 | HC06.putc(k); //mas significativa primero, menos despues si no no funciona!!! y con la orden PUTC solo asi le envia binarios |
Giovani_Cardona | 1:608758dfdcfd | 167 | } |
Giovani_Cardona | 1:608758dfdcfd | 168 | t2.reset(); |
Giovani_Cardona | 1:608758dfdcfd | 169 | } |
Giovani_Cardona | 0:53194b344442 | 170 | } |
Giovani_Cardona | 1:608758dfdcfd | 171 | else if(mode == 2) |
Giovani_Cardona | 1:608758dfdcfd | 172 | { |
Giovani_Cardona | 1:608758dfdcfd | 173 | if(t2.read_ms() > 300){ |
Giovani_Cardona | 1:608758dfdcfd | 174 | med = y.read()*999; |
Giovani_Cardona | 1:608758dfdcfd | 175 | d = (val/100.0); |
Giovani_Cardona | 1:608758dfdcfd | 176 | u.write(d); |
Giovani_Cardona | 1:608758dfdcfd | 177 | |
Giovani_Cardona | 1:608758dfdcfd | 178 | if(med<256){ //debo generar dos casos a APP inventor solo me recibe hex asi: 0xhhhh (4 cifras) |
Giovani_Cardona | 1:608758dfdcfd | 179 | HC06.putc(0); //si el numero es hasta 255 se le ponen dos ceros adelante a la secuencia de bits |
Giovani_Cardona | 1:608758dfdcfd | 180 | HC06.putc(med); //luego la cifra menos significativa |
Giovani_Cardona | 1:608758dfdcfd | 181 | } |
Giovani_Cardona | 1:608758dfdcfd | 182 | if(med>255){ //pero si es mayor a 255 las cifras deben ser convertidas a un hex de dos bytes de la siguiente forma |
Giovani_Cardona | 1:608758dfdcfd | 183 | j=med/256; //calculo la cifra mas significativa |
Giovani_Cardona | 1:608758dfdcfd | 184 | k=med-j*256; //calculo la cifra menos significativa |
Giovani_Cardona | 1:608758dfdcfd | 185 | HC06.putc(j); //las envio a la usart para que se las ponga al modulo bluetooth y la lleve al android |
Giovani_Cardona | 1:608758dfdcfd | 186 | HC06.putc(k); //mas significativa primero, menos despues si no no funciona!!! y con la orden PUTC solo asi le envia binarios |
Giovani_Cardona | 1:608758dfdcfd | 187 | } |
Giovani_Cardona | 1:608758dfdcfd | 188 | t2.reset(); |
Giovani_Cardona | 1:608758dfdcfd | 189 | } |
Giovani_Cardona | 0:53194b344442 | 190 | } |
Giovani_Cardona | 1:608758dfdcfd | 191 | //cleanBuffer(buffer,30); |
Giovani_Cardona | 1:608758dfdcfd | 192 | } |
Giovani_Cardona | 1:608758dfdcfd | 193 | } |