Sebastian Zapata / Mbed 2 deprecated Tarea3_PID_encoder

controlador PID con encoder

Dependencies:   DebouncedIn QEI TextLCD mbed

main.cpp@0:43669293f24e, 2015-11-13 (annotated)

Committer:
szapataa
Date:
Fri Nov 13 14:10:06 2015 +0000
Revision:
0:43669293f24e
PID con encoder

Who changed what in which revision?

UserRevisionLine numberNew contents of line
szapataa 0:43669293f24e 1 // PID con encoder y boton
szapataa 0:43669293f24e 2
szapataa 0:43669293f24e 3 #include "mbed.h"
szapataa 0:43669293f24e 4 #include "stdio.h"
szapataa 0:43669293f24e 5 #include "TextLCD.h"
szapataa 0:43669293f24e 6 #include "DebouncedIn.h"
szapataa 0:43669293f24e 7 #include "QEI.h"
szapataa 0:43669293f24e 8
szapataa 0:43669293f24e 9 //Declaracion de entradas y salidas
szapataa 0:43669293f24e 10 DebouncedIn Boton1(PTA1); //Boton para confirmar
szapataa 0:43669293f24e 11 DebouncedIn Boton2(PTC9); //Boton para pasar
szapataa 0:43669293f24e 12
szapataa 0:43669293f24e 13 //Configuracion encoder
szapataa 0:43669293f24e 14 QEI wheel (PTD5, PTD0, NC, 100);
szapataa 0:43669293f24e 15
szapataa 0:43669293f24e 16
szapataa 0:43669293f24e 17 PwmOut control(PTE29);
szapataa 0:43669293f24e 18 AnalogIn vsal(PTB0);
szapataa 0:43669293f24e 19
szapataa 0:43669293f24e 20 TextLCD lcd(PTB8,PTB9,PTB10,PTB11,PTE2,PTE3); // rs, e, d4-d7
szapataa 0:43669293f24e 21
szapataa 0:43669293f24e 22 //codigos movimiento del cursor LCD
szapataa 0:43669293f24e 23 int C2=0x18; // desplaza izquierda
szapataa 0:43669293f24e 24 int C3=0x1A; // desplaza derecha
szapataa 0:43669293f24e 25 int C4=0x0C; // quito cursor bajo
szapataa 0:43669293f24e 26 int C1=0x0F; // solo muestra el cursor
szapataa 0:43669293f24e 27
szapataa 0:43669293f24e 28 int Kp=0,Ki=0,Kd=0,Sp=0,p,i;
szapataa 0:43669293f24e 29 float rT,eT,iT,dT,yT,uT,iT0=0,eT0=0,iT_1=0,eT_1=0,spf=0;
szapataa 0:43669293f24e 30 Timer tu;
szapataa 0:43669293f24e 31 Timer td;
szapataa 0:43669293f24e 32
szapataa 0:43669293f24e 33 int main() {
szapataa 0:43669293f24e 34
szapataa 0:43669293f24e 35 lcd.cls();
szapataa 0:43669293f24e 36 lcd.locate(0,0);
szapataa 0:43669293f24e 37 lcd.printf("PID_ENCODER");
szapataa 0:43669293f24e 38 wait(1);
szapataa 0:43669293f24e 39 lcd.cls();
szapataa 0:43669293f24e 40 lcd.locate(0,0);
szapataa 0:43669293f24e 41 lcd.printf("Omar Torres");
szapataa 0:43669293f24e 42 lcd.locate(0,1);
szapataa 0:43669293f24e 43 lcd.printf("Sebastian Zapata");
szapataa 0:43669293f24e 44 wait(2);
szapataa 0:43669293f24e 45 lcd.cls();
szapataa 0:43669293f24e 46
szapataa 0:43669293f24e 47 //comando basado en el manual del LCD
szapataa 0:43669293f24e 48 lcd.locate(1,0);
szapataa 0:43669293f24e 49 lcd.printf("Kp=%d",Kp);
szapataa 0:43669293f24e 50 lcd.locate(9,0);
szapataa 0:43669293f24e 51 lcd.printf("Ki%d=",Ki);
szapataa 0:43669293f24e 52 lcd.locate(1,1);
szapataa 0:43669293f24e 53 lcd.printf("Kd=%d",Kd);
szapataa 0:43669293f24e 54 lcd.locate(9,1);
szapataa 0:43669293f24e 55 lcd.printf("Sp=%d",Sp);
szapataa 0:43669293f24e 56
szapataa 0:43669293f24e 57 set_Kp:
szapataa 0:43669293f24e 58 lcd.cls();
szapataa 0:43669293f24e 59 lcd.locate(0,0);
szapataa 0:43669293f24e 60 lcd.printf("|Kp= ");
szapataa 0:43669293f24e 61
szapataa 0:43669293f24e 62 lcd.locate(9,0);
szapataa 0:43669293f24e 63 lcd.printf("Ki=%d",Ki);
szapataa 0:43669293f24e 64 lcd.locate(1,1);
szapataa 0:43669293f24e 65 lcd.printf("Kd=%d",Kd);
szapataa 0:43669293f24e 66 lcd.locate(9,1);
szapataa 0:43669293f24e 67 lcd.printf("Sp=%d",Sp);
szapataa 0:43669293f24e 68
szapataa 0:43669293f24e 69 while(1){
szapataa 0:43669293f24e 70
szapataa 0:43669293f24e 71 Kp=Kp+wheel.getPulses();
szapataa 0:43669293f24e 72 wheel.reset();
szapataa 0:43669293f24e 73
szapataa 0:43669293f24e 74 if(Kp>=999){
szapataa 0:43669293f24e 75 Kp=999;
szapataa 0:43669293f24e 76 }
szapataa 0:43669293f24e 77 else if (Kp<=0){
szapataa 0:43669293f24e 78 Kp=0;
szapataa 0:43669293f24e 79 }
szapataa 0:43669293f24e 80
szapataa 0:43669293f24e 81 lcd.locate(0,0);
szapataa 0:43669293f24e 82 lcd.printf("|Kp=%d ",Kp);
szapataa 0:43669293f24e 83
szapataa 0:43669293f24e 84 if(Boton2.falling()){
szapataa 0:43669293f24e 85 goto set_Ki;
szapataa 0:43669293f24e 86 }
szapataa 0:43669293f24e 87
szapataa 0:43669293f24e 88 if(Boton1.falling()){
szapataa 0:43669293f24e 89 goto PID;
szapataa 0:43669293f24e 90 }
szapataa 0:43669293f24e 91 }
szapataa 0:43669293f24e 92
szapataa 0:43669293f24e 93 set_Ki:
szapataa 0:43669293f24e 94
szapataa 0:43669293f24e 95 lcd.cls();
szapataa 0:43669293f24e 96 lcd.locate(8,0);
szapataa 0:43669293f24e 97 lcd.printf("|Ki= ");
szapataa 0:43669293f24e 98
szapataa 0:43669293f24e 99 lcd.locate(1,0);
szapataa 0:43669293f24e 100 lcd.printf("Kp=%d",Kp);
szapataa 0:43669293f24e 101 lcd.locate(1,1);
szapataa 0:43669293f24e 102 lcd.printf("Kd=%d",Kd);
szapataa 0:43669293f24e 103 lcd.locate(9,1);
szapataa 0:43669293f24e 104 lcd.printf("Sp=%d",Sp);
szapataa 0:43669293f24e 105
szapataa 0:43669293f24e 106 //Ki=0;
szapataa 0:43669293f24e 107 while(1){
szapataa 0:43669293f24e 108
szapataa 0:43669293f24e 109 Ki=Ki+wheel.getPulses();
szapataa 0:43669293f24e 110 wheel.reset();
szapataa 0:43669293f24e 111
szapataa 0:43669293f24e 112 if(Ki>=999){
szapataa 0:43669293f24e 113 Ki=999;
szapataa 0:43669293f24e 114 }
szapataa 0:43669293f24e 115 else if (Ki<=0){
szapataa 0:43669293f24e 116 Ki=0;
szapataa 0:43669293f24e 117 }
szapataa 0:43669293f24e 118
szapataa 0:43669293f24e 119 lcd.locate(8,0);
szapataa 0:43669293f24e 120 lcd.printf("|Ki=%d ",Ki);
szapataa 0:43669293f24e 121
szapataa 0:43669293f24e 122 if(Boton2.falling()){
szapataa 0:43669293f24e 123 goto set_Kd;
szapataa 0:43669293f24e 124 }
szapataa 0:43669293f24e 125
szapataa 0:43669293f24e 126 if(Boton1.falling()){
szapataa 0:43669293f24e 127 goto PID;
szapataa 0:43669293f24e 128 }
szapataa 0:43669293f24e 129 }
szapataa 0:43669293f24e 130
szapataa 0:43669293f24e 131 set_Kd:
szapataa 0:43669293f24e 132
szapataa 0:43669293f24e 133 lcd.cls();
szapataa 0:43669293f24e 134 lcd.locate(0,1);
szapataa 0:43669293f24e 135 lcd.printf("|Kd= ");
szapataa 0:43669293f24e 136
szapataa 0:43669293f24e 137 lcd.locate(1,0);
szapataa 0:43669293f24e 138 lcd.printf("Kp=%d",Kp);
szapataa 0:43669293f24e 139 lcd.locate(9,0);
szapataa 0:43669293f24e 140 lcd.printf("Ki=%d",Ki);
szapataa 0:43669293f24e 141 lcd.locate(9,1);
szapataa 0:43669293f24e 142 lcd.printf("Sp=%d",Sp);
szapataa 0:43669293f24e 143
szapataa 0:43669293f24e 144 //Kd=0;
szapataa 0:43669293f24e 145 while(1){
szapataa 0:43669293f24e 146
szapataa 0:43669293f24e 147 Kd=Kd+wheel.getPulses();
szapataa 0:43669293f24e 148 wheel.reset();
szapataa 0:43669293f24e 149
szapataa 0:43669293f24e 150 if(Kd>=999){
szapataa 0:43669293f24e 151 Kd=999;
szapataa 0:43669293f24e 152 }
szapataa 0:43669293f24e 153 else if (Kd<=0){
szapataa 0:43669293f24e 154 Kd=0;
szapataa 0:43669293f24e 155 }
szapataa 0:43669293f24e 156
szapataa 0:43669293f24e 157 lcd.locate(0,1);
szapataa 0:43669293f24e 158 lcd.printf("|Kd=%d ",Kd);
szapataa 0:43669293f24e 159
szapataa 0:43669293f24e 160 if(Boton2.falling()){
szapataa 0:43669293f24e 161 goto set_Sp;
szapataa 0:43669293f24e 162 }
szapataa 0:43669293f24e 163
szapataa 0:43669293f24e 164 if(Boton1.falling()){
szapataa 0:43669293f24e 165 goto PID;
szapataa 0:43669293f24e 166 }
szapataa 0:43669293f24e 167 }
szapataa 0:43669293f24e 168
szapataa 0:43669293f24e 169 set_Sp:
szapataa 0:43669293f24e 170
szapataa 0:43669293f24e 171 lcd.cls();
szapataa 0:43669293f24e 172 lcd.locate(8,1);
szapataa 0:43669293f24e 173 lcd.printf("|Sp= ");
szapataa 0:43669293f24e 174
szapataa 0:43669293f24e 175 lcd.locate(1,0);
szapataa 0:43669293f24e 176 lcd.printf("Kp=%d",Kp);
szapataa 0:43669293f24e 177 lcd.locate(9,0);
szapataa 0:43669293f24e 178 lcd.printf("Ki=%d",Ki);
szapataa 0:43669293f24e 179 lcd.locate(1,1);
szapataa 0:43669293f24e 180 lcd.printf("Kd=%d",Kd);
szapataa 0:43669293f24e 181
szapataa 0:43669293f24e 182 //Sp=0;
szapataa 0:43669293f24e 183 while(1){
szapataa 0:43669293f24e 184
szapataa 0:43669293f24e 185 Sp=Sp+wheel.getPulses();
szapataa 0:43669293f24e 186 wheel.reset();
szapataa 0:43669293f24e 187
szapataa 0:43669293f24e 188 if(Sp>=330){
szapataa 0:43669293f24e 189 Sp=330;
szapataa 0:43669293f24e 190 }
szapataa 0:43669293f24e 191 else if (Sp<=0){
szapataa 0:43669293f24e 192 Sp=0;
szapataa 0:43669293f24e 193 }
szapataa 0:43669293f24e 194
szapataa 0:43669293f24e 195 lcd.locate(8,1);
szapataa 0:43669293f24e 196 lcd.printf("|Sp=%d ",Sp);
szapataa 0:43669293f24e 197
szapataa 0:43669293f24e 198 if(Boton2.falling()){
szapataa 0:43669293f24e 199 goto set_Kp;
szapataa 0:43669293f24e 200 }
szapataa 0:43669293f24e 201
szapataa 0:43669293f24e 202 if(Boton1.falling()){
szapataa 0:43669293f24e 203 goto PID;
szapataa 0:43669293f24e 204 }
szapataa 0:43669293f24e 205 }
szapataa 0:43669293f24e 206
szapataa 0:43669293f24e 207 PID:
szapataa 0:43669293f24e 208 lcd.cls();
szapataa 0:43669293f24e 209
szapataa 0:43669293f24e 210 lcd.locate(0,0);
szapataa 0:43669293f24e 211 lcd.printf("Guardando..");
szapataa 0:43669293f24e 212 wait(0.6);
szapataa 0:43669293f24e 213 lcd.cls();
szapataa 0:43669293f24e 214 lcd.locate(0,0);
szapataa 0:43669293f24e 215 lcd.printf("Guardando....");
szapataa 0:43669293f24e 216 wait(0.6);
szapataa 0:43669293f24e 217 lcd.cls();
szapataa 0:43669293f24e 218 lcd.locate(0,0);
szapataa 0:43669293f24e 219 lcd.printf("Guardando......");
szapataa 0:43669293f24e 220 wait(0.6);
szapataa 0:43669293f24e 221 lcd.cls();
szapataa 0:43669293f24e 222
szapataa 0:43669293f24e 223 spf=(float)Sp/100; //convirtiendo de decivoltios a voltios
szapataa 0:43669293f24e 224 while(1){
szapataa 0:43669293f24e 225
szapataa 0:43669293f24e 226 yT=vsal.read()*3.3;
szapataa 0:43669293f24e 227 eT=spf-yT;
szapataa 0:43669293f24e 228 iT=Ki*eT+iT0; //Accion Integral
szapataa 0:43669293f24e 229 dT=Kd*(eT-eT0); //Accion Derivativa
szapataa 0:43669293f24e 230 uT=iT+Kp*eT+dT;
szapataa 0:43669293f24e 231 if (uT>3.3) { //Salida PID si es mayor que el MAX
szapataa 0:43669293f24e 232 uT=3.3;}
szapataa 0:43669293f24e 233 else if (uT<0){ //Salida PID si es menor que el MIN
szapataa 0:43669293f24e 234 uT=0;
szapataa 0:43669293f24e 235 }
szapataa 0:43669293f24e 236 iT0=iT; //Guarda las variables
szapataa 0:43669293f24e 237 eT0=eT;
szapataa 0:43669293f24e 238 control=(float)uT/3.3;
szapataa 0:43669293f24e 239
szapataa 0:43669293f24e 240 lcd.locate(0,0);
szapataa 0:43669293f24e 241 lcd.printf("Err=%.2f ",eT);
szapataa 0:43669293f24e 242 lcd.locate(0,1);
szapataa 0:43669293f24e 243 lcd.printf("Sal=%.2f",yT);
szapataa 0:43669293f24e 244 wait(0.2);
szapataa 0:43669293f24e 245 }
szapataa 0:43669293f24e 246 }
szapataa 0:43669293f24e 247