Jesse Beltran / Mbed 2 deprecated guardadoantes

PID

Dependencies:   Debounced FPointer QEI TextLCD keypad mbed

main.cpp@1:7394ade42a1d, 2014-06-16 (annotated)

Committer:
Jesse
Date:
Mon Jun 16 04:11:36 2014 +0000
Revision:
1:7394ade42a1d
Parent:
0:413ccc1d18e1 
PID_UltimaTarea

Who changed what in which revision?

UserRevisionLine numberNew contents of line
Jesse 0:413ccc1d18e1 1 #include "mbed.h"
Jesse 0:413ccc1d18e1 2 #include "TextLCD.h"
Jesse 0:413ccc1d18e1 3 #include "QEI.h"
Jesse 0:413ccc1d18e1 4 #include "DebouncedIn.h"
Jesse 0:413ccc1d18e1 5
Jesse 0:413ccc1d18e1 6 PwmOut Pwm1(PTA5);//Definicion de las Salidas.
Jesse 0:413ccc1d18e1 7 PwmOut Pwm2(PTD4);//Definicion de las Salidas.D
Jesse 0:413ccc1d18e1 8 DigitalOut led(LED1);
Jesse 0:413ccc1d18e1 9 DigitalOut led2(LED2);
Jesse 0:413ccc1d18e1 10 DigitalOut led3(LED3);
Jesse 0:413ccc1d18e1 11 DebouncedIn button3(PTC16);
Jesse 0:413ccc1d18e1 12 DebouncedIn button4(PTC17);
Jesse 0:413ccc1d18e1 13
Jesse 0:413ccc1d18e1 14 Serial pc(USBTX, USBRX, "pc");
Jesse 0:413ccc1d18e1 15 QEI wheel (PTD5, PTD0, NC, 24);
Jesse 0:413ccc1d18e1 16 AnalogIn y(PTB1);
Jesse 0:413ccc1d18e1 17 AnalogOut u(PTE30);
Jesse 0:413ccc1d18e1 18 TextLCD lcd(PTB10, PTB11, PTE2, PTE3, PTE4, PTE5); // rs, e, d4-d7
Jesse 0:413ccc1d18e1 19
Jesse 0:413ccc1d18e1 20 int C4=0x0C; // quito cursor bajo
Jesse 0:413ccc1d18e1 21
Jesse 0:413ccc1d18e1 22 float PI,PIant;
Jesse 0:413ccc1d18e1 23 float medd;
Jesse 0:413ccc1d18e1 24 float sal;
Jesse 0:413ccc1d18e1 25 int i; // indice de la variable
Jesse 0:413ccc1d18e1 26 int j; //variable controla cambio 4 posiciones
Jesse 0:413ccc1d18e1 27 float kp=60, ki=0.26, kd=0, Sp,sp,Set=28,T;
Jesse 0:413ccc1d18e1 28 float Delta1=0,Delta2=0,Delta3=0,Delta22,Delta33;
Jesse 0:413ccc1d18e1 29 float yr, ap, ai, ad, err, med, err_v, cycle, pid,pid2;
Jesse 0:413ccc1d18e1 30 int b=0;
Jesse 0:413ccc1d18e1 31 float pidn;
Jesse 0:413ccc1d18e1 32
Jesse 0:413ccc1d18e1 33 float f=100; //Frecuencia del PWM
Jesse 0:413ccc1d18e1 34 float pp=1/f; //Perioo del PWM
Jesse 0:413ccc1d18e1 35 Timer t;
Jesse 0:413ccc1d18e1 36
Jesse 0:413ccc1d18e1 37 int Fs=10;
Jesse 0:413ccc1d18e1 38 float Ts=0.1;
Jesse 0:413ccc1d18e1 39 float inte;
Jesse 0:413ccc1d18e1 40 float intek;
Jesse 0:413ccc1d18e1 41
Jesse 0:413ccc1d18e1 42
Jesse 0:413ccc1d18e1 43 int PWMmodule1(float pp,float Dd)
Jesse 0:413ccc1d18e1 44 {
Jesse 0:413ccc1d18e1 45 Pwm1.period(pp);
Jesse 0:413ccc1d18e1 46 Pwm1.write(Dd);
Jesse 0:413ccc1d18e1 47 wait(0.01);
Jesse 0:413ccc1d18e1 48 }
Jesse 0:413ccc1d18e1 49
Jesse 0:413ccc1d18e1 50 int PWMmodule2(float pp,float Dd)
Jesse 0:413ccc1d18e1 51 {
Jesse 0:413ccc1d18e1 52 Pwm2.period(pp);
Jesse 0:413ccc1d18e1 53 Pwm2.write(Dd);
Jesse 0:413ccc1d18e1 54 wait(0.01);
Jesse 0:413ccc1d18e1 55 }
Jesse 0:413ccc1d18e1 56 int main()
Jesse 0:413ccc1d18e1 57 {
Jesse 0:413ccc1d18e1 58
Jesse 0:413ccc1d18e1 59 led=1;
Jesse 0:413ccc1d18e1 60 led2=0;
Jesse 0:413ccc1d18e1 61 medd=3.705503686*100*y.read();
Jesse 0:413ccc1d18e1 62 sp=medd;
Jesse 0:413ccc1d18e1 63 Sp=sp;
Jesse 0:413ccc1d18e1 64 inte=Set-medd;
Jesse 0:413ccc1d18e1 65 PIant=0;
Jesse 0:413ccc1d18e1 66 lcd.cls();
Jesse 0:413ccc1d18e1 67 //Valores Constantes, que se deben inicializar.
Jesse 0:413ccc1d18e1 68 lcd.locate(0,0);
Jesse 0:413ccc1d18e1 69 lcd.printf("Sp:%.1f",Set);
Jesse 0:413ccc1d18e1 70 lcd.locate(8,0);
Jesse 0:413ccc1d18e1 71 lcd.printf("T1:%.1f",Delta1);
Jesse 0:413ccc1d18e1 72 lcd.locate(0,1);
Jesse 0:413ccc1d18e1 73 lcd.printf("T2:%.1f",Delta2);
Jesse 0:413ccc1d18e1 74 lcd.locate(8,1);
Jesse 0:413ccc1d18e1 75 lcd.printf("T3:%.1f",Delta3);
Jesse 0:413ccc1d18e1 76
Jesse 0:413ccc1d18e1 77 while(1)
Jesse 0:413ccc1d18e1 78 {
Jesse 0:413ccc1d18e1 79 led3 =1;
Jesse 0:413ccc1d18e1 80 if (button3.falling())
Jesse 0:413ccc1d18e1 81 { //INCREMENTA POSICION DEL MENU CON BOTON 3 (Switche encoder)
Jesse 0:413ccc1d18e1 82 led3 =!led3;
Jesse 0:413ccc1d18e1 83 ++j;
Jesse 0:413ccc1d18e1 84 }
Jesse 0:413ccc1d18e1 85
Jesse 0:413ccc1d18e1 86 if(j==0)
Jesse 0:413ccc1d18e1 87 {
Jesse 0:413ccc1d18e1 88 Set=Set+wheel.getPulses();
Jesse 0:413ccc1d18e1 89 wheel.reset();
Jesse 0:413ccc1d18e1 90 if (Set>999)
Jesse 0:413ccc1d18e1 91 {
Jesse 0:413ccc1d18e1 92 Set=999;
Jesse 0:413ccc1d18e1 93 }
Jesse 0:413ccc1d18e1 94 if (Set<0)
Jesse 0:413ccc1d18e1 95 {
Jesse 0:413ccc1d18e1 96 Set=0;
Jesse 0:413ccc1d18e1 97 }
Jesse 0:413ccc1d18e1 98 lcd.locate(3,0);
Jesse 0:413ccc1d18e1 99 lcd.printf(" ",Set);
Jesse 0:413ccc1d18e1 100 lcd.locate(3,0);
Jesse 0:413ccc1d18e1 101 lcd.printf("%.1f",Set);
Jesse 0:413ccc1d18e1 102 wait(0.2);
Jesse 0:413ccc1d18e1 103
Jesse 0:413ccc1d18e1 104 if(button3.falling())
Jesse 0:413ccc1d18e1 105 {
Jesse 0:413ccc1d18e1 106 j=1;
Jesse 0:413ccc1d18e1 107 led3=0;
Jesse 0:413ccc1d18e1 108 wait(0.3);
Jesse 0:413ccc1d18e1 109 wheel.reset();
Jesse 0:413ccc1d18e1 110 }
Jesse 0:413ccc1d18e1 111
Jesse 0:413ccc1d18e1 112 }
Jesse 0:413ccc1d18e1 113 if(j==1)
Jesse 0:413ccc1d18e1 114 {
Jesse 0:413ccc1d18e1 115 Delta1=Delta1+wheel.getPulses();
Jesse 0:413ccc1d18e1 116 wheel.reset();
Jesse 0:413ccc1d18e1 117 if (Delta1>12000)
Jesse 0:413ccc1d18e1 118 {
Jesse 0:413ccc1d18e1 119 Delta1=12000;
Jesse 0:413ccc1d18e1 120 }
Jesse 0:413ccc1d18e1 121 if (Delta1<0)
Jesse 0:413ccc1d18e1 122 {
Jesse 0:413ccc1d18e1 123 Delta1=0;
Jesse 0:413ccc1d18e1 124 }
Jesse 0:413ccc1d18e1 125 lcd.locate(11,0);
Jesse 0:413ccc1d18e1 126 lcd.printf(" ");
Jesse 0:413ccc1d18e1 127 lcd.locate(11,0);
Jesse 0:413ccc1d18e1 128 lcd.printf("%.1f",Delta1);
Jesse 0:413ccc1d18e1 129 wait(0.2);
Jesse 0:413ccc1d18e1 130
Jesse 0:413ccc1d18e1 131 if(button3.falling())
Jesse 0:413ccc1d18e1 132 {
Jesse 0:413ccc1d18e1 133 j=2;
Jesse 0:413ccc1d18e1 134 led3=0;
Jesse 0:413ccc1d18e1 135 wait(0.3);
Jesse 0:413ccc1d18e1 136 wheel.reset();
Jesse 0:413ccc1d18e1 137 }
Jesse 0:413ccc1d18e1 138 }
Jesse 0:413ccc1d18e1 139 if(j==2)
Jesse 0:413ccc1d18e1 140 {
Jesse 0:413ccc1d18e1 141 Delta2=Delta2+wheel.getPulses();
Jesse 0:413ccc1d18e1 142 wheel.reset();
Jesse 0:413ccc1d18e1 143 if (Delta2>12000)
Jesse 0:413ccc1d18e1 144 {
Jesse 0:413ccc1d18e1 145 Delta2=12000;
Jesse 0:413ccc1d18e1 146 }
Jesse 0:413ccc1d18e1 147 if (Delta2<0)
Jesse 0:413ccc1d18e1 148 {
Jesse 0:413ccc1d18e1 149 Delta2=0;
Jesse 0:413ccc1d18e1 150 }
Jesse 0:413ccc1d18e1 151 lcd.locate(3,1);
Jesse 0:413ccc1d18e1 152 lcd.printf(" ");
Jesse 0:413ccc1d18e1 153 lcd.locate(3,1);
Jesse 0:413ccc1d18e1 154 lcd.printf("%.1f",Delta2);
Jesse 0:413ccc1d18e1 155 wait(0.2);
Jesse 0:413ccc1d18e1 156
Jesse 0:413ccc1d18e1 157 if(button3.falling())
Jesse 0:413ccc1d18e1 158 {
Jesse 0:413ccc1d18e1 159 j=3;
Jesse 0:413ccc1d18e1 160 led3=0;
Jesse 0:413ccc1d18e1 161 wait(0.3);
Jesse 0:413ccc1d18e1 162 wheel.reset();
Jesse 0:413ccc1d18e1 163 }
Jesse 0:413ccc1d18e1 164 }
Jesse 0:413ccc1d18e1 165
Jesse 0:413ccc1d18e1 166 if(j==3)
Jesse 0:413ccc1d18e1 167 {
Jesse 0:413ccc1d18e1 168 Delta3=Delta3+wheel.getPulses();
Jesse 0:413ccc1d18e1 169 wheel.reset();
Jesse 0:413ccc1d18e1 170 if (Delta3>12000)
Jesse 0:413ccc1d18e1 171 {
Jesse 0:413ccc1d18e1 172 Delta3=12000;
Jesse 0:413ccc1d18e1 173 }
Jesse 0:413ccc1d18e1 174 if (Delta3<0)
Jesse 0:413ccc1d18e1 175 {
Jesse 0:413ccc1d18e1 176 Delta3=0;
Jesse 0:413ccc1d18e1 177 }
Jesse 0:413ccc1d18e1 178 lcd.locate(11,1);
Jesse 0:413ccc1d18e1 179 lcd.printf(" ");
Jesse 0:413ccc1d18e1 180 lcd.locate(11,1);
Jesse 0:413ccc1d18e1 181 lcd.printf("%.1f",Delta3);
Jesse 0:413ccc1d18e1 182 wait(0.2);
Jesse 0:413ccc1d18e1 183
Jesse 0:413ccc1d18e1 184 if(button3.falling())
Jesse 0:413ccc1d18e1 185 {
Jesse 0:413ccc1d18e1 186 j=0;
Jesse 0:413ccc1d18e1 187 led3=0;
Jesse 0:413ccc1d18e1 188 wait(0.3);
Jesse 0:413ccc1d18e1 189 wheel.reset();
Jesse 0:413ccc1d18e1 190 }
Jesse 0:413ccc1d18e1 191 }
Jesse 0:413ccc1d18e1 192
Jesse 0:413ccc1d18e1 193 if(j==4)
Jesse 0:413ccc1d18e1 194 {
Jesse 0:413ccc1d18e1 195 j=0;
Jesse 0:413ccc1d18e1 196 }
Jesse 0:413ccc1d18e1 197
Jesse 0:413ccc1d18e1 198 if(!button4)
Jesse 0:413ccc1d18e1 199 {
Jesse 0:413ccc1d18e1 200 break;
Jesse 0:413ccc1d18e1 201 }
Jesse 0:413ccc1d18e1 202 }
Jesse 0:413ccc1d18e1 203 //%%Cierro While--------------------------------------------
Jesse 0:413ccc1d18e1 204 lcd.writeCommand(C4);
Jesse 0:413ccc1d18e1 205 lcd.cls(); //borra la pantalla
Jesse 0:413ccc1d18e1 206 lcd.printf(" GUARDAMOS \nVALORES |m|");
Jesse 0:413ccc1d18e1 207 wait(2);
Jesse 0:413ccc1d18e1 208
Jesse 0:413ccc1d18e1 209 Delta22=Delta2+Delta1;
Jesse 0:413ccc1d18e1 210 Delta33=Delta3+Delta2+Delta1;
Jesse 0:413ccc1d18e1 211
Jesse 0:413ccc1d18e1 212 medd=3.4503686*100*y.read();
Jesse 0:413ccc1d18e1 213 lcd.cls();
Jesse 0:413ccc1d18e1 214 lcd.locate(0,0);
Jesse 0:413ccc1d18e1 215 lcd.printf("Sp:%.1f",Sp);
Jesse 0:413ccc1d18e1 216 lcd.locate(0,1);
Jesse 0:413ccc1d18e1 217 lcd.printf("Ts=%.1f",T);
Jesse 0:413ccc1d18e1 218 //lcd.locate(8,0);
Jesse 0:413ccc1d18e1 219 //lcd.printf("Er:%.1f",err);
Jesse 0:413ccc1d18e1 220 lcd.locate(8,0);
Jesse 0:413ccc1d18e1 221 lcd.printf("T%.1f",medd);
Jesse 0:413ccc1d18e1 222 lcd.locate(8,1);
Jesse 0:413ccc1d18e1 223 lcd.printf("u:%.1f",PI);
Jesse 0:413ccc1d18e1 224 wait(2);
Jesse 0:413ccc1d18e1 225
Jesse 0:413ccc1d18e1 226 while(1)
Jesse 0:413ccc1d18e1 227 {
Jesse 0:413ccc1d18e1 228
Jesse 0:413ccc1d18e1 229 medd=3.45503686*100*y.read();
Jesse 0:413ccc1d18e1 230 err = (Sp-medd);
Jesse 0:413ccc1d18e1 231 inte=inte+err*Ts;
Jesse 0:413ccc1d18e1 232 PI=(kp*err+ ki*inte);
Jesse 0:413ccc1d18e1 233 pid=PI/100;
Jesse 0:413ccc1d18e1 234 pid2=1-pid;
Jesse 0:413ccc1d18e1 235 if(PI>100)
Jesse 0:413ccc1d18e1 236 {
Jesse 0:413ccc1d18e1 237 PI=100;
Jesse 0:413ccc1d18e1 238 }
Jesse 0:413ccc1d18e1 239 if(PI<0)
Jesse 0:413ccc1d18e1 240 {
Jesse 0:413ccc1d18e1 241 PI=0;
Jesse 0:413ccc1d18e1 242 }
Jesse 0:413ccc1d18e1 243
Jesse 0:413ccc1d18e1 244
Jesse 0:413ccc1d18e1 245
Jesse 0:413ccc1d18e1 246
Jesse 0:413ccc1d18e1 247
Jesse 0:413ccc1d18e1 248 //Funcion Rampa
Jesse 0:413ccc1d18e1 249 if(T<=Delta1)
Jesse 0:413ccc1d18e1 250 {
Jesse 0:413ccc1d18e1 251 lcd.cls();
Jesse 0:413ccc1d18e1 252 lcd.locate(0,0);
Jesse 0:413ccc1d18e1 253 Sp=Sp+(((Set-sp)/Delta1))*Ts;
Jesse 0:413ccc1d18e1 254 lcd.printf("Sp=%.1f",Sp);
Jesse 0:413ccc1d18e1 255 lcd.locate(0,1);
Jesse 0:413ccc1d18e1 256 lcd.printf("Ts=%.1f",T);
Jesse 0:413ccc1d18e1 257 lcd.locate(8,0);
Jesse 0:413ccc1d18e1 258 lcd.printf("Er:%.1f",medd);
Jesse 0:413ccc1d18e1 259 lcd.locate(8,1);
Jesse 0:413ccc1d18e1 260 lcd.printf("u:%.1f",PI);
Jesse 0:413ccc1d18e1 261 PWMmodule1(pp,pid);
Jesse 0:413ccc1d18e1 262 PWMmodule2(pp,pid2);
Jesse 0:413ccc1d18e1 263
Jesse 0:413ccc1d18e1 264 }
Jesse 0:413ccc1d18e1 265 if((T>Delta1)&&(T<=Delta22))
Jesse 0:413ccc1d18e1 266 {
Jesse 0:413ccc1d18e1 267 Sp=Sp;
Jesse 0:413ccc1d18e1 268 lcd.cls();
Jesse 0:413ccc1d18e1 269 lcd.printf("Sp=%.1f",Sp);
Jesse 0:413ccc1d18e1 270 lcd.locate(0,1);
Jesse 0:413ccc1d18e1 271 lcd.printf("Ts=%.1f",T);
Jesse 0:413ccc1d18e1 272 lcd.locate(8,0);
Jesse 0:413ccc1d18e1 273 lcd.printf("Er:%.1f",medd);
Jesse 0:413ccc1d18e1 274 lcd.locate(8,1);
Jesse 0:413ccc1d18e1 275 lcd.printf("u:%.1f",PI);
Jesse 0:413ccc1d18e1 276 PWMmodule1(pp,pid);
Jesse 0:413ccc1d18e1 277 PWMmodule2(pp,pid2);
Jesse 0:413ccc1d18e1 278 }
Jesse 0:413ccc1d18e1 279 if((T>Delta22)&&(T<=Delta33))
Jesse 0:413ccc1d18e1 280 {
Jesse 0:413ccc1d18e1 281 lcd.cls();
Jesse 0:413ccc1d18e1 282 lcd.printf("Sp=%.1f",Sp);
Jesse 0:413ccc1d18e1 283 lcd.locate(0,1);
Jesse 0:413ccc1d18e1 284 Sp=Sp-(((Set-sp)/Delta3))*Ts;
Jesse 0:413ccc1d18e1 285 lcd.printf("Ts=%.1f",T);
Jesse 0:413ccc1d18e1 286 lcd.locate(8,0);
Jesse 0:413ccc1d18e1 287 lcd.printf("Er:%.1f",medd);
Jesse 0:413ccc1d18e1 288 lcd.locate(8,1);
Jesse 0:413ccc1d18e1 289 lcd.printf("u:%.1f",PI);
Jesse 0:413ccc1d18e1 290 PWMmodule1(pp,pid);
Jesse 0:413ccc1d18e1 291 PWMmodule2(pp,pid2);
Jesse 0:413ccc1d18e1 292
Jesse 0:413ccc1d18e1 293 //pid=1-(PI/100);
Jesse 0:413ccc1d18e1 294 led=1;
Jesse 0:413ccc1d18e1 295 led2=0;
Jesse 1:7394ade42a1d 296
Jesse 0:413ccc1d18e1 297 }
Jesse 0:413ccc1d18e1 298 */
Jesse 0:413ccc1d18e1 299 PIant=PI;
Jesse 0:413ccc1d18e1 300
Jesse 0:413ccc1d18e1 301 lcd.locate(8,0);
Jesse 0:413ccc1d18e1 302 lcd.printf("Er:%.1f",medd);
Jesse 0:413ccc1d18e1 303 lcd.locate(8,1);
Jesse 0:413ccc1d18e1 304 lcd.printf("u:%.1f",PI);
Jesse 0:413ccc1d18e1 305
Jesse 0:413ccc1d18e1 306 T=T+Ts;
Jesse 0:413ccc1d18e1 307 }
Jesse 0:413ccc1d18e1 308
Jesse 0:413ccc1d18e1 309
Jesse 0:413ccc1d18e1 310
Jesse 0:413ccc1d18e1 311 }