Daniel Múnera / Mbed 2 deprecated A_TUNE

CONTROLADOR PID

Dependencies:   DebounceIn DebouncedIn QEI TextLCD mbed

main.cpp@0:efe7bab86201, 2017-11-30 (annotated)

Committer:
dsmunerat
Date:
Thu Nov 30 01:04:42 2017 +0000
Revision:
0:efe7bab86201
CONTROLADOR PID CON AUTO_TUNNING

Who changed what in which revision?

UserRevisionLine numberNew contents of line
dsmunerat 0:efe7bab86201 1 #include "mbed.h"
dsmunerat 0:efe7bab86201 2 #include "TextLCD.h"
dsmunerat 0:efe7bab86201 3 #include "DebouncedIn.h"
dsmunerat 0:efe7bab86201 4 #include "QEI.h"
dsmunerat 0:efe7bab86201 5
dsmunerat 0:efe7bab86201 6 //Declaracion de entradas y salidas
dsmunerat 0:efe7bab86201 7 DebouncedIn Boton1(PTE20); //Boton para confirmar
dsmunerat 0:efe7bab86201 8 DebouncedIn Boton2(PTE21); //Boton para confirmar
dsmunerat 0:efe7bab86201 9 AnalogOut accion_u(PTE30);
dsmunerat 0:efe7bab86201 10 AnalogIn vcond(PTB0);
dsmunerat 0:efe7bab86201 11
dsmunerat 0:efe7bab86201 12 //Configuracion encoder
dsmunerat 0:efe7bab86201 13 QEI wheel (PTD5, PTD0, NC, 100);
dsmunerat 0:efe7bab86201 14
dsmunerat 0:efe7bab86201 15 //Configuracion pantalla LCD
dsmunerat 0:efe7bab86201 16 TextLCD lcd(PTB8,PTB9,PTB10,PTB11,PTE2,PTE3); // rs, e, d4-d7
dsmunerat 0:efe7bab86201 17
dsmunerat 0:efe7bab86201 18 int Kp=0,Ki=0,Kd=0,read=0,var,Kc=0,Tc,readc=0,cont_mayor=0;
dsmunerat 0:efe7bab86201 19 float Sp=0;
dsmunerat 0:efe7bab86201 20 float K1,K2,K3,ek=0,ekm1=0,ekm2=0,Ts=0.01; //Ts Tiempo Muestreo
dsmunerat 0:efe7bab86201 21 float yT,uk=0,ukm1=0,uk_inicial,uk_mayor,delta_uk,delta_uk_mayor,uc_mayor;
dsmunerat 0:efe7bab86201 22 Timer timer;
dsmunerat 0:efe7bab86201 23 Timer tc;
dsmunerat 0:efe7bab86201 24
dsmunerat 0:efe7bab86201 25
dsmunerat 0:efe7bab86201 26 int main()
dsmunerat 0:efe7bab86201 27 {
dsmunerat 0:efe7bab86201 28
dsmunerat 0:efe7bab86201 29
dsmunerat 0:efe7bab86201 30 lcd.cls();
dsmunerat 0:efe7bab86201 31 lcd.locate(1,0);
dsmunerat 0:efe7bab86201 32 lcd.printf("Kp=%d",Kp);
dsmunerat 0:efe7bab86201 33 lcd.locate(9,0);
dsmunerat 0:efe7bab86201 34 lcd.printf("Ki%d=",Ki);
dsmunerat 0:efe7bab86201 35 lcd.locate(1,1);
dsmunerat 0:efe7bab86201 36 lcd.printf("Kd=%d",Kd);
dsmunerat 0:efe7bab86201 37 lcd.locate(9,1);
dsmunerat 0:efe7bab86201 38 lcd.printf("Sp=%.1f",Sp);
dsmunerat 0:efe7bab86201 39
dsmunerat 0:efe7bab86201 40
dsmunerat 0:efe7bab86201 41 set_Kp:
dsmunerat 0:efe7bab86201 42 lcd.cls();
dsmunerat 0:efe7bab86201 43 lcd.locate(0,0);
dsmunerat 0:efe7bab86201 44 lcd.printf(">Kp= ");
dsmunerat 0:efe7bab86201 45
dsmunerat 0:efe7bab86201 46 lcd.locate(9,0);
dsmunerat 0:efe7bab86201 47 lcd.printf("Ki=%d",Ki);
dsmunerat 0:efe7bab86201 48 lcd.locate(1,1);
dsmunerat 0:efe7bab86201 49 lcd.printf("Kd=%d",Kd);
dsmunerat 0:efe7bab86201 50 lcd.locate(9,1);
dsmunerat 0:efe7bab86201 51 lcd.printf("Sp=%.1f",Sp);
dsmunerat 0:efe7bab86201 52
dsmunerat 0:efe7bab86201 53
dsmunerat 0:efe7bab86201 54 while(1) {
dsmunerat 0:efe7bab86201 55
dsmunerat 0:efe7bab86201 56 Kp=Kp+wheel.getPulses();
dsmunerat 0:efe7bab86201 57 wheel.reset();
dsmunerat 0:efe7bab86201 58
dsmunerat 0:efe7bab86201 59 if(Kp>=999) {
dsmunerat 0:efe7bab86201 60 Kp=999;
dsmunerat 0:efe7bab86201 61 } else if (Kp<=0) {
dsmunerat 0:efe7bab86201 62 Kp=0;
dsmunerat 0:efe7bab86201 63 }
dsmunerat 0:efe7bab86201 64
dsmunerat 0:efe7bab86201 65 lcd.locate(0,0);
dsmunerat 0:efe7bab86201 66 lcd.printf(">Kp=%d ",Kp);
dsmunerat 0:efe7bab86201 67
dsmunerat 0:efe7bab86201 68 if(Boton2.falling()) {
dsmunerat 0:efe7bab86201 69 goto set_Ki;
dsmunerat 0:efe7bab86201 70 }
dsmunerat 0:efe7bab86201 71
dsmunerat 0:efe7bab86201 72
dsmunerat 0:efe7bab86201 73 if(Boton1.falling()) {
dsmunerat 0:efe7bab86201 74 goto PID;
dsmunerat 0:efe7bab86201 75 }
dsmunerat 0:efe7bab86201 76
dsmunerat 0:efe7bab86201 77 }
dsmunerat 0:efe7bab86201 78
dsmunerat 0:efe7bab86201 79 set_Ki:
dsmunerat 0:efe7bab86201 80
dsmunerat 0:efe7bab86201 81 lcd.cls();
dsmunerat 0:efe7bab86201 82 lcd.locate(8,0);
dsmunerat 0:efe7bab86201 83 lcd.printf(">Ki= ");
dsmunerat 0:efe7bab86201 84
dsmunerat 0:efe7bab86201 85 lcd.locate(1,0);
dsmunerat 0:efe7bab86201 86 lcd.printf("Kp=%d",Kp);
dsmunerat 0:efe7bab86201 87 lcd.locate(1,1);
dsmunerat 0:efe7bab86201 88 lcd.printf("Kd=%d",Kd);
dsmunerat 0:efe7bab86201 89 lcd.locate(9,1);
dsmunerat 0:efe7bab86201 90 lcd.printf("Sp=%.1f",Sp);
dsmunerat 0:efe7bab86201 91
dsmunerat 0:efe7bab86201 92 //Ki=0;
dsmunerat 0:efe7bab86201 93 while(1) {
dsmunerat 0:efe7bab86201 94
dsmunerat 0:efe7bab86201 95 Ki=Ki+wheel.getPulses();
dsmunerat 0:efe7bab86201 96 wheel.reset();
dsmunerat 0:efe7bab86201 97
dsmunerat 0:efe7bab86201 98 if(Ki>=999) {
dsmunerat 0:efe7bab86201 99 Ki=999;
dsmunerat 0:efe7bab86201 100 } else if (Ki<=0) {
dsmunerat 0:efe7bab86201 101 Ki=0;
dsmunerat 0:efe7bab86201 102 }
dsmunerat 0:efe7bab86201 103
dsmunerat 0:efe7bab86201 104 lcd.locate(8,0);
dsmunerat 0:efe7bab86201 105 lcd.printf(">Ki=%d ",Ki);
dsmunerat 0:efe7bab86201 106
dsmunerat 0:efe7bab86201 107 if(Boton2.falling()) {
dsmunerat 0:efe7bab86201 108 goto set_Kd;
dsmunerat 0:efe7bab86201 109 }
dsmunerat 0:efe7bab86201 110
dsmunerat 0:efe7bab86201 111 if(Boton1.falling()) {
dsmunerat 0:efe7bab86201 112 goto PID;
dsmunerat 0:efe7bab86201 113 }
dsmunerat 0:efe7bab86201 114
dsmunerat 0:efe7bab86201 115 }
dsmunerat 0:efe7bab86201 116
dsmunerat 0:efe7bab86201 117 set_Kd:
dsmunerat 0:efe7bab86201 118
dsmunerat 0:efe7bab86201 119 lcd.cls();
dsmunerat 0:efe7bab86201 120 lcd.locate(0,1);
dsmunerat 0:efe7bab86201 121 lcd.printf(">Kd= ");
dsmunerat 0:efe7bab86201 122
dsmunerat 0:efe7bab86201 123 lcd.locate(1,0);
dsmunerat 0:efe7bab86201 124 lcd.printf("Kp=%d",Kp);
dsmunerat 0:efe7bab86201 125 lcd.locate(9,0);
dsmunerat 0:efe7bab86201 126 lcd.printf("Ki=%d",Ki);
dsmunerat 0:efe7bab86201 127 lcd.locate(9,1);
dsmunerat 0:efe7bab86201 128 lcd.printf("Sp=%.1f",Sp);
dsmunerat 0:efe7bab86201 129
dsmunerat 0:efe7bab86201 130
dsmunerat 0:efe7bab86201 131
dsmunerat 0:efe7bab86201 132 while(1) {
dsmunerat 0:efe7bab86201 133
dsmunerat 0:efe7bab86201 134 Kd=Kd+wheel.getPulses();
dsmunerat 0:efe7bab86201 135 wheel.reset();
dsmunerat 0:efe7bab86201 136
dsmunerat 0:efe7bab86201 137 if(Kd>=999) {
dsmunerat 0:efe7bab86201 138 Kd=999;
dsmunerat 0:efe7bab86201 139 } else if (Kd<=0) {
dsmunerat 0:efe7bab86201 140 Kd=0;
dsmunerat 0:efe7bab86201 141 }
dsmunerat 0:efe7bab86201 142
dsmunerat 0:efe7bab86201 143 lcd.locate(0,1);
dsmunerat 0:efe7bab86201 144 lcd.printf(">Kd=%d ",Kd);
dsmunerat 0:efe7bab86201 145
dsmunerat 0:efe7bab86201 146 if(Boton2.falling()) {
dsmunerat 0:efe7bab86201 147 goto set_Sp;
dsmunerat 0:efe7bab86201 148 }
dsmunerat 0:efe7bab86201 149
dsmunerat 0:efe7bab86201 150 if(Boton1.falling()) {
dsmunerat 0:efe7bab86201 151 goto PID;
dsmunerat 0:efe7bab86201 152 }
dsmunerat 0:efe7bab86201 153
dsmunerat 0:efe7bab86201 154 }
dsmunerat 0:efe7bab86201 155
dsmunerat 0:efe7bab86201 156 set_Sp:
dsmunerat 0:efe7bab86201 157
dsmunerat 0:efe7bab86201 158 lcd.cls();
dsmunerat 0:efe7bab86201 159 lcd.locate(8,1);
dsmunerat 0:efe7bab86201 160 lcd.printf(">Sp= ");
dsmunerat 0:efe7bab86201 161
dsmunerat 0:efe7bab86201 162 lcd.locate(1,0);
dsmunerat 0:efe7bab86201 163 lcd.printf("Kp=%d",Kp);
dsmunerat 0:efe7bab86201 164 lcd.locate(9,0);
dsmunerat 0:efe7bab86201 165 lcd.printf("Ki=%d",Ki);
dsmunerat 0:efe7bab86201 166 lcd.locate(1,1);
dsmunerat 0:efe7bab86201 167 lcd.printf("Kd=%d",Kd);
dsmunerat 0:efe7bab86201 168
dsmunerat 0:efe7bab86201 169 //Sp=0;
dsmunerat 0:efe7bab86201 170 while(1) {
dsmunerat 0:efe7bab86201 171
dsmunerat 0:efe7bab86201 172 Sp=Sp+0.1*wheel.getPulses();
dsmunerat 0:efe7bab86201 173 wheel.reset();
dsmunerat 0:efe7bab86201 174
dsmunerat 0:efe7bab86201 175 if(Sp>=3.3) {
dsmunerat 0:efe7bab86201 176 Sp=3.3;
dsmunerat 0:efe7bab86201 177 } else if (Sp<=0) {
dsmunerat 0:efe7bab86201 178 Sp=0;
dsmunerat 0:efe7bab86201 179 }
dsmunerat 0:efe7bab86201 180
dsmunerat 0:efe7bab86201 181 lcd.locate(8,1);
dsmunerat 0:efe7bab86201 182 lcd.printf(">Sp=%.1f ",Sp);
dsmunerat 0:efe7bab86201 183
dsmunerat 0:efe7bab86201 184 if(Boton2.falling()) {
dsmunerat 0:efe7bab86201 185 goto PID2;
dsmunerat 0:efe7bab86201 186 }
dsmunerat 0:efe7bab86201 187
dsmunerat 0:efe7bab86201 188 if(Boton1.falling()) {
dsmunerat 0:efe7bab86201 189 goto PID;
dsmunerat 0:efe7bab86201 190 }
dsmunerat 0:efe7bab86201 191 }
dsmunerat 0:efe7bab86201 192
dsmunerat 0:efe7bab86201 193
dsmunerat 0:efe7bab86201 194
dsmunerat 0:efe7bab86201 195 PID2:
dsmunerat 0:efe7bab86201 196 lcd.cls();
dsmunerat 0:efe7bab86201 197 Kp = 1;
dsmunerat 0:efe7bab86201 198
dsmunerat 0:efe7bab86201 199 while (Kp<100) {
dsmunerat 0:efe7bab86201 200
dsmunerat 0:efe7bab86201 201 read = timer.read();
dsmunerat 0:efe7bab86201 202
dsmunerat 0:efe7bab86201 203 if (read==0) {
dsmunerat 0:efe7bab86201 204 timer.start();
dsmunerat 0:efe7bab86201 205 uk_mayor=0;
dsmunerat 0:efe7bab86201 206 }
dsmunerat 0:efe7bab86201 207
dsmunerat 0:efe7bab86201 208 if (read==1) {
dsmunerat 0:efe7bab86201 209 uk_inicial=uk;
dsmunerat 0:efe7bab86201 210 }
dsmunerat 0:efe7bab86201 211
dsmunerat 0:efe7bab86201 212 if (read==5) {
dsmunerat 0:efe7bab86201 213 if (Kp<100) {
dsmunerat 0:efe7bab86201 214 Kp = Kp+10;
dsmunerat 0:efe7bab86201 215 }
dsmunerat 0:efe7bab86201 216 timer.reset();
dsmunerat 0:efe7bab86201 217 }
dsmunerat 0:efe7bab86201 218
dsmunerat 0:efe7bab86201 219 if (uk>uk_mayor) {
dsmunerat 0:efe7bab86201 220 uk_mayor=uk;
dsmunerat 0:efe7bab86201 221 }
dsmunerat 0:efe7bab86201 222
dsmunerat 0:efe7bab86201 223 if (read==4) {
dsmunerat 0:efe7bab86201 224 delta_uk=uk_mayor-uk_inicial;
dsmunerat 0:efe7bab86201 225 if (delta_uk>delta_uk_mayor) {
dsmunerat 0:efe7bab86201 226 delta_uk_mayor=delta_uk;
dsmunerat 0:efe7bab86201 227 Kc=Kp;
dsmunerat 0:efe7bab86201 228 //Tc=2;
dsmunerat 0:efe7bab86201 229 uc_mayor=uk_mayor;
dsmunerat 0:efe7bab86201 230
dsmunerat 0:efe7bab86201 231 }
dsmunerat 0:efe7bab86201 232 }
dsmunerat 0:efe7bab86201 233
dsmunerat 0:efe7bab86201 234 if (Kp>100) {
dsmunerat 0:efe7bab86201 235 goto CALCULO_TC;
dsmunerat 0:efe7bab86201 236 }
dsmunerat 0:efe7bab86201 237
dsmunerat 0:efe7bab86201 238
dsmunerat 0:efe7bab86201 239
dsmunerat 0:efe7bab86201 240 yT=vcond.read()*3.3;
dsmunerat 0:efe7bab86201 241 ek=Sp-yT;
dsmunerat 0:efe7bab86201 242
dsmunerat 0:efe7bab86201 243
dsmunerat 0:efe7bab86201 244 K1=Kp+(Ki/2)*Ts+(Kd/Ts);
dsmunerat 0:efe7bab86201 245 K2=-Kp+(Ki/2)*Ts-(2*Kd/Ts);
dsmunerat 0:efe7bab86201 246 K3=Kd/Ts;
dsmunerat 0:efe7bab86201 247
dsmunerat 0:efe7bab86201 248 uk=ukm1+K1*ek+K2*ekm1+K3*ekm2; //Accion de control
dsmunerat 0:efe7bab86201 249
dsmunerat 0:efe7bab86201 250 if (uk>3.3) { //Salida PID si es mayor que el MAX
dsmunerat 0:efe7bab86201 251 uk=3.3;
dsmunerat 0:efe7bab86201 252 } else if (uk<0) { //Salida PID si es menor que el MIN
dsmunerat 0:efe7bab86201 253 uk=0;
dsmunerat 0:efe7bab86201 254 }
dsmunerat 0:efe7bab86201 255
dsmunerat 0:efe7bab86201 256 //control=(float)uk/3.3; //Accion de control mapeada a PWM; salida de la tarjeta.
dsmunerat 0:efe7bab86201 257 accion_u = uk;
dsmunerat 0:efe7bab86201 258
dsmunerat 0:efe7bab86201 259 if (uk>uk_mayor) {
dsmunerat 0:efe7bab86201 260 uk_mayor=uk;
dsmunerat 0:efe7bab86201 261 }
dsmunerat 0:efe7bab86201 262
dsmunerat 0:efe7bab86201 263 ekm2=ekm1;
dsmunerat 0:efe7bab86201 264 ekm1=ek;
dsmunerat 0:efe7bab86201 265 ukm1=uk;
dsmunerat 0:efe7bab86201 266 lcd.locate(0,0);
dsmunerat 0:efe7bab86201 267 //lcd.printf("Error=%.2f ",ek);
dsmunerat 0:efe7bab86201 268 lcd.printf("a_tunning");
dsmunerat 0:efe7bab86201 269
dsmunerat 0:efe7bab86201 270 }
dsmunerat 0:efe7bab86201 271
dsmunerat 0:efe7bab86201 272
dsmunerat 0:efe7bab86201 273
dsmunerat 0:efe7bab86201 274 PID:
dsmunerat 0:efe7bab86201 275 lcd.cls();
dsmunerat 0:efe7bab86201 276 while(1){
dsmunerat 0:efe7bab86201 277
dsmunerat 0:efe7bab86201 278 yT=vcond.read()*3.3;
dsmunerat 0:efe7bab86201 279 ek=Sp-yT;
dsmunerat 0:efe7bab86201 280
dsmunerat 0:efe7bab86201 281 //Conversion de parametros PID Continuo a parametros de PID Digital
dsmunerat 0:efe7bab86201 282 //ESTO ES UN PID DIGITAL Y LOS PARAMETROS AGREGADOS SON PARA UN PID CONTINUO!!
dsmunerat 0:efe7bab86201 283 K1=Kp+(Ki/2)*Ts+(Kd/Ts);
dsmunerat 0:efe7bab86201 284 K2=-Kp+(Ki/2)*Ts-(2*Kd/Ts);
dsmunerat 0:efe7bab86201 285 K3=Kd/Ts;
dsmunerat 0:efe7bab86201 286
dsmunerat 0:efe7bab86201 287 uk=ukm1+K1*ek+K2*ekm1+K3*ekm2; //Accion de control
dsmunerat 0:efe7bab86201 288
dsmunerat 0:efe7bab86201 289 if (uk>3.3) { //Salida PID si es mayor que el MAX
dsmunerat 0:efe7bab86201 290 uk=3.3;}
dsmunerat 0:efe7bab86201 291 else if (uk<0){ //Salida PID si es menor que el MIN
dsmunerat 0:efe7bab86201 292 uk=0;
dsmunerat 0:efe7bab86201 293 }
dsmunerat 0:efe7bab86201 294
dsmunerat 0:efe7bab86201 295 //control=(float)uk/3.3; //Accion de control mapeada a PWM; salida de la tarjeta.
dsmunerat 0:efe7bab86201 296 accion_u = uk;
dsmunerat 0:efe7bab86201 297
dsmunerat 0:efe7bab86201 298 ekm2=ekm1;
dsmunerat 0:efe7bab86201 299 ekm1=ek;
dsmunerat 0:efe7bab86201 300 ukm1=uk;
dsmunerat 0:efe7bab86201 301 lcd.locate(0,0);
dsmunerat 0:efe7bab86201 302 lcd.printf("Error=%.2f ",ek);
dsmunerat 0:efe7bab86201 303 lcd.locate(0,1);
dsmunerat 0:efe7bab86201 304 lcd.printf("Y_act=%.2f",yT);
dsmunerat 0:efe7bab86201 305 wait(Ts); // Muestreos
dsmunerat 0:efe7bab86201 306 }
dsmunerat 0:efe7bab86201 307
dsmunerat 0:efe7bab86201 308
dsmunerat 0:efe7bab86201 309
dsmunerat 0:efe7bab86201 310 CALCULO_TC:
dsmunerat 0:efe7bab86201 311 while(readc<21) {
dsmunerat 0:efe7bab86201 312
dsmunerat 0:efe7bab86201 313 Kp=Kc;
dsmunerat 0:efe7bab86201 314 tc.start();
dsmunerat 0:efe7bab86201 315 readc=tc.read();
dsmunerat 0:efe7bab86201 316
dsmunerat 0:efe7bab86201 317 if (uk==uc_mayor) {
dsmunerat 0:efe7bab86201 318 cont_mayor= cont_mayor+1;
dsmunerat 0:efe7bab86201 319 }
dsmunerat 0:efe7bab86201 320
dsmunerat 0:efe7bab86201 321 if (readc==20) {
dsmunerat 0:efe7bab86201 322 Tc=20/cont_mayor;
dsmunerat 0:efe7bab86201 323 goto CONTROL_AT;
dsmunerat 0:efe7bab86201 324 }
dsmunerat 0:efe7bab86201 325
dsmunerat 0:efe7bab86201 326 yT=vcond.read()*3.3;
dsmunerat 0:efe7bab86201 327 ek=Sp-yT;
dsmunerat 0:efe7bab86201 328
dsmunerat 0:efe7bab86201 329
dsmunerat 0:efe7bab86201 330 K1=Kp+(Ki/2)*Ts+(Kd/Ts);
dsmunerat 0:efe7bab86201 331 K2=-Kp+(Ki/2)*Ts-(2*Kd/Ts);
dsmunerat 0:efe7bab86201 332 K3=Kd/Ts;
dsmunerat 0:efe7bab86201 333
dsmunerat 0:efe7bab86201 334 uk=ukm1+K1*ek+K2*ekm1+K3*ekm2; //Accion de control
dsmunerat 0:efe7bab86201 335
dsmunerat 0:efe7bab86201 336 if (uk>3.3) { //Salida PID si es mayor que el MAX
dsmunerat 0:efe7bab86201 337 uk=3.3;
dsmunerat 0:efe7bab86201 338 } else if (uk<0) { //Salida PID si es menor que el MIN
dsmunerat 0:efe7bab86201 339 uk=0;
dsmunerat 0:efe7bab86201 340 }
dsmunerat 0:efe7bab86201 341
dsmunerat 0:efe7bab86201 342 //control=(float)uk/3.3; //Accion de control mapeada a PWM; salida de la tarjeta.
dsmunerat 0:efe7bab86201 343 accion_u = uk;
dsmunerat 0:efe7bab86201 344
dsmunerat 0:efe7bab86201 345 if (uk>uk_mayor) {
dsmunerat 0:efe7bab86201 346 uk_mayor=uk;
dsmunerat 0:efe7bab86201 347 }
dsmunerat 0:efe7bab86201 348
dsmunerat 0:efe7bab86201 349 ekm2=ekm1;
dsmunerat 0:efe7bab86201 350 ekm1=ek;
dsmunerat 0:efe7bab86201 351 ukm1=uk;
dsmunerat 0:efe7bab86201 352 lcd.locate(0,0);
dsmunerat 0:efe7bab86201 353
dsmunerat 0:efe7bab86201 354 lcd.printf("a_tunnig");
dsmunerat 0:efe7bab86201 355
dsmunerat 0:efe7bab86201 356
dsmunerat 0:efe7bab86201 357 }
dsmunerat 0:efe7bab86201 358
dsmunerat 0:efe7bab86201 359 CONTROL_AT:
dsmunerat 0:efe7bab86201 360
dsmunerat 0:efe7bab86201 361 while(1) {
dsmunerat 0:efe7bab86201 362 Kp=0.45*Kc;
dsmunerat 0:efe7bab86201 363 Ki=0.54*Kc/Tc;
dsmunerat 0:efe7bab86201 364 Kd=0;
dsmunerat 0:efe7bab86201 365
dsmunerat 0:efe7bab86201 366 yT=vcond.read()*3.3;
dsmunerat 0:efe7bab86201 367 ek=Sp-yT;
dsmunerat 0:efe7bab86201 368
dsmunerat 0:efe7bab86201 369
dsmunerat 0:efe7bab86201 370 K1=Kp+(Ki/2)*Ts+(Kd/Ts);
dsmunerat 0:efe7bab86201 371 K2=-Kp+(Ki/2)*Ts-(2*Kd/Ts);
dsmunerat 0:efe7bab86201 372 K3=Kd/Ts;
dsmunerat 0:efe7bab86201 373
dsmunerat 0:efe7bab86201 374 uk=ukm1+K1*ek+K2*ekm1+K3*ekm2; //Accion de control
dsmunerat 0:efe7bab86201 375
dsmunerat 0:efe7bab86201 376 if (uk>3.3) { //Salida PID si es mayor que el MAX
dsmunerat 0:efe7bab86201 377 uk=3.3;
dsmunerat 0:efe7bab86201 378 } else if (uk<0) { //Salida PID si es menor que el MIN
dsmunerat 0:efe7bab86201 379 uk=0;
dsmunerat 0:efe7bab86201 380 }
dsmunerat 0:efe7bab86201 381
dsmunerat 0:efe7bab86201 382 //control=(float)uk/3.3; //Accion de control mapeada a PWM; salida de la tarjeta.
dsmunerat 0:efe7bab86201 383 accion_u = uk;
dsmunerat 0:efe7bab86201 384
dsmunerat 0:efe7bab86201 385 if (uk>uk_mayor) {
dsmunerat 0:efe7bab86201 386 uk_mayor=uk;
dsmunerat 0:efe7bab86201 387 }
dsmunerat 0:efe7bab86201 388
dsmunerat 0:efe7bab86201 389 ekm2=ekm1;
dsmunerat 0:efe7bab86201 390 ekm1=ek;
dsmunerat 0:efe7bab86201 391 ukm1=uk;
dsmunerat 0:efe7bab86201 392 //lcd.locate(0,0);
dsmunerat 0:efe7bab86201 393 //lcd.printf("Error=%.2f ",ek);
dsmunerat 0:efe7bab86201 394 //lcd.printf("delta_mayor=%.2f",delta_uk_mayor);
dsmunerat 0:efe7bab86201 395 //lcd.locate(0,1);
dsmunerat 0:efe7bab86201 396 //lcd.printf("Kc=%I",Kc);
dsmunerat 0:efe7bab86201 397 lcd.locate(0,0);
dsmunerat 0:efe7bab86201 398 lcd.printf("Error=%.2f ",ek);
dsmunerat 0:efe7bab86201 399 lcd.locate(0,1);
dsmunerat 0:efe7bab86201 400 lcd.printf("Y_act=%.2f",yT);
dsmunerat 0:efe7bab86201 401 wait(Ts); // Muestreos
dsmunerat 0:efe7bab86201 402 lcd.locate(13,1);
dsmunerat 0:efe7bab86201 403 lcd.printf("%.I",Kp);
dsmunerat 0:efe7bab86201 404 wait(Ts); // Muestreos
dsmunerat 0:efe7bab86201 405 }
dsmunerat 0:efe7bab86201 406
dsmunerat 0:efe7bab86201 407 }
dsmunerat 0:efe7bab86201 408
dsmunerat 0:efe7bab86201 409
dsmunerat 0:efe7bab86201 410