Julian Palacio / Mbed 2 deprecated PID2

PID

Dependencies:   Debounced FPointer QEI TextLCD keypad mbed

main.cpp@0:5f9a6e0aa3b0, 2014-06-13 (annotated)

Committer:
Jepalacioh
Date:
Fri Jun 13 18:52:55 2014 +0000
Revision:
0:5f9a6e0aa3b0
PID

Who changed what in which revision?

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