demo san / Mbed 2 deprecated LUCHO

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?

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