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@1:f0fa90a6fb6e, 2015-10-27 (annotated)
- Committer:
- demo71
- Date:
- Tue Oct 27 01:10:35 2015 +0000
- Revision:
- 1:f0fa90a6fb6e
- Parent:
- 0:694446246c35
Control PID de un circuito RC con parametros agregados mediante un encoder de cuadratura y visualizado en 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 | 1:f0fa90a6fb6e | 7 | #include "QEI.h" |
demo71 | 0:694446246c35 | 8 | |
demo71 | 0:694446246c35 | 9 | //Declaracion de entradas y salidas |
demo71 | 1:f0fa90a6fb6e | 10 | DebouncedIn Boton1(PTE20); //Boton para confirmar |
demo71 | 1:f0fa90a6fb6e | 11 | DebouncedIn Boton2(PTE21); //Boton para confirmar |
demo71 | 1:f0fa90a6fb6e | 12 | |
demo71 | 1:f0fa90a6fb6e | 13 | //Configuracion encoder |
demo71 | 1:f0fa90a6fb6e | 14 | QEI wheel (PTD5, PTD0, NC, 100); |
demo71 | 0:694446246c35 | 15 | |
demo71 | 0:694446246c35 | 16 | PwmOut control(PTE29); |
demo71 | 0:694446246c35 | 17 | AnalogIn vcond(PTB0); |
demo71 | 1:f0fa90a6fb6e | 18 | |
demo71 | 1:f0fa90a6fb6e | 19 | //Configuracion pantalla LCD |
demo71 | 0:694446246c35 | 20 | TextLCD lcd(PTB8,PTB9,PTB10,PTB11,PTE2,PTE3); // rs, e, d4-d7 |
demo71 | 0:694446246c35 | 21 | |
demo71 | 0:694446246c35 | 22 | //codigos movimiento del cursor LCD |
demo71 | 1:f0fa90a6fb6e | 23 | //int C2=0x18; // desplaza izquierda |
demo71 | 1:f0fa90a6fb6e | 24 | //int C3=0x1A; // desplaza derecha |
demo71 | 1:f0fa90a6fb6e | 25 | //int C4=0x0C; // quito cursor bajo |
demo71 | 1:f0fa90a6fb6e | 26 | //int C1=0x0F; // solo muestra el cursor |
demo71 | 0:694446246c35 | 27 | |
demo71 | 1:f0fa90a6fb6e | 28 | int Kp=0,Ki=0,Kd=0; |
demo71 | 1:f0fa90a6fb6e | 29 | float Sp=0; |
demo71 | 1:f0fa90a6fb6e | 30 | float K1,K2,K3,ek=0,ekm1=0,ekm2=0,Ts=0.1; //Ts Tiempo Muestreo |
demo71 | 1:f0fa90a6fb6e | 31 | float yT,uk=0,ukm1=0; |
demo71 | 1:f0fa90a6fb6e | 32 | |
demo71 | 1:f0fa90a6fb6e | 33 | |
demo71 | 0:694446246c35 | 34 | |
demo71 | 0:694446246c35 | 35 | int main() { |
demo71 | 0:694446246c35 | 36 | |
demo71 | 0:694446246c35 | 37 | lcd.cls(); |
demo71 | 0:694446246c35 | 38 | lcd.locate(1,0); |
demo71 | 0:694446246c35 | 39 | lcd.printf("Kp=%d",Kp); |
demo71 | 0:694446246c35 | 40 | lcd.locate(9,0); |
demo71 | 0:694446246c35 | 41 | lcd.printf("Ki%d=",Ki); |
demo71 | 0:694446246c35 | 42 | lcd.locate(1,1); |
demo71 | 0:694446246c35 | 43 | lcd.printf("Kd=%d",Kd); |
demo71 | 0:694446246c35 | 44 | lcd.locate(9,1); |
demo71 | 1:f0fa90a6fb6e | 45 | lcd.printf("Sp=%.1f",Sp); |
demo71 | 0:694446246c35 | 46 | |
demo71 | 0:694446246c35 | 47 | |
demo71 | 0:694446246c35 | 48 | set_Kp: |
demo71 | 0:694446246c35 | 49 | lcd.cls(); |
demo71 | 0:694446246c35 | 50 | lcd.locate(0,0); |
demo71 | 0:694446246c35 | 51 | lcd.printf(">Kp= "); |
demo71 | 0:694446246c35 | 52 | |
demo71 | 0:694446246c35 | 53 | lcd.locate(9,0); |
demo71 | 0:694446246c35 | 54 | lcd.printf("Ki=%d",Ki); |
demo71 | 0:694446246c35 | 55 | lcd.locate(1,1); |
demo71 | 0:694446246c35 | 56 | lcd.printf("Kd=%d",Kd); |
demo71 | 0:694446246c35 | 57 | lcd.locate(9,1); |
demo71 | 1:f0fa90a6fb6e | 58 | lcd.printf("Sp=%.1f",Sp); |
demo71 | 0:694446246c35 | 59 | |
demo71 | 0:694446246c35 | 60 | |
demo71 | 0:694446246c35 | 61 | while(1){ |
demo71 | 0:694446246c35 | 62 | |
demo71 | 1:f0fa90a6fb6e | 63 | Kp=Kp+wheel.getPulses(); |
demo71 | 1:f0fa90a6fb6e | 64 | wheel.reset(); |
demo71 | 1:f0fa90a6fb6e | 65 | |
demo71 | 0:694446246c35 | 66 | if(Kp>=999){ |
demo71 | 0:694446246c35 | 67 | Kp=999; |
demo71 | 0:694446246c35 | 68 | } |
demo71 | 0:694446246c35 | 69 | else if (Kp<=0){ |
demo71 | 0:694446246c35 | 70 | Kp=0; |
demo71 | 0:694446246c35 | 71 | } |
demo71 | 0:694446246c35 | 72 | |
demo71 | 0:694446246c35 | 73 | lcd.locate(0,0); |
demo71 | 0:694446246c35 | 74 | lcd.printf(">Kp=%d ",Kp); |
demo71 | 0:694446246c35 | 75 | |
demo71 | 1:f0fa90a6fb6e | 76 | if(Boton2.falling()){ |
demo71 | 0:694446246c35 | 77 | goto set_Ki; |
demo71 | 0:694446246c35 | 78 | } |
demo71 | 0:694446246c35 | 79 | |
demo71 | 1:f0fa90a6fb6e | 80 | |
demo71 | 1:f0fa90a6fb6e | 81 | if(Boton1.falling()){ |
demo71 | 0:694446246c35 | 82 | goto PID; |
demo71 | 0:694446246c35 | 83 | } |
demo71 | 0:694446246c35 | 84 | |
demo71 | 0:694446246c35 | 85 | } |
demo71 | 0:694446246c35 | 86 | |
demo71 | 0:694446246c35 | 87 | set_Ki: |
demo71 | 0:694446246c35 | 88 | |
demo71 | 0:694446246c35 | 89 | lcd.cls(); |
demo71 | 0:694446246c35 | 90 | lcd.locate(8,0); |
demo71 | 0:694446246c35 | 91 | lcd.printf(">Ki= "); |
demo71 | 0:694446246c35 | 92 | |
demo71 | 0:694446246c35 | 93 | lcd.locate(1,0); |
demo71 | 0:694446246c35 | 94 | lcd.printf("Kp=%d",Kp); |
demo71 | 0:694446246c35 | 95 | lcd.locate(1,1); |
demo71 | 0:694446246c35 | 96 | lcd.printf("Kd=%d",Kd); |
demo71 | 0:694446246c35 | 97 | lcd.locate(9,1); |
demo71 | 1:f0fa90a6fb6e | 98 | lcd.printf("Sp=%.1f",Sp); |
demo71 | 0:694446246c35 | 99 | |
demo71 | 0:694446246c35 | 100 | //Ki=0; |
demo71 | 0:694446246c35 | 101 | while(1){ |
demo71 | 0:694446246c35 | 102 | |
demo71 | 1:f0fa90a6fb6e | 103 | Ki=Ki+wheel.getPulses(); |
demo71 | 1:f0fa90a6fb6e | 104 | wheel.reset(); |
demo71 | 0:694446246c35 | 105 | |
demo71 | 0:694446246c35 | 106 | if(Ki>=999){ |
demo71 | 0:694446246c35 | 107 | Ki=999; |
demo71 | 0:694446246c35 | 108 | } |
demo71 | 0:694446246c35 | 109 | else if (Ki<=0){ |
demo71 | 0:694446246c35 | 110 | Ki=0; |
demo71 | 0:694446246c35 | 111 | } |
demo71 | 0:694446246c35 | 112 | |
demo71 | 0:694446246c35 | 113 | lcd.locate(8,0); |
demo71 | 0:694446246c35 | 114 | lcd.printf(">Ki=%d ",Ki); |
demo71 | 0:694446246c35 | 115 | |
demo71 | 1:f0fa90a6fb6e | 116 | if(Boton2.falling()){ |
demo71 | 0:694446246c35 | 117 | goto set_Kd; |
demo71 | 0:694446246c35 | 118 | } |
demo71 | 0:694446246c35 | 119 | |
demo71 | 1:f0fa90a6fb6e | 120 | if(Boton1.falling()){ |
demo71 | 0:694446246c35 | 121 | goto PID; |
demo71 | 0:694446246c35 | 122 | } |
demo71 | 0:694446246c35 | 123 | |
demo71 | 0:694446246c35 | 124 | } |
demo71 | 0:694446246c35 | 125 | |
demo71 | 0:694446246c35 | 126 | set_Kd: |
demo71 | 0:694446246c35 | 127 | |
demo71 | 0:694446246c35 | 128 | lcd.cls(); |
demo71 | 0:694446246c35 | 129 | lcd.locate(0,1); |
demo71 | 0:694446246c35 | 130 | lcd.printf(">Kd= "); |
demo71 | 0:694446246c35 | 131 | |
demo71 | 0:694446246c35 | 132 | lcd.locate(1,0); |
demo71 | 0:694446246c35 | 133 | lcd.printf("Kp=%d",Kp); |
demo71 | 0:694446246c35 | 134 | lcd.locate(9,0); |
demo71 | 0:694446246c35 | 135 | lcd.printf("Ki=%d",Ki); |
demo71 | 0:694446246c35 | 136 | lcd.locate(9,1); |
demo71 | 1:f0fa90a6fb6e | 137 | lcd.printf("Sp=%.1f",Sp); |
demo71 | 0:694446246c35 | 138 | |
demo71 | 0:694446246c35 | 139 | |
demo71 | 0:694446246c35 | 140 | //Kd=0; |
demo71 | 0:694446246c35 | 141 | while(1){ |
demo71 | 0:694446246c35 | 142 | |
demo71 | 1:f0fa90a6fb6e | 143 | Kd=Kd+wheel.getPulses(); |
demo71 | 1:f0fa90a6fb6e | 144 | wheel.reset(); |
demo71 | 0:694446246c35 | 145 | |
demo71 | 0:694446246c35 | 146 | if(Kd>=999){ |
demo71 | 0:694446246c35 | 147 | Kd=999; |
demo71 | 0:694446246c35 | 148 | } |
demo71 | 0:694446246c35 | 149 | else if (Kd<=0){ |
demo71 | 0:694446246c35 | 150 | Kd=0; |
demo71 | 0:694446246c35 | 151 | } |
demo71 | 0:694446246c35 | 152 | |
demo71 | 0:694446246c35 | 153 | lcd.locate(0,1); |
demo71 | 0:694446246c35 | 154 | lcd.printf(">Kd=%d ",Kd); |
demo71 | 0:694446246c35 | 155 | |
demo71 | 1:f0fa90a6fb6e | 156 | if(Boton2.falling()){ |
demo71 | 0:694446246c35 | 157 | goto set_Sp; |
demo71 | 0:694446246c35 | 158 | } |
demo71 | 0:694446246c35 | 159 | |
demo71 | 1:f0fa90a6fb6e | 160 | if(Boton1.falling()){ |
demo71 | 0:694446246c35 | 161 | goto PID; |
demo71 | 0:694446246c35 | 162 | } |
demo71 | 0:694446246c35 | 163 | |
demo71 | 0:694446246c35 | 164 | } |
demo71 | 0:694446246c35 | 165 | |
demo71 | 0:694446246c35 | 166 | set_Sp: |
demo71 | 0:694446246c35 | 167 | |
demo71 | 0:694446246c35 | 168 | lcd.cls(); |
demo71 | 0:694446246c35 | 169 | lcd.locate(8,1); |
demo71 | 0:694446246c35 | 170 | lcd.printf(">Sp= "); |
demo71 | 0:694446246c35 | 171 | |
demo71 | 0:694446246c35 | 172 | lcd.locate(1,0); |
demo71 | 0:694446246c35 | 173 | lcd.printf("Kp=%d",Kp); |
demo71 | 0:694446246c35 | 174 | lcd.locate(9,0); |
demo71 | 0:694446246c35 | 175 | lcd.printf("Ki=%d",Ki); |
demo71 | 0:694446246c35 | 176 | lcd.locate(1,1); |
demo71 | 0:694446246c35 | 177 | lcd.printf("Kd=%d",Kd); |
demo71 | 0:694446246c35 | 178 | |
demo71 | 0:694446246c35 | 179 | //Sp=0; |
demo71 | 0:694446246c35 | 180 | while(1){ |
demo71 | 0:694446246c35 | 181 | |
demo71 | 1:f0fa90a6fb6e | 182 | Sp=Sp+0.1*wheel.getPulses(); |
demo71 | 1:f0fa90a6fb6e | 183 | wheel.reset(); |
demo71 | 0:694446246c35 | 184 | |
demo71 | 1:f0fa90a6fb6e | 185 | if(Sp>=3.3){ |
demo71 | 1:f0fa90a6fb6e | 186 | Sp=3.3; |
demo71 | 0:694446246c35 | 187 | } |
demo71 | 0:694446246c35 | 188 | else if (Sp<=0){ |
demo71 | 0:694446246c35 | 189 | Sp=0; |
demo71 | 0:694446246c35 | 190 | } |
demo71 | 0:694446246c35 | 191 | |
demo71 | 0:694446246c35 | 192 | lcd.locate(8,1); |
demo71 | 1:f0fa90a6fb6e | 193 | lcd.printf(">Sp=%.1f ",Sp); |
demo71 | 0:694446246c35 | 194 | |
demo71 | 1:f0fa90a6fb6e | 195 | if(Boton2.falling()){ |
demo71 | 0:694446246c35 | 196 | goto set_Kp; |
demo71 | 0:694446246c35 | 197 | } |
demo71 | 0:694446246c35 | 198 | |
demo71 | 1:f0fa90a6fb6e | 199 | if(Boton1.falling()){ |
demo71 | 0:694446246c35 | 200 | goto PID; |
demo71 | 0:694446246c35 | 201 | } |
demo71 | 0:694446246c35 | 202 | |
demo71 | 0:694446246c35 | 203 | } |
demo71 | 0:694446246c35 | 204 | |
demo71 | 0:694446246c35 | 205 | PID: |
demo71 | 1:f0fa90a6fb6e | 206 | |
demo71 | 0:694446246c35 | 207 | lcd.cls(); |
demo71 | 0:694446246c35 | 208 | while(1){ |
demo71 | 0:694446246c35 | 209 | |
demo71 | 0:694446246c35 | 210 | yT=vcond.read()*3.3; |
demo71 | 1:f0fa90a6fb6e | 211 | ek=Sp-yT; |
demo71 | 1:f0fa90a6fb6e | 212 | |
demo71 | 1:f0fa90a6fb6e | 213 | //Conversion de parametros PID Continuo a parametros de PID Digital |
demo71 | 1:f0fa90a6fb6e | 214 | //ESTO ES UN PID DIGITAL Y LOS PARAMETROS AGREGADOS SON PARA UN PID CONTINUO!! |
demo71 | 1:f0fa90a6fb6e | 215 | K1=Kp+(Ki/2)*Ts+(Kd/Ts); |
demo71 | 1:f0fa90a6fb6e | 216 | K2=-Kp+(Ki/2)*Ts-(2*Kd/Ts); |
demo71 | 1:f0fa90a6fb6e | 217 | K3=Kd/Ts; |
demo71 | 1:f0fa90a6fb6e | 218 | |
demo71 | 1:f0fa90a6fb6e | 219 | uk=ukm1+K1*ek+K2*ekm1+K3*ekm2; //Accion de control |
demo71 | 1:f0fa90a6fb6e | 220 | |
demo71 | 1:f0fa90a6fb6e | 221 | if (uk>3.3) { //Salida PID si es mayor que el MAX |
demo71 | 1:f0fa90a6fb6e | 222 | uk=3.3;} |
demo71 | 1:f0fa90a6fb6e | 223 | else if (uk<0){ //Salida PID si es menor que el MIN |
demo71 | 1:f0fa90a6fb6e | 224 | uk=0; |
demo71 | 1:f0fa90a6fb6e | 225 | } |
demo71 | 1:f0fa90a6fb6e | 226 | |
demo71 | 1:f0fa90a6fb6e | 227 | control=(float)uk/3.3; //Accion de control mapeada a PWM; salida de la tarjeta. |
demo71 | 0:694446246c35 | 228 | |
demo71 | 1:f0fa90a6fb6e | 229 | ekm2=ekm1; |
demo71 | 1:f0fa90a6fb6e | 230 | ekm1=ek; |
demo71 | 1:f0fa90a6fb6e | 231 | ukm1=uk; |
demo71 | 0:694446246c35 | 232 | lcd.locate(0,0); |
demo71 | 1:f0fa90a6fb6e | 233 | lcd.printf("Error=%.2f ",ek); |
demo71 | 0:694446246c35 | 234 | lcd.locate(0,1); |
demo71 | 0:694446246c35 | 235 | lcd.printf("Y_act=%.2f",yT); |
demo71 | 1:f0fa90a6fb6e | 236 | wait(Ts); // Muestreos |
demo71 | 0:694446246c35 | 237 | } |
demo71 | 1:f0fa90a6fb6e | 238 | |
demo71 | 0:694446246c35 | 239 | } |
demo71 | 0:694446246c35 | 240 | |
demo71 | 0:694446246c35 | 241 |