Procesadores 2017 / Mbed 2 deprecated PID_ZN_ENCODER

PID que funciona con encoder y tiene autotunning.

Dependencies:   QEI TextLCD mbed

Fork of TAREA_4_PROCESADORES by john lopez

main.cpp@1:9ca362d07dd0, 2014-04-08 (annotated)

Committer:
lopjohn_26
Date:
Tue Apr 08 19:02:44 2014 +0000
Revision:
1:9ca362d07dd0
Parent:
0:dd68101b1b99
Child:
2:f854af43e0c2
sonido del bot?n encoder

Who changed what in which revision?

UserRevisionLine numberNew contents of line
Wilmar87 0:dd68101b1b99 1 #include "mbed.h"
Wilmar87 0:dd68101b1b99 2 #include "TextLCD.h"
Wilmar87 0:dd68101b1b99 3 #include "DebouncedIn.h"
Wilmar87 0:dd68101b1b99 4 #include "QEI.h"
Wilmar87 0:dd68101b1b99 5
Wilmar87 0:dd68101b1b99 6 AnalogIn Vin(PTC2);
Wilmar87 0:dd68101b1b99 7 AnalogOut Vout(PTE30);
lopjohn_26 1:9ca362d07dd0 8 PwmOut sound(PTA12);
Wilmar87 0:dd68101b1b99 9
Wilmar87 0:dd68101b1b99 10 TextLCD lcd(PTB10, PTB11, PTE2, PTE3, PTE4, PTE5); //Puertos LCD rs, e, d4, d5, d6, d7
lopjohn_26 1:9ca362d07dd0 11 QEI Encoder (PTA1, PTA2, NC, 624); //Puertos de la tarjeta asignados para el Encoder
Wilmar87 0:dd68101b1b99 12
Wilmar87 0:dd68101b1b99 13 DigitalOut led1(LED1); //led de cambio de posición
Wilmar87 0:dd68101b1b99 14 DigitalOut led2(LED2); //led incremento de parámetros
Wilmar87 0:dd68101b1b99 15 DigitalOut led3(LED3); //led decremento de parámetros
lopjohn_26 1:9ca362d07dd0 16 DebouncedIn bot1(PTC12); //cambiar la posición (CONECTO ESTA ENTRADA A LA ULTIMA SALIDA DEL ENCONDER, ASI SE UTILIZARA EL BOTON DEL ENCODER)
Wilmar87 0:dd68101b1b99 17 DebouncedIn bot4(PTC17); //salida de bucle
Wilmar87 0:dd68101b1b99 18
Wilmar87 0:dd68101b1b99 19 //Códigos LCD
Wilmar87 0:dd68101b1b99 20 int C1=0x0E; // Muestra el cursor
Wilmar87 0:dd68101b1b99 21 // 0x18; // desplazamiento izquierda
Wilmar87 0:dd68101b1b99 22 // 0x1A; // desplazamiento derecha
Wilmar87 0:dd68101b1b99 23 int C4=0x0C;
Wilmar87 0:dd68101b1b99 24
Wilmar87 0:dd68101b1b99 25
Wilmar87 0:dd68101b1b99 26 int a, kp, ki, kd, sp, ciclo, valor, v; // indice de la variable
Wilmar87 0:dd68101b1b99 27 float med, sp0, pid, ap, err, ai, ad,err_v;
Wilmar87 0:dd68101b1b99 28
Wilmar87 0:dd68101b1b99 29
Wilmar87 0:dd68101b1b99 30
Wilmar87 0:dd68101b1b99 31 int main() {
Wilmar87 0:dd68101b1b99 32
Wilmar87 0:dd68101b1b99 33 led1=led2=led3=1;
Wilmar87 0:dd68101b1b99 34 lcd.printf("Control PID");
Wilmar87 0:dd68101b1b99 35 wait(1.5);
Wilmar87 0:dd68101b1b99 36 lcd.cls();
Wilmar87 0:dd68101b1b99 37
Wilmar87 0:dd68101b1b99 38 lcd.writeCommand(C1); //comando para mostrar el cursor en el LCD
Wilmar87 0:dd68101b1b99 39
Wilmar87 0:dd68101b1b99 40 lcd.locate(0,0); // Ubica e imprime nombre de los parámetros en del PID en la pantalla LCD
Wilmar87 0:dd68101b1b99 41 lcd.printf("Sp=");
Wilmar87 0:dd68101b1b99 42 lcd.locate(8,0);
Wilmar87 0:dd68101b1b99 43 lcd.printf("Kp=");
Wilmar87 0:dd68101b1b99 44 lcd.locate(0,1);
Wilmar87 0:dd68101b1b99 45 lcd.printf("Ki=");
Wilmar87 0:dd68101b1b99 46 lcd.locate(8,1);
Wilmar87 0:dd68101b1b99 47 lcd.printf("Kd=");
Wilmar87 0:dd68101b1b99 48
Wilmar87 0:dd68101b1b99 49
Wilmar87 0:dd68101b1b99 50 //Inicio del ciclo
Wilmar87 0:dd68101b1b99 51
Wilmar87 0:dd68101b1b99 52 while(1) {
Wilmar87 0:dd68101b1b99 53 if (bot1.falling()) { //----------------- Aumenta de posición el cursor a la primera línea de menu
lopjohn_26 1:9ca362d07dd0 54
lopjohn_26 1:9ca362d07dd0 55 sound=1.0f;
lopjohn_26 1:9ca362d07dd0 56 wait(0.1);
lopjohn_26 1:9ca362d07dd0 57 sound=0;
lopjohn_26 1:9ca362d07dd0 58
Wilmar87 0:dd68101b1b99 59 led1=0;
Wilmar87 0:dd68101b1b99 60 wait(.15);
Wilmar87 0:dd68101b1b99 61 led1=1;
Wilmar87 0:dd68101b1b99 62 ++a;
Wilmar87 0:dd68101b1b99 63 }
Wilmar87 0:dd68101b1b99 64
Wilmar87 0:dd68101b1b99 65 valor = Encoder.getPulses(); //------------- Asigna el valor de los pulsos del encoder a una variable llamada "valor"
Wilmar87 0:dd68101b1b99 66
Wilmar87 0:dd68101b1b99 67 switch(a) {
Wilmar87 0:dd68101b1b99 68
Wilmar87 0:dd68101b1b99 69 case 0:
Wilmar87 0:dd68101b1b99 70
Wilmar87 0:dd68101b1b99 71 sp = sp + valor; //------------- Asigna el valor del encoder al parámetro sp y tiene en cuenta el valor anterior
Wilmar87 0:dd68101b1b99 72 Encoder.reset(); //------------- Resetea el valor del encoder
Wilmar87 0:dd68101b1b99 73 if (sp<0){
Wilmar87 0:dd68101b1b99 74 sp=0; //------------- No se admite valores negativos
Wilmar87 0:dd68101b1b99 75 }
Wilmar87 0:dd68101b1b99 76
Wilmar87 0:dd68101b1b99 77 lcd.locate(2,0); //------------- Ubica e imprime el parámetro "sp" en la pantalla LCD
Wilmar87 0:dd68101b1b99 78 lcd.printf("= ",sp);
Wilmar87 0:dd68101b1b99 79 lcd.locate(3,0);
Wilmar87 0:dd68101b1b99 80 lcd.printf("%i",sp);
Wilmar87 0:dd68101b1b99 81 wait(0.15);
Wilmar87 0:dd68101b1b99 82
Wilmar87 0:dd68101b1b99 83 if (bot1.falling()) { //--------------Aumenta de posición el cursor a la segunda línea de menu
lopjohn_26 1:9ca362d07dd0 84
lopjohn_26 1:9ca362d07dd0 85 sound=1.0f;
lopjohn_26 1:9ca362d07dd0 86 wait(0.1);
lopjohn_26 1:9ca362d07dd0 87 sound=0;
lopjohn_26 1:9ca362d07dd0 88
Wilmar87 0:dd68101b1b99 89 a=1;
Wilmar87 0:dd68101b1b99 90 led1=0;
Wilmar87 0:dd68101b1b99 91 wait(.15);
Wilmar87 0:dd68101b1b99 92 led1=1;
Wilmar87 0:dd68101b1b99 93 Encoder.reset(); //------------- Resetea el valor del encoder
Wilmar87 0:dd68101b1b99 94 }
Wilmar87 0:dd68101b1b99 95 break;
Wilmar87 0:dd68101b1b99 96
Wilmar87 0:dd68101b1b99 97 case 1:
Wilmar87 0:dd68101b1b99 98
Wilmar87 0:dd68101b1b99 99 kp = kp + valor; //------------- Asigna el valor del encoder al parámetro kp y tiene en cuenta el valor anterior
Wilmar87 0:dd68101b1b99 100 Encoder.reset(); //------------- Resetea el valor del encoder
Wilmar87 0:dd68101b1b99 101 if (kp<0){
Wilmar87 0:dd68101b1b99 102 kp=0; //------------- No se admite valores negativos
Wilmar87 0:dd68101b1b99 103 }
Wilmar87 0:dd68101b1b99 104
Wilmar87 0:dd68101b1b99 105 lcd.locate(10,0); //------------- Ubica e imprime el parámetro "sp" en la pantalla LCD
Wilmar87 0:dd68101b1b99 106 lcd.printf("= ",kp);
Wilmar87 0:dd68101b1b99 107 lcd.locate(11,0);
Wilmar87 0:dd68101b1b99 108 lcd.printf("%i",kp);
Wilmar87 0:dd68101b1b99 109 wait(0.15);
Wilmar87 0:dd68101b1b99 110
Wilmar87 0:dd68101b1b99 111 if (bot1.falling()) { //--------------Aumenta de posición el cursor a la tercera línea de menu
lopjohn_26 1:9ca362d07dd0 112
lopjohn_26 1:9ca362d07dd0 113 sound=1.0f;
lopjohn_26 1:9ca362d07dd0 114 wait(0.1);
lopjohn_26 1:9ca362d07dd0 115 sound=0;
lopjohn_26 1:9ca362d07dd0 116
Wilmar87 0:dd68101b1b99 117 a=2;
Wilmar87 0:dd68101b1b99 118 led1=0;
Wilmar87 0:dd68101b1b99 119 wait(.15);
Wilmar87 0:dd68101b1b99 120 led1=1;
Wilmar87 0:dd68101b1b99 121 Encoder.reset(); //------------- Resetea el valor del encoder
Wilmar87 0:dd68101b1b99 122 }
Wilmar87 0:dd68101b1b99 123 break;
Wilmar87 0:dd68101b1b99 124
Wilmar87 0:dd68101b1b99 125 case 2:
Wilmar87 0:dd68101b1b99 126
Wilmar87 0:dd68101b1b99 127 ki = ki + valor; //------------- Asigna el valor del encoder al parámetro ki y tiene en cuenta el valor anterior
Wilmar87 0:dd68101b1b99 128 Encoder.reset(); //------------- Resetea el valor del encoder
Wilmar87 0:dd68101b1b99 129 if (ki<0){
Wilmar87 0:dd68101b1b99 130 ki=0; //------------- No se admite valores negativos
Wilmar87 0:dd68101b1b99 131 }
Wilmar87 0:dd68101b1b99 132
Wilmar87 0:dd68101b1b99 133 lcd.locate(2,1); //------------- Ubica e imprime el parámetro "sp" en la pantalla LCD
Wilmar87 0:dd68101b1b99 134 lcd.printf("= ",ki);
Wilmar87 0:dd68101b1b99 135 lcd.locate(3,1);
Wilmar87 0:dd68101b1b99 136 lcd.printf("%i",ki);
Wilmar87 0:dd68101b1b99 137 wait(0.15);
Wilmar87 0:dd68101b1b99 138
Wilmar87 0:dd68101b1b99 139 if (bot1.falling()) { //--------------Aumenta de posición el cursor a la cuarta línea de menu
lopjohn_26 1:9ca362d07dd0 140
lopjohn_26 1:9ca362d07dd0 141 sound=1.0f;
lopjohn_26 1:9ca362d07dd0 142 wait(0.1);
lopjohn_26 1:9ca362d07dd0 143 sound=0;
lopjohn_26 1:9ca362d07dd0 144
Wilmar87 0:dd68101b1b99 145 a=3;
Wilmar87 0:dd68101b1b99 146 led1=0;
Wilmar87 0:dd68101b1b99 147 wait(.15);
Wilmar87 0:dd68101b1b99 148 led1=1;
Wilmar87 0:dd68101b1b99 149 Encoder.reset(); //------------- Resetea el valor del encoder
Wilmar87 0:dd68101b1b99 150 }
Wilmar87 0:dd68101b1b99 151 break;
Wilmar87 0:dd68101b1b99 152
Wilmar87 0:dd68101b1b99 153 case 3:
Wilmar87 0:dd68101b1b99 154 kd = kd + valor; //------------- Asigna el valor del encoder al parámetro kd y tiene en cuenta el valor anterior
Wilmar87 0:dd68101b1b99 155 Encoder.reset(); //------------- Resetea el valor del encoder
Wilmar87 0:dd68101b1b99 156 if (kd<0){
Wilmar87 0:dd68101b1b99 157 kd=0; //------------- No se admite valores negativos
Wilmar87 0:dd68101b1b99 158 }
Wilmar87 0:dd68101b1b99 159 lcd.locate(10,1);//------------- Ubica e imprime el parámetro "kd" en la pantalla LCD
Wilmar87 0:dd68101b1b99 160 lcd.printf("= ",kd);
Wilmar87 0:dd68101b1b99 161 lcd.locate(11,1);
Wilmar87 0:dd68101b1b99 162 lcd.printf("%i",kd);
Wilmar87 0:dd68101b1b99 163 wait(0.15);
Wilmar87 0:dd68101b1b99 164
Wilmar87 0:dd68101b1b99 165 if (bot1.falling()) { //--------------Aumenta de posición el cursor a la cuarta línea de menu
lopjohn_26 1:9ca362d07dd0 166
lopjohn_26 1:9ca362d07dd0 167 sound=1.0f;
lopjohn_26 1:9ca362d07dd0 168 wait(0.1);
lopjohn_26 1:9ca362d07dd0 169 sound=0;
lopjohn_26 1:9ca362d07dd0 170
Wilmar87 0:dd68101b1b99 171 a=0;
Wilmar87 0:dd68101b1b99 172 led1=0;
Wilmar87 0:dd68101b1b99 173 wait(.15);
Wilmar87 0:dd68101b1b99 174 led1=1;
Wilmar87 0:dd68101b1b99 175 Encoder.reset(); //------------- Resetea el valor del encoder
Wilmar87 0:dd68101b1b99 176 }
Wilmar87 0:dd68101b1b99 177 break;
Wilmar87 0:dd68101b1b99 178
Wilmar87 0:dd68101b1b99 179 }
Wilmar87 0:dd68101b1b99 180
Wilmar87 0:dd68101b1b99 181 if (bot4.falling()){
Wilmar87 0:dd68101b1b99 182 led1=led2=led3=0; //------------- Flash para salir del bucle
Wilmar87 0:dd68101b1b99 183 wait(0.25);
Wilmar87 0:dd68101b1b99 184 led1=led2=led3=1;
Wilmar87 0:dd68101b1b99 185 break; //------------- sale del bucle de la pantalla
Wilmar87 0:dd68101b1b99 186 }
Wilmar87 0:dd68101b1b99 187 }
Wilmar87 0:dd68101b1b99 188
Wilmar87 0:dd68101b1b99 189 //---------------------------------------------------------------------------------------------------------------------------------------------------
Wilmar87 0:dd68101b1b99 190 lcd.writeCommand(C4); //Quita el cursor bajo de la pantalla LCD
Wilmar87 0:dd68101b1b99 191 lcd.cls(); //borra la pantalla
Wilmar87 0:dd68101b1b99 192 lcd.printf("GUARDADOS!");
Wilmar87 0:dd68101b1b99 193 wait(2);
Wilmar87 0:dd68101b1b99 194 lcd.cls();
Wilmar87 0:dd68101b1b99 195 lcd.printf("INICIA EL PID");
Wilmar87 0:dd68101b1b99 196 wait(2);
Wilmar87 0:dd68101b1b99 197 // se imprimen los parches del control *****************************************
Wilmar87 0:dd68101b1b99 198 lcd.cls();
Wilmar87 0:dd68101b1b99 199
Wilmar87 0:dd68101b1b99 200
Wilmar87 0:dd68101b1b99 201 lcd.printf("Er%d",err);
Wilmar87 0:dd68101b1b99 202 lcd.locate(8,0);
Wilmar87 0:dd68101b1b99 203 lcd.printf("Me%d",med);
Wilmar87 0:dd68101b1b99 204 lcd.locate(0,1);
Wilmar87 0:dd68101b1b99 205 lcd.printf("Sp%d",sp0);
Wilmar87 0:dd68101b1b99 206 lcd.locate(8,1);
Wilmar87 0:dd68101b1b99 207 lcd.printf("Pid%d",pid);
Wilmar87 0:dd68101b1b99 208
Wilmar87 0:dd68101b1b99 209
Wilmar87 0:dd68101b1b99 210 while(1) {
Wilmar87 0:dd68101b1b99 211
Wilmar87 0:dd68101b1b99 212 wait(0.001);
Wilmar87 0:dd68101b1b99 213 //leer puerto analogo y asignar a med
Wilmar87 0:dd68101b1b99 214
Wilmar87 0:dd68101b1b99 215
Wilmar87 0:dd68101b1b99 216 med = (Vin*3.27);
Wilmar87 0:dd68101b1b99 217 sp0 = sp*0.0327;
Wilmar87 0:dd68101b1b99 218 err = (sp0-med);
Wilmar87 0:dd68101b1b99 219 float kp0;
Wilmar87 0:dd68101b1b99 220 kp0 = kp*0.001;
Wilmar87 0:dd68101b1b99 221 ap = kp0*err;
Wilmar87 0:dd68101b1b99 222 float ki0;
Wilmar87 0:dd68101b1b99 223 ki0 = ki*0.001;
Wilmar87 0:dd68101b1b99 224 ai = (ki0*err)+ai; //calculo de la integral del error
Wilmar87 0:dd68101b1b99 225 float kd0;
Wilmar87 0:dd68101b1b99 226 kd0 = kd*0.0001;
Wilmar87 0:dd68101b1b99 227 ad = kd0*(err-err_v); //calculo de la accion derivativa
Wilmar87 0:dd68101b1b99 228 pid = (ap+ai+ad);
Wilmar87 0:dd68101b1b99 229
Wilmar87 0:dd68101b1b99 230
Wilmar87 0:dd68101b1b99 231 // se verifica que pid sea menor o igual la valor maximo *****************
Wilmar87 0:dd68101b1b99 232 if (pid > 0.999){
Wilmar87 0:dd68101b1b99 233 pid=1;
Wilmar87 0:dd68101b1b99 234 }
Wilmar87 0:dd68101b1b99 235
Wilmar87 0:dd68101b1b99 236 // se verifica que pid sea positivo **************************************
Wilmar87 0:dd68101b1b99 237 if (pid < 0){
Wilmar87 0:dd68101b1b99 238 pid=0;
Wilmar87 0:dd68101b1b99 239 }
Wilmar87 0:dd68101b1b99 240
Wilmar87 0:dd68101b1b99 241 // se verifica que la accion integral no sea muy grande
Wilmar87 0:dd68101b1b99 242 if (ai > 999){
Wilmar87 0:dd68101b1b99 243 ai=1000;
Wilmar87 0:dd68101b1b99 244 }
Wilmar87 0:dd68101b1b99 245
Wilmar87 0:dd68101b1b99 246 Vout=pid;
Wilmar87 0:dd68101b1b99 247 //Mostrar resultados PID
Wilmar87 0:dd68101b1b99 248 if(ciclo>700) {
Wilmar87 0:dd68101b1b99 249 lcd.locate(2,0);
Wilmar87 0:dd68101b1b99 250 lcd.printf(" ");
Wilmar87 0:dd68101b1b99 251 lcd.locate(0,0);
Wilmar87 0:dd68101b1b99 252 lcd.printf("Er%2.1f",err);
Wilmar87 0:dd68101b1b99 253 lcd.locate(10,0);
Wilmar87 0:dd68101b1b99 254 lcd.printf(" ");
Wilmar87 0:dd68101b1b99 255 lcd.locate(8,0);
Wilmar87 0:dd68101b1b99 256 lcd.printf("Me%4.2f",med);
Wilmar87 0:dd68101b1b99 257 lcd.locate(2,1);
Wilmar87 0:dd68101b1b99 258 lcd.printf(" ");
Wilmar87 0:dd68101b1b99 259 lcd.locate(0,1);
Wilmar87 0:dd68101b1b99 260 lcd.printf("Sp%4.2f",sp0);
Wilmar87 0:dd68101b1b99 261 lcd.locate(10,1);
Wilmar87 0:dd68101b1b99 262 lcd.printf(" ");
Wilmar87 0:dd68101b1b99 263 lcd.locate(8,1);
Wilmar87 0:dd68101b1b99 264 lcd.printf("Pid%4.3f",pid);
Wilmar87 0:dd68101b1b99 265 ciclo=0;
Wilmar87 0:dd68101b1b99 266 }
Wilmar87 0:dd68101b1b99 267 else
Wilmar87 0:dd68101b1b99 268 ciclo++;
Wilmar87 0:dd68101b1b99 269 err_v = err; //guarda el error
Wilmar87 0:dd68101b1b99 270 } // Envía parámetro pid al puerto analogico de salida (D/A) y se repite el ciclo
Wilmar87 0:dd68101b1b99 271 }
Wilmar87 0:dd68101b1b99 272