Control PID de un circuito RC con parametros agregados mediante pulsadores y visualizado en LCD 16x2
Dependencies: DebouncedIn TextLCD mbed
Fork of TAREA_3_PID by
main.cpp@1:93e71ac5cf91, 2017-11-08 (annotated)
- Committer:
- demo71
- Date:
- Wed Nov 08 21:04:13 2017 +0000
- Revision:
- 1:93e71ac5cf91
- Parent:
- 0:694446246c35
PID CONTINUO
Who changed what in which revision?
User | Revision | Line number | New contents of line |
---|---|---|---|
demo71 | 0:694446246c35 | 1 | // Hello World! for the TextLCD |
demo71 | 0:694446246c35 | 2 | |
demo71 | 0:694446246c35 | 3 | #include "mbed.h" |
demo71 | 0:694446246c35 | 4 | #include "stdio.h" |
demo71 | 0:694446246c35 | 5 | #include "TextLCD.h" |
demo71 | 0:694446246c35 | 6 | #include "DebouncedIn.h" |
demo71 | 0:694446246c35 | 7 | |
demo71 | 0:694446246c35 | 8 | //Declaracion de entradas y salidas |
demo71 | 0:694446246c35 | 9 | DebouncedIn Boton1(PTE20); //Selector |
demo71 | 0:694446246c35 | 10 | DebouncedIn Boton2(PTE21); //Aumento |
demo71 | 0:694446246c35 | 11 | DebouncedIn Boton3(PTE22); //Disminuye |
demo71 | 0:694446246c35 | 12 | DebouncedIn Boton4(PTE23); //Confirma |
demo71 | 0:694446246c35 | 13 | |
demo71 | 0:694446246c35 | 14 | PwmOut control(PTE29); |
demo71 | 0:694446246c35 | 15 | AnalogIn vcond(PTB0); |
demo71 | 0:694446246c35 | 16 | |
demo71 | 0:694446246c35 | 17 | TextLCD lcd(PTB8,PTB9,PTB10,PTB11,PTE2,PTE3); // rs, e, d4-d7 |
demo71 | 0:694446246c35 | 18 | |
demo71 | 0:694446246c35 | 19 | //codigos movimiento del cursor LCD |
demo71 | 0:694446246c35 | 20 | int C2=0x18; // desplaza izquierda |
demo71 | 0:694446246c35 | 21 | int C3=0x1A; // desplaza derecha |
demo71 | 0:694446246c35 | 22 | int C4=0x0C; // quito cursor bajo |
demo71 | 0:694446246c35 | 23 | int C1=0x0F; // solo muestra el cursor |
demo71 | 0:694446246c35 | 24 | |
demo71 | 1:93e71ac5cf91 | 25 | //Variables y parametros del Controlador PID |
demo71 | 0:694446246c35 | 26 | int Kp=0,Ki=0,Kd=0,Sp=0,p,i; |
demo71 | 0:694446246c35 | 27 | float rT,eT,iT,dT,yT,uT,iT0=0,eT0=0,iT_1=0,eT_1=0; |
demo71 | 0:694446246c35 | 28 | Timer tu; |
demo71 | 0:694446246c35 | 29 | Timer td; |
demo71 | 0:694446246c35 | 30 | |
demo71 | 0:694446246c35 | 31 | int main() { |
demo71 | 0:694446246c35 | 32 | |
demo71 | 0:694446246c35 | 33 | lcd.cls(); |
demo71 | 0:694446246c35 | 34 | lcd.locate(1,0); |
demo71 | 0:694446246c35 | 35 | lcd.printf("Kp=%d",Kp); |
demo71 | 0:694446246c35 | 36 | lcd.locate(9,0); |
demo71 | 0:694446246c35 | 37 | lcd.printf("Ki%d=",Ki); |
demo71 | 0:694446246c35 | 38 | lcd.locate(1,1); |
demo71 | 0:694446246c35 | 39 | lcd.printf("Kd=%d",Kd); |
demo71 | 0:694446246c35 | 40 | lcd.locate(9,1); |
demo71 | 0:694446246c35 | 41 | lcd.printf("Sp=%d",Sp); |
demo71 | 0:694446246c35 | 42 | |
demo71 | 1:93e71ac5cf91 | 43 | //Configuracion del parametro Kp |
demo71 | 0:694446246c35 | 44 | set_Kp: |
demo71 | 0:694446246c35 | 45 | lcd.cls(); |
demo71 | 0:694446246c35 | 46 | lcd.locate(0,0); |
demo71 | 0:694446246c35 | 47 | lcd.printf(">Kp= "); |
demo71 | 0:694446246c35 | 48 | |
demo71 | 0:694446246c35 | 49 | lcd.locate(9,0); |
demo71 | 0:694446246c35 | 50 | lcd.printf("Ki=%d",Ki); |
demo71 | 0:694446246c35 | 51 | lcd.locate(1,1); |
demo71 | 0:694446246c35 | 52 | lcd.printf("Kd=%d",Kd); |
demo71 | 0:694446246c35 | 53 | lcd.locate(9,1); |
demo71 | 0:694446246c35 | 54 | lcd.printf("Sp=%d",Sp); |
demo71 | 0:694446246c35 | 55 | |
demo71 | 0:694446246c35 | 56 | |
demo71 | 1:93e71ac5cf91 | 57 | |
demo71 | 0:694446246c35 | 58 | while(1){ |
demo71 | 0:694446246c35 | 59 | |
demo71 | 0:694446246c35 | 60 | if(Boton2.falling()){ |
demo71 | 0:694446246c35 | 61 | Kp++; |
demo71 | 0:694446246c35 | 62 | tu.start(); |
demo71 | 0:694446246c35 | 63 | } |
demo71 | 0:694446246c35 | 64 | //AUMENTO*10 ************** |
demo71 | 0:694446246c35 | 65 | if(Boton2.rising()){ |
demo71 | 0:694446246c35 | 66 | tu.stop(); |
demo71 | 0:694446246c35 | 67 | tu.reset(); |
demo71 | 0:694446246c35 | 68 | } |
demo71 | 0:694446246c35 | 69 | if(tu.read()>=2.0 && tu.read()<4.0){ |
demo71 | 0:694446246c35 | 70 | Kp=Kp+10; |
demo71 | 0:694446246c35 | 71 | wait(0.3); |
demo71 | 0:694446246c35 | 72 | } |
demo71 | 0:694446246c35 | 73 | else if(tu.read()>=4.0){ |
demo71 | 0:694446246c35 | 74 | Kp=Kp+100; |
demo71 | 0:694446246c35 | 75 | wait(0.3); |
demo71 | 0:694446246c35 | 76 | } |
demo71 | 0:694446246c35 | 77 | //************************* |
demo71 | 0:694446246c35 | 78 | if(Boton3.falling()){ |
demo71 | 0:694446246c35 | 79 | Kp--; |
demo71 | 0:694446246c35 | 80 | td.start(); |
demo71 | 0:694446246c35 | 81 | } |
demo71 | 0:694446246c35 | 82 | //DISMINUYE*10 ************** |
demo71 | 0:694446246c35 | 83 | if(Boton3.rising()){ |
demo71 | 0:694446246c35 | 84 | td.stop(); |
demo71 | 0:694446246c35 | 85 | td.reset(); |
demo71 | 0:694446246c35 | 86 | } |
demo71 | 0:694446246c35 | 87 | if(td.read()>=2.0 && td.read()<4.0){ |
demo71 | 0:694446246c35 | 88 | Kp=Kp-10; |
demo71 | 0:694446246c35 | 89 | wait(0.3); |
demo71 | 0:694446246c35 | 90 | } |
demo71 | 0:694446246c35 | 91 | else if(td.read()>=4.0){ |
demo71 | 0:694446246c35 | 92 | Kp=Kp-100; |
demo71 | 0:694446246c35 | 93 | wait(0.3); |
demo71 | 0:694446246c35 | 94 | } |
demo71 | 0:694446246c35 | 95 | //************************* |
demo71 | 0:694446246c35 | 96 | |
demo71 | 0:694446246c35 | 97 | if(Kp>=999){ |
demo71 | 0:694446246c35 | 98 | Kp=999; |
demo71 | 0:694446246c35 | 99 | } |
demo71 | 0:694446246c35 | 100 | else if (Kp<=0){ |
demo71 | 0:694446246c35 | 101 | Kp=0; |
demo71 | 0:694446246c35 | 102 | } |
demo71 | 0:694446246c35 | 103 | |
demo71 | 0:694446246c35 | 104 | lcd.locate(0,0); |
demo71 | 0:694446246c35 | 105 | lcd.printf(">Kp=%d ",Kp); |
demo71 | 0:694446246c35 | 106 | |
demo71 | 0:694446246c35 | 107 | if(Boton1.falling()){ |
demo71 | 0:694446246c35 | 108 | goto set_Ki; |
demo71 | 0:694446246c35 | 109 | } |
demo71 | 0:694446246c35 | 110 | |
demo71 | 0:694446246c35 | 111 | if(Boton4.falling()){ |
demo71 | 0:694446246c35 | 112 | goto PID; |
demo71 | 0:694446246c35 | 113 | } |
demo71 | 0:694446246c35 | 114 | |
demo71 | 0:694446246c35 | 115 | } |
demo71 | 0:694446246c35 | 116 | |
demo71 | 1:93e71ac5cf91 | 117 | //Configuracion del parametro Ki |
demo71 | 0:694446246c35 | 118 | set_Ki: |
demo71 | 0:694446246c35 | 119 | |
demo71 | 0:694446246c35 | 120 | lcd.cls(); |
demo71 | 0:694446246c35 | 121 | lcd.locate(8,0); |
demo71 | 0:694446246c35 | 122 | lcd.printf(">Ki= "); |
demo71 | 0:694446246c35 | 123 | |
demo71 | 0:694446246c35 | 124 | lcd.locate(1,0); |
demo71 | 0:694446246c35 | 125 | lcd.printf("Kp=%d",Kp); |
demo71 | 0:694446246c35 | 126 | lcd.locate(1,1); |
demo71 | 0:694446246c35 | 127 | lcd.printf("Kd=%d",Kd); |
demo71 | 0:694446246c35 | 128 | lcd.locate(9,1); |
demo71 | 0:694446246c35 | 129 | lcd.printf("Sp=%d",Sp); |
demo71 | 0:694446246c35 | 130 | |
demo71 | 0:694446246c35 | 131 | //Ki=0; |
demo71 | 0:694446246c35 | 132 | while(1){ |
demo71 | 0:694446246c35 | 133 | |
demo71 | 0:694446246c35 | 134 | if(Boton2.falling()){ |
demo71 | 0:694446246c35 | 135 | Ki++; |
demo71 | 0:694446246c35 | 136 | tu.start(); |
demo71 | 0:694446246c35 | 137 | } |
demo71 | 0:694446246c35 | 138 | //AUMENTO*10 ************** |
demo71 | 0:694446246c35 | 139 | if(Boton2.rising()){ |
demo71 | 0:694446246c35 | 140 | tu.stop(); |
demo71 | 0:694446246c35 | 141 | tu.reset(); |
demo71 | 0:694446246c35 | 142 | } |
demo71 | 0:694446246c35 | 143 | if(tu.read()>=2.0 && tu.read()<4.0){ |
demo71 | 0:694446246c35 | 144 | Ki=Ki+10; |
demo71 | 0:694446246c35 | 145 | wait(0.3); |
demo71 | 0:694446246c35 | 146 | } |
demo71 | 0:694446246c35 | 147 | else if(tu.read()>=4.0){ |
demo71 | 0:694446246c35 | 148 | Ki=Ki+100; |
demo71 | 0:694446246c35 | 149 | wait(0.3); |
demo71 | 0:694446246c35 | 150 | } |
demo71 | 0:694446246c35 | 151 | //************************* |
demo71 | 0:694446246c35 | 152 | if(Boton3.falling()){ |
demo71 | 0:694446246c35 | 153 | Ki--; |
demo71 | 0:694446246c35 | 154 | td.start(); |
demo71 | 0:694446246c35 | 155 | } |
demo71 | 0:694446246c35 | 156 | //DISMINUYE*10 ************** |
demo71 | 0:694446246c35 | 157 | if(Boton3.rising()){ |
demo71 | 0:694446246c35 | 158 | td.stop(); |
demo71 | 0:694446246c35 | 159 | td.reset(); |
demo71 | 0:694446246c35 | 160 | } |
demo71 | 0:694446246c35 | 161 | if(td.read()>=2.0 && td.read()<4.0){ |
demo71 | 0:694446246c35 | 162 | Ki=Ki-10; |
demo71 | 0:694446246c35 | 163 | wait(0.3); |
demo71 | 0:694446246c35 | 164 | } |
demo71 | 0:694446246c35 | 165 | else if(td.read()>=4.0){ |
demo71 | 0:694446246c35 | 166 | Ki=Ki-100; |
demo71 | 0:694446246c35 | 167 | wait(0.3); |
demo71 | 0:694446246c35 | 168 | } |
demo71 | 0:694446246c35 | 169 | //************************* |
demo71 | 0:694446246c35 | 170 | |
demo71 | 0:694446246c35 | 171 | if(Ki>=999){ |
demo71 | 0:694446246c35 | 172 | Ki=999; |
demo71 | 0:694446246c35 | 173 | } |
demo71 | 0:694446246c35 | 174 | else if (Ki<=0){ |
demo71 | 0:694446246c35 | 175 | Ki=0; |
demo71 | 0:694446246c35 | 176 | } |
demo71 | 0:694446246c35 | 177 | |
demo71 | 0:694446246c35 | 178 | lcd.locate(8,0); |
demo71 | 0:694446246c35 | 179 | lcd.printf(">Ki=%d ",Ki); |
demo71 | 0:694446246c35 | 180 | |
demo71 | 0:694446246c35 | 181 | if(Boton1.falling()){ |
demo71 | 0:694446246c35 | 182 | goto set_Kd; |
demo71 | 0:694446246c35 | 183 | } |
demo71 | 0:694446246c35 | 184 | |
demo71 | 0:694446246c35 | 185 | if(Boton4.falling()){ |
demo71 | 0:694446246c35 | 186 | goto PID; |
demo71 | 0:694446246c35 | 187 | } |
demo71 | 0:694446246c35 | 188 | |
demo71 | 0:694446246c35 | 189 | } |
demo71 | 0:694446246c35 | 190 | |
demo71 | 1:93e71ac5cf91 | 191 | //Configuracion del parametro Kd |
demo71 | 0:694446246c35 | 192 | set_Kd: |
demo71 | 0:694446246c35 | 193 | |
demo71 | 0:694446246c35 | 194 | lcd.cls(); |
demo71 | 0:694446246c35 | 195 | lcd.locate(0,1); |
demo71 | 0:694446246c35 | 196 | lcd.printf(">Kd= "); |
demo71 | 0:694446246c35 | 197 | |
demo71 | 0:694446246c35 | 198 | lcd.locate(1,0); |
demo71 | 0:694446246c35 | 199 | lcd.printf("Kp=%d",Kp); |
demo71 | 0:694446246c35 | 200 | lcd.locate(9,0); |
demo71 | 0:694446246c35 | 201 | lcd.printf("Ki=%d",Ki); |
demo71 | 0:694446246c35 | 202 | lcd.locate(9,1); |
demo71 | 0:694446246c35 | 203 | lcd.printf("Sp=%d",Sp); |
demo71 | 0:694446246c35 | 204 | |
demo71 | 0:694446246c35 | 205 | |
demo71 | 1:93e71ac5cf91 | 206 | |
demo71 | 0:694446246c35 | 207 | while(1){ |
demo71 | 0:694446246c35 | 208 | |
demo71 | 0:694446246c35 | 209 | if(Boton2.falling()){ |
demo71 | 0:694446246c35 | 210 | Kd++; |
demo71 | 0:694446246c35 | 211 | tu.start(); |
demo71 | 0:694446246c35 | 212 | } |
demo71 | 0:694446246c35 | 213 | //AUMENTO*10 ************** |
demo71 | 0:694446246c35 | 214 | if(Boton2.rising()){ |
demo71 | 0:694446246c35 | 215 | tu.stop(); |
demo71 | 0:694446246c35 | 216 | tu.reset(); |
demo71 | 0:694446246c35 | 217 | } |
demo71 | 0:694446246c35 | 218 | if(tu.read()>=2.0 && tu.read()<4.0){ |
demo71 | 0:694446246c35 | 219 | Kd=Kd+10; |
demo71 | 0:694446246c35 | 220 | wait(0.3); |
demo71 | 0:694446246c35 | 221 | } |
demo71 | 0:694446246c35 | 222 | else if(tu.read()>=4.0){ |
demo71 | 0:694446246c35 | 223 | Kd=Kd+100; |
demo71 | 0:694446246c35 | 224 | wait(0.3); |
demo71 | 0:694446246c35 | 225 | } |
demo71 | 0:694446246c35 | 226 | //************************* |
demo71 | 0:694446246c35 | 227 | if(Boton3.falling()){ |
demo71 | 0:694446246c35 | 228 | Kd--; |
demo71 | 0:694446246c35 | 229 | td.start(); |
demo71 | 0:694446246c35 | 230 | } |
demo71 | 0:694446246c35 | 231 | //DISMINUYE*10 ************** |
demo71 | 0:694446246c35 | 232 | if(Boton3.rising()){ |
demo71 | 0:694446246c35 | 233 | td.stop(); |
demo71 | 0:694446246c35 | 234 | td.reset(); |
demo71 | 0:694446246c35 | 235 | } |
demo71 | 0:694446246c35 | 236 | if(td.read()>=2.0 && td.read()<4.0){ |
demo71 | 0:694446246c35 | 237 | Kd=Kd-10; |
demo71 | 0:694446246c35 | 238 | wait(0.3); |
demo71 | 0:694446246c35 | 239 | } |
demo71 | 0:694446246c35 | 240 | else if(td.read()>=4.0){ |
demo71 | 0:694446246c35 | 241 | Kd=Kd-100; |
demo71 | 0:694446246c35 | 242 | wait(0.3); |
demo71 | 0:694446246c35 | 243 | } |
demo71 | 0:694446246c35 | 244 | //************************* |
demo71 | 0:694446246c35 | 245 | |
demo71 | 0:694446246c35 | 246 | if(Kd>=999){ |
demo71 | 0:694446246c35 | 247 | Kd=999; |
demo71 | 0:694446246c35 | 248 | } |
demo71 | 0:694446246c35 | 249 | else if (Kd<=0){ |
demo71 | 0:694446246c35 | 250 | Kd=0; |
demo71 | 0:694446246c35 | 251 | } |
demo71 | 0:694446246c35 | 252 | |
demo71 | 0:694446246c35 | 253 | lcd.locate(0,1); |
demo71 | 0:694446246c35 | 254 | lcd.printf(">Kd=%d ",Kd); |
demo71 | 0:694446246c35 | 255 | |
demo71 | 0:694446246c35 | 256 | if(Boton1.falling()){ |
demo71 | 0:694446246c35 | 257 | goto set_Sp; |
demo71 | 0:694446246c35 | 258 | } |
demo71 | 0:694446246c35 | 259 | |
demo71 | 0:694446246c35 | 260 | if(Boton4.falling()){ |
demo71 | 0:694446246c35 | 261 | goto PID; |
demo71 | 0:694446246c35 | 262 | } |
demo71 | 0:694446246c35 | 263 | |
demo71 | 0:694446246c35 | 264 | } |
demo71 | 0:694446246c35 | 265 | |
demo71 | 1:93e71ac5cf91 | 266 | //Configuracion del parametro Sp |
demo71 | 0:694446246c35 | 267 | set_Sp: |
demo71 | 0:694446246c35 | 268 | |
demo71 | 0:694446246c35 | 269 | lcd.cls(); |
demo71 | 0:694446246c35 | 270 | lcd.locate(8,1); |
demo71 | 0:694446246c35 | 271 | lcd.printf(">Sp= "); |
demo71 | 0:694446246c35 | 272 | |
demo71 | 0:694446246c35 | 273 | lcd.locate(1,0); |
demo71 | 0:694446246c35 | 274 | lcd.printf("Kp=%d",Kp); |
demo71 | 0:694446246c35 | 275 | lcd.locate(9,0); |
demo71 | 0:694446246c35 | 276 | lcd.printf("Ki=%d",Ki); |
demo71 | 0:694446246c35 | 277 | lcd.locate(1,1); |
demo71 | 0:694446246c35 | 278 | lcd.printf("Kd=%d",Kd); |
demo71 | 0:694446246c35 | 279 | |
demo71 | 1:93e71ac5cf91 | 280 | |
demo71 | 0:694446246c35 | 281 | while(1){ |
demo71 | 0:694446246c35 | 282 | |
demo71 | 0:694446246c35 | 283 | if(Boton2.falling()){ |
demo71 | 0:694446246c35 | 284 | Sp++; |
demo71 | 0:694446246c35 | 285 | tu.start(); |
demo71 | 0:694446246c35 | 286 | } |
demo71 | 0:694446246c35 | 287 | //AUMENTO*10 ************** |
demo71 | 0:694446246c35 | 288 | if(Boton2.rising()){ |
demo71 | 0:694446246c35 | 289 | tu.stop(); |
demo71 | 0:694446246c35 | 290 | tu.reset(); |
demo71 | 0:694446246c35 | 291 | } |
demo71 | 0:694446246c35 | 292 | if(tu.read()>=2.0 && tu.read()<4.0){ |
demo71 | 0:694446246c35 | 293 | Sp=Sp+10; |
demo71 | 0:694446246c35 | 294 | wait(0.3); |
demo71 | 0:694446246c35 | 295 | } |
demo71 | 0:694446246c35 | 296 | else if(tu.read()>=4.0){ |
demo71 | 0:694446246c35 | 297 | Sp=Sp+100; |
demo71 | 0:694446246c35 | 298 | wait(0.3); |
demo71 | 0:694446246c35 | 299 | } |
demo71 | 0:694446246c35 | 300 | //************************* |
demo71 | 0:694446246c35 | 301 | if(Boton3.falling()){ |
demo71 | 0:694446246c35 | 302 | Sp--; |
demo71 | 0:694446246c35 | 303 | td.start(); |
demo71 | 0:694446246c35 | 304 | } |
demo71 | 0:694446246c35 | 305 | //DISMINUYE*10 ************** |
demo71 | 0:694446246c35 | 306 | if(Boton3.rising()){ |
demo71 | 0:694446246c35 | 307 | td.stop(); |
demo71 | 0:694446246c35 | 308 | td.reset(); |
demo71 | 0:694446246c35 | 309 | } |
demo71 | 0:694446246c35 | 310 | if(td.read()>=2.0 && td.read()<4.0){ |
demo71 | 0:694446246c35 | 311 | Sp=Sp-10; |
demo71 | 0:694446246c35 | 312 | wait(0.3); |
demo71 | 0:694446246c35 | 313 | } |
demo71 | 0:694446246c35 | 314 | else if(td.read()>=4.0){ |
demo71 | 0:694446246c35 | 315 | Sp=Sp-100; |
demo71 | 0:694446246c35 | 316 | wait(0.3); |
demo71 | 0:694446246c35 | 317 | } |
demo71 | 0:694446246c35 | 318 | //************************* |
demo71 | 0:694446246c35 | 319 | |
demo71 | 0:694446246c35 | 320 | if(Sp>=999){ |
demo71 | 0:694446246c35 | 321 | Sp=999; |
demo71 | 0:694446246c35 | 322 | } |
demo71 | 0:694446246c35 | 323 | else if (Sp<=0){ |
demo71 | 0:694446246c35 | 324 | Sp=0; |
demo71 | 0:694446246c35 | 325 | } |
demo71 | 0:694446246c35 | 326 | |
demo71 | 0:694446246c35 | 327 | lcd.locate(8,1); |
demo71 | 0:694446246c35 | 328 | lcd.printf(">Sp=%d ",Sp); |
demo71 | 0:694446246c35 | 329 | |
demo71 | 0:694446246c35 | 330 | if(Boton1.falling()){ |
demo71 | 0:694446246c35 | 331 | goto set_Kp; |
demo71 | 0:694446246c35 | 332 | } |
demo71 | 0:694446246c35 | 333 | |
demo71 | 0:694446246c35 | 334 | if(Boton4.falling()){ |
demo71 | 0:694446246c35 | 335 | goto PID; |
demo71 | 0:694446246c35 | 336 | } |
demo71 | 0:694446246c35 | 337 | |
demo71 | 0:694446246c35 | 338 | } |
demo71 | 1:93e71ac5cf91 | 339 | //Inicia algoritmo del PID |
demo71 | 0:694446246c35 | 340 | PID: |
demo71 | 0:694446246c35 | 341 | lcd.cls(); |
demo71 | 0:694446246c35 | 342 | |
demo71 | 0:694446246c35 | 343 | while(1){ |
demo71 | 0:694446246c35 | 344 | |
demo71 | 0:694446246c35 | 345 | yT=vcond.read()*3.3; |
demo71 | 0:694446246c35 | 346 | eT=Sp-yT; |
demo71 | 0:694446246c35 | 347 | iT=Ki*eT+iT0; //Accion Integral |
demo71 | 0:694446246c35 | 348 | dT=Kd*(eT-eT0); //Accion Derivativa |
demo71 | 0:694446246c35 | 349 | uT=iT+Kp*eT+dT; |
demo71 | 0:694446246c35 | 350 | if (uT>3.3) { //Salida PID si es mayor que el MAX |
demo71 | 0:694446246c35 | 351 | uT=3.3;} |
demo71 | 0:694446246c35 | 352 | else if (uT<0){ //Salida PID si es menor que el MIN |
demo71 | 0:694446246c35 | 353 | uT=0; |
demo71 | 0:694446246c35 | 354 | } |
demo71 | 1:93e71ac5cf91 | 355 | iT0=iT; //Actualizando variables para el paso siguiente |
demo71 | 0:694446246c35 | 356 | eT0=eT; |
demo71 | 1:93e71ac5cf91 | 357 | control=(float)uT/3.3;//Accion de control mapeada a PWM; salida de la tarjeta. |
demo71 | 0:694446246c35 | 358 | |
demo71 | 1:93e71ac5cf91 | 359 | //Mostrando error y salida actual |
demo71 | 0:694446246c35 | 360 | lcd.locate(0,0); |
demo71 | 0:694446246c35 | 361 | lcd.printf("Error=%.2f ",eT); |
demo71 | 0:694446246c35 | 362 | lcd.locate(0,1); |
demo71 | 0:694446246c35 | 363 | lcd.printf("Y_act=%.2f",yT); |
demo71 | 0:694446246c35 | 364 | wait(0.2); |
demo71 | 0:694446246c35 | 365 | } |
demo71 | 1:93e71ac5cf91 | 366 | |
demo71 | 0:694446246c35 | 367 | |
demo71 | 0:694446246c35 | 368 | |
demo71 | 0:694446246c35 | 369 | } |
demo71 | 0:694446246c35 | 370 | |
demo71 | 0:694446246c35 | 371 |