PID DISCRETO
Dependencies: mbed QEI TextLCD DebouncedIn
main.cpp@2:921bc4072c83, 2019-06-07 (annotated)
- Committer:
- demo71
- Date:
- Fri Jun 07 18:23:59 2019 +0000
- Revision:
- 2:921bc4072c83
- Parent:
- 1:f0fa90a6fb6e
PID DISCRETO;
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 | 2:921bc4072c83 | 22 | //Variables y parametros del control PID |
demo71 | 1:f0fa90a6fb6e | 23 | int Kp=0,Ki=0,Kd=0; |
demo71 | 1:f0fa90a6fb6e | 24 | float Sp=0; |
demo71 | 1:f0fa90a6fb6e | 25 | float K1,K2,K3,ek=0,ekm1=0,ekm2=0,Ts=0.1; //Ts Tiempo Muestreo |
demo71 | 1:f0fa90a6fb6e | 26 | float yT,uk=0,ukm1=0; |
demo71 | 1:f0fa90a6fb6e | 27 | |
demo71 | 1:f0fa90a6fb6e | 28 | |
demo71 | 0:694446246c35 | 29 | |
demo71 | 0:694446246c35 | 30 | int main() { |
demo71 | 0:694446246c35 | 31 | |
demo71 | 0:694446246c35 | 32 | lcd.cls(); |
demo71 | 0:694446246c35 | 33 | lcd.locate(1,0); |
demo71 | 0:694446246c35 | 34 | lcd.printf("Kp=%d",Kp); |
demo71 | 0:694446246c35 | 35 | lcd.locate(9,0); |
demo71 | 0:694446246c35 | 36 | lcd.printf("Ki%d=",Ki); |
demo71 | 0:694446246c35 | 37 | lcd.locate(1,1); |
demo71 | 0:694446246c35 | 38 | lcd.printf("Kd=%d",Kd); |
demo71 | 0:694446246c35 | 39 | lcd.locate(9,1); |
demo71 | 1:f0fa90a6fb6e | 40 | lcd.printf("Sp=%.1f",Sp); |
demo71 | 0:694446246c35 | 41 | |
demo71 | 2:921bc4072c83 | 42 | //Configuracion de constante Kp |
demo71 | 0:694446246c35 | 43 | set_Kp: |
demo71 | 0:694446246c35 | 44 | lcd.cls(); |
demo71 | 0:694446246c35 | 45 | lcd.locate(0,0); |
demo71 | 0:694446246c35 | 46 | lcd.printf(">Kp= "); |
demo71 | 0:694446246c35 | 47 | |
demo71 | 0:694446246c35 | 48 | lcd.locate(9,0); |
demo71 | 0:694446246c35 | 49 | lcd.printf("Ki=%d",Ki); |
demo71 | 0:694446246c35 | 50 | lcd.locate(1,1); |
demo71 | 0:694446246c35 | 51 | lcd.printf("Kd=%d",Kd); |
demo71 | 0:694446246c35 | 52 | lcd.locate(9,1); |
demo71 | 1:f0fa90a6fb6e | 53 | lcd.printf("Sp=%.1f",Sp); |
demo71 | 0:694446246c35 | 54 | |
demo71 | 0:694446246c35 | 55 | |
demo71 | 0:694446246c35 | 56 | while(1){ |
demo71 | 0:694446246c35 | 57 | |
demo71 | 1:f0fa90a6fb6e | 58 | Kp=Kp+wheel.getPulses(); |
demo71 | 1:f0fa90a6fb6e | 59 | wheel.reset(); |
demo71 | 1:f0fa90a6fb6e | 60 | |
demo71 | 0:694446246c35 | 61 | if(Kp>=999){ |
demo71 | 0:694446246c35 | 62 | Kp=999; |
demo71 | 0:694446246c35 | 63 | } |
demo71 | 0:694446246c35 | 64 | else if (Kp<=0){ |
demo71 | 0:694446246c35 | 65 | Kp=0; |
demo71 | 0:694446246c35 | 66 | } |
demo71 | 0:694446246c35 | 67 | |
demo71 | 0:694446246c35 | 68 | lcd.locate(0,0); |
demo71 | 0:694446246c35 | 69 | lcd.printf(">Kp=%d ",Kp); |
demo71 | 0:694446246c35 | 70 | |
demo71 | 1:f0fa90a6fb6e | 71 | if(Boton2.falling()){ |
demo71 | 0:694446246c35 | 72 | goto set_Ki; |
demo71 | 0:694446246c35 | 73 | } |
demo71 | 0:694446246c35 | 74 | |
demo71 | 1:f0fa90a6fb6e | 75 | |
demo71 | 1:f0fa90a6fb6e | 76 | if(Boton1.falling()){ |
demo71 | 0:694446246c35 | 77 | goto PID; |
demo71 | 0:694446246c35 | 78 | } |
demo71 | 0:694446246c35 | 79 | |
demo71 | 0:694446246c35 | 80 | } |
demo71 | 0:694446246c35 | 81 | |
demo71 | 2:921bc4072c83 | 82 | //Configuracion de Ki |
demo71 | 0:694446246c35 | 83 | set_Ki: |
demo71 | 0:694446246c35 | 84 | |
demo71 | 0:694446246c35 | 85 | lcd.cls(); |
demo71 | 0:694446246c35 | 86 | lcd.locate(8,0); |
demo71 | 0:694446246c35 | 87 | lcd.printf(">Ki= "); |
demo71 | 0:694446246c35 | 88 | |
demo71 | 0:694446246c35 | 89 | lcd.locate(1,0); |
demo71 | 0:694446246c35 | 90 | lcd.printf("Kp=%d",Kp); |
demo71 | 0:694446246c35 | 91 | lcd.locate(1,1); |
demo71 | 0:694446246c35 | 92 | lcd.printf("Kd=%d",Kd); |
demo71 | 0:694446246c35 | 93 | lcd.locate(9,1); |
demo71 | 1:f0fa90a6fb6e | 94 | lcd.printf("Sp=%.1f",Sp); |
demo71 | 0:694446246c35 | 95 | |
demo71 | 0:694446246c35 | 96 | while(1){ |
demo71 | 0:694446246c35 | 97 | |
demo71 | 1:f0fa90a6fb6e | 98 | Ki=Ki+wheel.getPulses(); |
demo71 | 1:f0fa90a6fb6e | 99 | wheel.reset(); |
demo71 | 0:694446246c35 | 100 | |
demo71 | 0:694446246c35 | 101 | if(Ki>=999){ |
demo71 | 0:694446246c35 | 102 | Ki=999; |
demo71 | 0:694446246c35 | 103 | } |
demo71 | 0:694446246c35 | 104 | else if (Ki<=0){ |
demo71 | 0:694446246c35 | 105 | Ki=0; |
demo71 | 0:694446246c35 | 106 | } |
demo71 | 0:694446246c35 | 107 | |
demo71 | 0:694446246c35 | 108 | lcd.locate(8,0); |
demo71 | 0:694446246c35 | 109 | lcd.printf(">Ki=%d ",Ki); |
demo71 | 0:694446246c35 | 110 | |
demo71 | 1:f0fa90a6fb6e | 111 | if(Boton2.falling()){ |
demo71 | 0:694446246c35 | 112 | goto set_Kd; |
demo71 | 0:694446246c35 | 113 | } |
demo71 | 0:694446246c35 | 114 | |
demo71 | 1:f0fa90a6fb6e | 115 | if(Boton1.falling()){ |
demo71 | 0:694446246c35 | 116 | goto PID; |
demo71 | 0:694446246c35 | 117 | } |
demo71 | 0:694446246c35 | 118 | |
demo71 | 0:694446246c35 | 119 | } |
demo71 | 0:694446246c35 | 120 | |
demo71 | 2:921bc4072c83 | 121 | //Configuracion de Kd |
demo71 | 0:694446246c35 | 122 | set_Kd: |
demo71 | 0:694446246c35 | 123 | |
demo71 | 0:694446246c35 | 124 | lcd.cls(); |
demo71 | 0:694446246c35 | 125 | lcd.locate(0,1); |
demo71 | 0:694446246c35 | 126 | lcd.printf(">Kd= "); |
demo71 | 0:694446246c35 | 127 | |
demo71 | 0:694446246c35 | 128 | lcd.locate(1,0); |
demo71 | 0:694446246c35 | 129 | lcd.printf("Kp=%d",Kp); |
demo71 | 0:694446246c35 | 130 | lcd.locate(9,0); |
demo71 | 0:694446246c35 | 131 | lcd.printf("Ki=%d",Ki); |
demo71 | 0:694446246c35 | 132 | lcd.locate(9,1); |
demo71 | 1:f0fa90a6fb6e | 133 | lcd.printf("Sp=%.1f",Sp); |
demo71 | 0:694446246c35 | 134 | |
demo71 | 0:694446246c35 | 135 | |
demo71 | 2:921bc4072c83 | 136 | |
demo71 | 0:694446246c35 | 137 | while(1){ |
demo71 | 0:694446246c35 | 138 | |
demo71 | 1:f0fa90a6fb6e | 139 | Kd=Kd+wheel.getPulses(); |
demo71 | 1:f0fa90a6fb6e | 140 | wheel.reset(); |
demo71 | 0:694446246c35 | 141 | |
demo71 | 0:694446246c35 | 142 | if(Kd>=999){ |
demo71 | 0:694446246c35 | 143 | Kd=999; |
demo71 | 0:694446246c35 | 144 | } |
demo71 | 0:694446246c35 | 145 | else if (Kd<=0){ |
demo71 | 0:694446246c35 | 146 | Kd=0; |
demo71 | 0:694446246c35 | 147 | } |
demo71 | 0:694446246c35 | 148 | |
demo71 | 0:694446246c35 | 149 | lcd.locate(0,1); |
demo71 | 0:694446246c35 | 150 | lcd.printf(">Kd=%d ",Kd); |
demo71 | 0:694446246c35 | 151 | |
demo71 | 1:f0fa90a6fb6e | 152 | if(Boton2.falling()){ |
demo71 | 0:694446246c35 | 153 | goto set_Sp; |
demo71 | 0:694446246c35 | 154 | } |
demo71 | 0:694446246c35 | 155 | |
demo71 | 1:f0fa90a6fb6e | 156 | if(Boton1.falling()){ |
demo71 | 0:694446246c35 | 157 | goto PID; |
demo71 | 0:694446246c35 | 158 | } |
demo71 | 0:694446246c35 | 159 | |
demo71 | 0:694446246c35 | 160 | } |
demo71 | 0:694446246c35 | 161 | |
demo71 | 2:921bc4072c83 | 162 | //Configuracion de Sp |
demo71 | 0:694446246c35 | 163 | set_Sp: |
demo71 | 0:694446246c35 | 164 | |
demo71 | 0:694446246c35 | 165 | lcd.cls(); |
demo71 | 0:694446246c35 | 166 | lcd.locate(8,1); |
demo71 | 0:694446246c35 | 167 | lcd.printf(">Sp= "); |
demo71 | 0:694446246c35 | 168 | |
demo71 | 0:694446246c35 | 169 | lcd.locate(1,0); |
demo71 | 0:694446246c35 | 170 | lcd.printf("Kp=%d",Kp); |
demo71 | 0:694446246c35 | 171 | lcd.locate(9,0); |
demo71 | 0:694446246c35 | 172 | lcd.printf("Ki=%d",Ki); |
demo71 | 0:694446246c35 | 173 | lcd.locate(1,1); |
demo71 | 0:694446246c35 | 174 | lcd.printf("Kd=%d",Kd); |
demo71 | 0:694446246c35 | 175 | |
demo71 | 2:921bc4072c83 | 176 | |
demo71 | 0:694446246c35 | 177 | while(1){ |
demo71 | 0:694446246c35 | 178 | |
demo71 | 1:f0fa90a6fb6e | 179 | Sp=Sp+0.1*wheel.getPulses(); |
demo71 | 1:f0fa90a6fb6e | 180 | wheel.reset(); |
demo71 | 0:694446246c35 | 181 | |
demo71 | 1:f0fa90a6fb6e | 182 | if(Sp>=3.3){ |
demo71 | 1:f0fa90a6fb6e | 183 | Sp=3.3; |
demo71 | 0:694446246c35 | 184 | } |
demo71 | 0:694446246c35 | 185 | else if (Sp<=0){ |
demo71 | 0:694446246c35 | 186 | Sp=0; |
demo71 | 0:694446246c35 | 187 | } |
demo71 | 0:694446246c35 | 188 | |
demo71 | 0:694446246c35 | 189 | lcd.locate(8,1); |
demo71 | 1:f0fa90a6fb6e | 190 | lcd.printf(">Sp=%.1f ",Sp); |
demo71 | 0:694446246c35 | 191 | |
demo71 | 1:f0fa90a6fb6e | 192 | if(Boton2.falling()){ |
demo71 | 0:694446246c35 | 193 | goto set_Kp; |
demo71 | 0:694446246c35 | 194 | } |
demo71 | 0:694446246c35 | 195 | |
demo71 | 1:f0fa90a6fb6e | 196 | if(Boton1.falling()){ |
demo71 | 0:694446246c35 | 197 | goto PID; |
demo71 | 0:694446246c35 | 198 | } |
demo71 | 0:694446246c35 | 199 | |
demo71 | 0:694446246c35 | 200 | } |
demo71 | 0:694446246c35 | 201 | |
demo71 | 2:921bc4072c83 | 202 | //Inicia algoritmo del PID |
demo71 | 0:694446246c35 | 203 | PID: |
demo71 | 1:f0fa90a6fb6e | 204 | |
demo71 | 0:694446246c35 | 205 | lcd.cls(); |
demo71 | 0:694446246c35 | 206 | while(1){ |
demo71 | 0:694446246c35 | 207 | |
demo71 | 0:694446246c35 | 208 | yT=vcond.read()*3.3; |
demo71 | 1:f0fa90a6fb6e | 209 | ek=Sp-yT; |
demo71 | 1:f0fa90a6fb6e | 210 | |
demo71 | 1:f0fa90a6fb6e | 211 | //Conversion de parametros PID Continuo a parametros de PID Digital |
demo71 | 1:f0fa90a6fb6e | 212 | //ESTO ES UN PID DIGITAL Y LOS PARAMETROS AGREGADOS SON PARA UN PID CONTINUO!! |
demo71 | 2:921bc4072c83 | 213 | |
demo71 | 1:f0fa90a6fb6e | 214 | K1=Kp+(Ki/2)*Ts+(Kd/Ts); |
demo71 | 1:f0fa90a6fb6e | 215 | K2=-Kp+(Ki/2)*Ts-(2*Kd/Ts); |
demo71 | 1:f0fa90a6fb6e | 216 | K3=Kd/Ts; |
demo71 | 1:f0fa90a6fb6e | 217 | |
demo71 | 1:f0fa90a6fb6e | 218 | uk=ukm1+K1*ek+K2*ekm1+K3*ekm2; //Accion de control |
demo71 | 1:f0fa90a6fb6e | 219 | |
demo71 | 1:f0fa90a6fb6e | 220 | if (uk>3.3) { //Salida PID si es mayor que el MAX |
demo71 | 1:f0fa90a6fb6e | 221 | uk=3.3;} |
demo71 | 1:f0fa90a6fb6e | 222 | else if (uk<0){ //Salida PID si es menor que el MIN |
demo71 | 1:f0fa90a6fb6e | 223 | uk=0; |
demo71 | 1:f0fa90a6fb6e | 224 | } |
demo71 | 1:f0fa90a6fb6e | 225 | |
demo71 | 1:f0fa90a6fb6e | 226 | control=(float)uk/3.3; //Accion de control mapeada a PWM; salida de la tarjeta. |
demo71 | 0:694446246c35 | 227 | |
demo71 | 2:921bc4072c83 | 228 | //Actualizacion de datos para el siguiente paso |
demo71 | 2:921bc4072c83 | 229 | ekm2=ekm1; |
demo71 | 1:f0fa90a6fb6e | 230 | ekm1=ek; |
demo71 | 1:f0fa90a6fb6e | 231 | ukm1=uk; |
demo71 | 2:921bc4072c83 | 232 | |
demo71 | 2:921bc4072c83 | 233 | //Mostrando error y salida actual |
demo71 | 0:694446246c35 | 234 | lcd.locate(0,0); |
demo71 | 1:f0fa90a6fb6e | 235 | lcd.printf("Error=%.2f ",ek); |
demo71 | 0:694446246c35 | 236 | lcd.locate(0,1); |
demo71 | 0:694446246c35 | 237 | lcd.printf("Y_act=%.2f",yT); |
demo71 | 2:921bc4072c83 | 238 | wait(Ts); //Tiempo de muestreo |
demo71 | 0:694446246c35 | 239 | } |
demo71 | 1:f0fa90a6fb6e | 240 | |
demo71 | 0:694446246c35 | 241 | } |
demo71 | 0:694446246c35 | 242 | |
demo71 | 0:694446246c35 | 243 |