![](/media/cache/img/default_profile.jpg.50x50_q85.jpg)
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?
User | Revision | Line number | New 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 |