Control PID de un circuito RC con parametros agregados mediante encoder de cuadratura y visualizado en LCD 16x2
Dependencies: DebouncedIn QEI TextLCD mbed
main.cpp@0:694446246c35, 2015-09-28 (annotated)
- Committer:
- demo71
- Date:
- Mon Sep 28 22:43:43 2015 +0000
- Revision:
- 0:694446246c35
- Child:
- 1:f0fa90a6fb6e
Control PID para un circuito RC con parametros ingresados mediante pulsadores y visualizado en una pantalla LCD 16x2
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 | 0:694446246c35 | 25 | int Kp=0,Ki=0,Kd=0,Sp=0,p,i; |
demo71 | 0:694446246c35 | 26 | float rT,eT,iT,dT,yT,uT,iT0=0,eT0=0,iT_1=0,eT_1=0; |
demo71 | 0:694446246c35 | 27 | Timer tu; |
demo71 | 0:694446246c35 | 28 | Timer td; |
demo71 | 0:694446246c35 | 29 | |
demo71 | 0:694446246c35 | 30 | int main() { |
demo71 | 0:694446246c35 | 31 | |
demo71 | 0:694446246c35 | 32 | lcd.cls(); |
demo71 | 0:694446246c35 | 33 | //escribimos un comando segun el manual del modulo LCD |
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 | 0:694446246c35 | 43 | |
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 | 0:694446246c35 | 57 | //Kp=0; |
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 | 0:694446246c35 | 117 | set_Ki: |
demo71 | 0:694446246c35 | 118 | |
demo71 | 0:694446246c35 | 119 | lcd.cls(); |
demo71 | 0:694446246c35 | 120 | lcd.locate(8,0); |
demo71 | 0:694446246c35 | 121 | lcd.printf(">Ki= "); |
demo71 | 0:694446246c35 | 122 | |
demo71 | 0:694446246c35 | 123 | lcd.locate(1,0); |
demo71 | 0:694446246c35 | 124 | lcd.printf("Kp=%d",Kp); |
demo71 | 0:694446246c35 | 125 | lcd.locate(1,1); |
demo71 | 0:694446246c35 | 126 | lcd.printf("Kd=%d",Kd); |
demo71 | 0:694446246c35 | 127 | lcd.locate(9,1); |
demo71 | 0:694446246c35 | 128 | lcd.printf("Sp=%d",Sp); |
demo71 | 0:694446246c35 | 129 | |
demo71 | 0:694446246c35 | 130 | //Ki=0; |
demo71 | 0:694446246c35 | 131 | while(1){ |
demo71 | 0:694446246c35 | 132 | |
demo71 | 0:694446246c35 | 133 | if(Boton2.falling()){ |
demo71 | 0:694446246c35 | 134 | Ki++; |
demo71 | 0:694446246c35 | 135 | tu.start(); |
demo71 | 0:694446246c35 | 136 | } |
demo71 | 0:694446246c35 | 137 | //AUMENTO*10 ************** |
demo71 | 0:694446246c35 | 138 | if(Boton2.rising()){ |
demo71 | 0:694446246c35 | 139 | tu.stop(); |
demo71 | 0:694446246c35 | 140 | tu.reset(); |
demo71 | 0:694446246c35 | 141 | } |
demo71 | 0:694446246c35 | 142 | if(tu.read()>=2.0 && tu.read()<4.0){ |
demo71 | 0:694446246c35 | 143 | Ki=Ki+10; |
demo71 | 0:694446246c35 | 144 | wait(0.3); |
demo71 | 0:694446246c35 | 145 | } |
demo71 | 0:694446246c35 | 146 | else if(tu.read()>=4.0){ |
demo71 | 0:694446246c35 | 147 | Ki=Ki+100; |
demo71 | 0:694446246c35 | 148 | wait(0.3); |
demo71 | 0:694446246c35 | 149 | } |
demo71 | 0:694446246c35 | 150 | //************************* |
demo71 | 0:694446246c35 | 151 | if(Boton3.falling()){ |
demo71 | 0:694446246c35 | 152 | Ki--; |
demo71 | 0:694446246c35 | 153 | td.start(); |
demo71 | 0:694446246c35 | 154 | } |
demo71 | 0:694446246c35 | 155 | //DISMINUYE*10 ************** |
demo71 | 0:694446246c35 | 156 | if(Boton3.rising()){ |
demo71 | 0:694446246c35 | 157 | td.stop(); |
demo71 | 0:694446246c35 | 158 | td.reset(); |
demo71 | 0:694446246c35 | 159 | } |
demo71 | 0:694446246c35 | 160 | if(td.read()>=2.0 && td.read()<4.0){ |
demo71 | 0:694446246c35 | 161 | Ki=Ki-10; |
demo71 | 0:694446246c35 | 162 | wait(0.3); |
demo71 | 0:694446246c35 | 163 | } |
demo71 | 0:694446246c35 | 164 | else if(td.read()>=4.0){ |
demo71 | 0:694446246c35 | 165 | Ki=Ki-100; |
demo71 | 0:694446246c35 | 166 | wait(0.3); |
demo71 | 0:694446246c35 | 167 | } |
demo71 | 0:694446246c35 | 168 | //************************* |
demo71 | 0:694446246c35 | 169 | |
demo71 | 0:694446246c35 | 170 | if(Ki>=999){ |
demo71 | 0:694446246c35 | 171 | Ki=999; |
demo71 | 0:694446246c35 | 172 | } |
demo71 | 0:694446246c35 | 173 | else if (Ki<=0){ |
demo71 | 0:694446246c35 | 174 | Ki=0; |
demo71 | 0:694446246c35 | 175 | } |
demo71 | 0:694446246c35 | 176 | |
demo71 | 0:694446246c35 | 177 | lcd.locate(8,0); |
demo71 | 0:694446246c35 | 178 | lcd.printf(">Ki=%d ",Ki); |
demo71 | 0:694446246c35 | 179 | |
demo71 | 0:694446246c35 | 180 | if(Boton1.falling()){ |
demo71 | 0:694446246c35 | 181 | goto set_Kd; |
demo71 | 0:694446246c35 | 182 | } |
demo71 | 0:694446246c35 | 183 | |
demo71 | 0:694446246c35 | 184 | if(Boton4.falling()){ |
demo71 | 0:694446246c35 | 185 | goto PID; |
demo71 | 0:694446246c35 | 186 | } |
demo71 | 0:694446246c35 | 187 | |
demo71 | 0:694446246c35 | 188 | } |
demo71 | 0:694446246c35 | 189 | |
demo71 | 0:694446246c35 | 190 | set_Kd: |
demo71 | 0:694446246c35 | 191 | |
demo71 | 0:694446246c35 | 192 | lcd.cls(); |
demo71 | 0:694446246c35 | 193 | lcd.locate(0,1); |
demo71 | 0:694446246c35 | 194 | lcd.printf(">Kd= "); |
demo71 | 0:694446246c35 | 195 | |
demo71 | 0:694446246c35 | 196 | lcd.locate(1,0); |
demo71 | 0:694446246c35 | 197 | lcd.printf("Kp=%d",Kp); |
demo71 | 0:694446246c35 | 198 | lcd.locate(9,0); |
demo71 | 0:694446246c35 | 199 | lcd.printf("Ki=%d",Ki); |
demo71 | 0:694446246c35 | 200 | lcd.locate(9,1); |
demo71 | 0:694446246c35 | 201 | lcd.printf("Sp=%d",Sp); |
demo71 | 0:694446246c35 | 202 | |
demo71 | 0:694446246c35 | 203 | |
demo71 | 0:694446246c35 | 204 | //Kd=0; |
demo71 | 0:694446246c35 | 205 | while(1){ |
demo71 | 0:694446246c35 | 206 | |
demo71 | 0:694446246c35 | 207 | if(Boton2.falling()){ |
demo71 | 0:694446246c35 | 208 | Kd++; |
demo71 | 0:694446246c35 | 209 | tu.start(); |
demo71 | 0:694446246c35 | 210 | } |
demo71 | 0:694446246c35 | 211 | //AUMENTO*10 ************** |
demo71 | 0:694446246c35 | 212 | if(Boton2.rising()){ |
demo71 | 0:694446246c35 | 213 | tu.stop(); |
demo71 | 0:694446246c35 | 214 | tu.reset(); |
demo71 | 0:694446246c35 | 215 | } |
demo71 | 0:694446246c35 | 216 | if(tu.read()>=2.0 && tu.read()<4.0){ |
demo71 | 0:694446246c35 | 217 | Kd=Kd+10; |
demo71 | 0:694446246c35 | 218 | wait(0.3); |
demo71 | 0:694446246c35 | 219 | } |
demo71 | 0:694446246c35 | 220 | else if(tu.read()>=4.0){ |
demo71 | 0:694446246c35 | 221 | Kd=Kd+100; |
demo71 | 0:694446246c35 | 222 | wait(0.3); |
demo71 | 0:694446246c35 | 223 | } |
demo71 | 0:694446246c35 | 224 | //************************* |
demo71 | 0:694446246c35 | 225 | if(Boton3.falling()){ |
demo71 | 0:694446246c35 | 226 | Kd--; |
demo71 | 0:694446246c35 | 227 | td.start(); |
demo71 | 0:694446246c35 | 228 | } |
demo71 | 0:694446246c35 | 229 | //DISMINUYE*10 ************** |
demo71 | 0:694446246c35 | 230 | if(Boton3.rising()){ |
demo71 | 0:694446246c35 | 231 | td.stop(); |
demo71 | 0:694446246c35 | 232 | td.reset(); |
demo71 | 0:694446246c35 | 233 | } |
demo71 | 0:694446246c35 | 234 | if(td.read()>=2.0 && td.read()<4.0){ |
demo71 | 0:694446246c35 | 235 | Kd=Kd-10; |
demo71 | 0:694446246c35 | 236 | wait(0.3); |
demo71 | 0:694446246c35 | 237 | } |
demo71 | 0:694446246c35 | 238 | else if(td.read()>=4.0){ |
demo71 | 0:694446246c35 | 239 | Kd=Kd-100; |
demo71 | 0:694446246c35 | 240 | wait(0.3); |
demo71 | 0:694446246c35 | 241 | } |
demo71 | 0:694446246c35 | 242 | //************************* |
demo71 | 0:694446246c35 | 243 | |
demo71 | 0:694446246c35 | 244 | if(Kd>=999){ |
demo71 | 0:694446246c35 | 245 | Kd=999; |
demo71 | 0:694446246c35 | 246 | } |
demo71 | 0:694446246c35 | 247 | else if (Kd<=0){ |
demo71 | 0:694446246c35 | 248 | Kd=0; |
demo71 | 0:694446246c35 | 249 | } |
demo71 | 0:694446246c35 | 250 | |
demo71 | 0:694446246c35 | 251 | lcd.locate(0,1); |
demo71 | 0:694446246c35 | 252 | lcd.printf(">Kd=%d ",Kd); |
demo71 | 0:694446246c35 | 253 | |
demo71 | 0:694446246c35 | 254 | if(Boton1.falling()){ |
demo71 | 0:694446246c35 | 255 | goto set_Sp; |
demo71 | 0:694446246c35 | 256 | } |
demo71 | 0:694446246c35 | 257 | |
demo71 | 0:694446246c35 | 258 | if(Boton4.falling()){ |
demo71 | 0:694446246c35 | 259 | goto PID; |
demo71 | 0:694446246c35 | 260 | } |
demo71 | 0:694446246c35 | 261 | |
demo71 | 0:694446246c35 | 262 | } |
demo71 | 0:694446246c35 | 263 | |
demo71 | 0:694446246c35 | 264 | set_Sp: |
demo71 | 0:694446246c35 | 265 | |
demo71 | 0:694446246c35 | 266 | lcd.cls(); |
demo71 | 0:694446246c35 | 267 | lcd.locate(8,1); |
demo71 | 0:694446246c35 | 268 | lcd.printf(">Sp= "); |
demo71 | 0:694446246c35 | 269 | |
demo71 | 0:694446246c35 | 270 | lcd.locate(1,0); |
demo71 | 0:694446246c35 | 271 | lcd.printf("Kp=%d",Kp); |
demo71 | 0:694446246c35 | 272 | lcd.locate(9,0); |
demo71 | 0:694446246c35 | 273 | lcd.printf("Ki=%d",Ki); |
demo71 | 0:694446246c35 | 274 | lcd.locate(1,1); |
demo71 | 0:694446246c35 | 275 | lcd.printf("Kd=%d",Kd); |
demo71 | 0:694446246c35 | 276 | |
demo71 | 0:694446246c35 | 277 | //Sp=0; |
demo71 | 0:694446246c35 | 278 | while(1){ |
demo71 | 0:694446246c35 | 279 | |
demo71 | 0:694446246c35 | 280 | if(Boton2.falling()){ |
demo71 | 0:694446246c35 | 281 | Sp++; |
demo71 | 0:694446246c35 | 282 | tu.start(); |
demo71 | 0:694446246c35 | 283 | } |
demo71 | 0:694446246c35 | 284 | //AUMENTO*10 ************** |
demo71 | 0:694446246c35 | 285 | if(Boton2.rising()){ |
demo71 | 0:694446246c35 | 286 | tu.stop(); |
demo71 | 0:694446246c35 | 287 | tu.reset(); |
demo71 | 0:694446246c35 | 288 | } |
demo71 | 0:694446246c35 | 289 | if(tu.read()>=2.0 && tu.read()<4.0){ |
demo71 | 0:694446246c35 | 290 | Sp=Sp+10; |
demo71 | 0:694446246c35 | 291 | wait(0.3); |
demo71 | 0:694446246c35 | 292 | } |
demo71 | 0:694446246c35 | 293 | else if(tu.read()>=4.0){ |
demo71 | 0:694446246c35 | 294 | Sp=Sp+100; |
demo71 | 0:694446246c35 | 295 | wait(0.3); |
demo71 | 0:694446246c35 | 296 | } |
demo71 | 0:694446246c35 | 297 | //************************* |
demo71 | 0:694446246c35 | 298 | if(Boton3.falling()){ |
demo71 | 0:694446246c35 | 299 | Sp--; |
demo71 | 0:694446246c35 | 300 | td.start(); |
demo71 | 0:694446246c35 | 301 | } |
demo71 | 0:694446246c35 | 302 | //DISMINUYE*10 ************** |
demo71 | 0:694446246c35 | 303 | if(Boton3.rising()){ |
demo71 | 0:694446246c35 | 304 | td.stop(); |
demo71 | 0:694446246c35 | 305 | td.reset(); |
demo71 | 0:694446246c35 | 306 | } |
demo71 | 0:694446246c35 | 307 | if(td.read()>=2.0 && td.read()<4.0){ |
demo71 | 0:694446246c35 | 308 | Sp=Sp-10; |
demo71 | 0:694446246c35 | 309 | wait(0.3); |
demo71 | 0:694446246c35 | 310 | } |
demo71 | 0:694446246c35 | 311 | else if(td.read()>=4.0){ |
demo71 | 0:694446246c35 | 312 | Sp=Sp-100; |
demo71 | 0:694446246c35 | 313 | wait(0.3); |
demo71 | 0:694446246c35 | 314 | } |
demo71 | 0:694446246c35 | 315 | //************************* |
demo71 | 0:694446246c35 | 316 | |
demo71 | 0:694446246c35 | 317 | if(Sp>=999){ |
demo71 | 0:694446246c35 | 318 | Sp=999; |
demo71 | 0:694446246c35 | 319 | } |
demo71 | 0:694446246c35 | 320 | else if (Sp<=0){ |
demo71 | 0:694446246c35 | 321 | Sp=0; |
demo71 | 0:694446246c35 | 322 | } |
demo71 | 0:694446246c35 | 323 | |
demo71 | 0:694446246c35 | 324 | lcd.locate(8,1); |
demo71 | 0:694446246c35 | 325 | lcd.printf(">Sp=%d ",Sp); |
demo71 | 0:694446246c35 | 326 | |
demo71 | 0:694446246c35 | 327 | if(Boton1.falling()){ |
demo71 | 0:694446246c35 | 328 | goto set_Kp; |
demo71 | 0:694446246c35 | 329 | } |
demo71 | 0:694446246c35 | 330 | |
demo71 | 0:694446246c35 | 331 | if(Boton4.falling()){ |
demo71 | 0:694446246c35 | 332 | goto PID; |
demo71 | 0:694446246c35 | 333 | } |
demo71 | 0:694446246c35 | 334 | |
demo71 | 0:694446246c35 | 335 | } |
demo71 | 0:694446246c35 | 336 | |
demo71 | 0:694446246c35 | 337 | PID: |
demo71 | 0:694446246c35 | 338 | lcd.cls(); |
demo71 | 0:694446246c35 | 339 | |
demo71 | 0:694446246c35 | 340 | //Sp=2; |
demo71 | 0:694446246c35 | 341 | //Kp=(float)12.105; |
demo71 | 0:694446246c35 | 342 | //Ki=(float)5.32; |
demo71 | 0:694446246c35 | 343 | //Kd=(float)2; |
demo71 | 0:694446246c35 | 344 | while(1){ |
demo71 | 0:694446246c35 | 345 | |
demo71 | 0:694446246c35 | 346 | yT=vcond.read()*3.3; |
demo71 | 0:694446246c35 | 347 | eT=Sp-yT; |
demo71 | 0:694446246c35 | 348 | iT=Ki*eT+iT0; //Accion Integral |
demo71 | 0:694446246c35 | 349 | dT=Kd*(eT-eT0); //Accion Derivativa |
demo71 | 0:694446246c35 | 350 | uT=iT+Kp*eT+dT; |
demo71 | 0:694446246c35 | 351 | if (uT>3.3) { //Salida PID si es mayor que el MAX |
demo71 | 0:694446246c35 | 352 | uT=3.3;} |
demo71 | 0:694446246c35 | 353 | else if (uT<0){ //Salida PID si es menor que el MIN |
demo71 | 0:694446246c35 | 354 | uT=0; |
demo71 | 0:694446246c35 | 355 | } |
demo71 | 0:694446246c35 | 356 | iT0=iT; //Guardar variables |
demo71 | 0:694446246c35 | 357 | eT0=eT; |
demo71 | 0:694446246c35 | 358 | control=(float)uT/3.3; |
demo71 | 0:694446246c35 | 359 | |
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 | 0:694446246c35 | 366 | //lcd.locate(3,0); |
demo71 | 0:694446246c35 | 367 | //lcd.writeCommand(C1); |
demo71 | 0:694446246c35 | 368 | |
demo71 | 0:694446246c35 | 369 | |
demo71 | 0:694446246c35 | 370 | |
demo71 | 0:694446246c35 | 371 | } |
demo71 | 0:694446246c35 | 372 | |
demo71 | 0:694446246c35 | 373 |