demo san / Mbed 2 deprecated TAREA_4_PID_MEJORADO_ENCODER

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?

UserRevisionLine numberNew 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