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@0:dd68101b1b99, 2013-12-13 (annotated)

Committer:
Wilmar87
Date:
Fri Dec 13 04:14:14 2013 +0000
Revision:
0:dd68101b1b99
Child:
1:9ca362d07dd0
yes;

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