isra ceron / Mbed 2 deprecated pid_digital_4

Dependencies:   mbed

main.cpp@0:100b7ad913c1, 2017-03-14 (annotated)

Committer:
icmembed
Date:
Tue Mar 14 21:46:39 2017 +0000
Revision:
0:100b7ad913c1
saludos comparto un control PID digital;

Who changed what in which revision?

UserRevisionLine numberNew contents of line
icmembed 0:100b7ad913c1 1 #include "mbed.h"
icmembed 0:100b7ad913c1 2
icmembed 0:100b7ad913c1 3 //AnalogIn analog_value(A0);
icmembed 0:100b7ad913c1 4 //AnalogIn prop(A1);
icmembed 0:100b7ad913c1 5 //DigitalOut PWM(D3);
icmembed 0:100b7ad913c1 6 //DigitalOut PWM1(D4);
icmembed 0:100b7ad913c1 7 PwmOut mypwm(D5);
icmembed 0:100b7ad913c1 8 PwmOut PWM(D3);
icmembed 0:100b7ad913c1 9 PwmOut PWM1(D4);
icmembed 0:100b7ad913c1 10 Serial pc(SERIAL_TX, SERIAL_RX);
icmembed 0:100b7ad913c1 11
icmembed 0:100b7ad913c1 12 // debemos definir tres entradas analigica para KP, KI, KD
icmembed 0:100b7ad913c1 13 //AnalogIn KP(A0);
icmembed 0:100b7ad913c1 14 //AnalogIn KI(A1);
icmembed 0:100b7ad913c1 15 //AnalogIn KD(A3);
icmembed 0:100b7ad913c1 16 // declaramos una entrada analogica para la referencia
icmembed 0:100b7ad913c1 17 //AnalogIn REF(A4);
icmembed 0:100b7ad913c1 18 // declaramos la salida analogica PID
icmembed 0:100b7ad913c1 19 AnalogOut salida(PA_4); // esta es A2
icmembed 0:100b7ad913c1 20 // declaramos la entrada de retroalimentacion
icmembed 0:100b7ad913c1 21 AnalogIn retro(A5);
icmembed 0:100b7ad913c1 22
icmembed 0:100b7ad913c1 23 // variables
icmembed 0:100b7ad913c1 24 double KP=0.1;
icmembed 0:100b7ad913c1 25 double KI =0.1;
icmembed 0:100b7ad913c1 26 double KD =0.001;
icmembed 0:100b7ad913c1 27 double REF = 0.5;
icmembed 0:100b7ad913c1 28
icmembed 0:100b7ad913c1 29 float Apid=0;
icmembed 0:100b7ad913c1 30
icmembed 0:100b7ad913c1 31 float Vo_1 = 0;
icmembed 0:100b7ad913c1 32 float Vo_2 = 0;
icmembed 0:100b7ad913c1 33 float Vo = 0;
icmembed 0:100b7ad913c1 34 float Vi_2 = 0;
icmembed 0:100b7ad913c1 35 float Vi_1 = 0;
icmembed 0:100b7ad913c1 36 float Vi = 1;
icmembed 0:100b7ad913c1 37 float R = 1;
icmembed 0:100b7ad913c1 38 float C = 1;
icmembed 0:100b7ad913c1 39 float L = 1;
icmembed 0:100b7ad913c1 40 float T = 0.001;
icmembed 0:100b7ad913c1 41 // PID
icmembed 0:100b7ad913c1 42 double IT_1=0;
icmembed 0:100b7ad913c1 43 double DT_1=0;
icmembed 0:100b7ad913c1 44 double UT_1=0;
icmembed 0:100b7ad913c1 45 double YT_1=0;
icmembed 0:100b7ad913c1 46 double YT;
icmembed 0:100b7ad913c1 47 double UT;
icmembed 0:100b7ad913c1 48 double KP1 =0.2;
icmembed 0:100b7ad913c1 49 double PT;
icmembed 0:100b7ad913c1 50 double IT = 0;
icmembed 0:100b7ad913c1 51 double Ti =0.9; //0.017;
icmembed 0:100b7ad913c1 52 double Td =0.9; //0.017;
icmembed 0:100b7ad913c1 53 double N =30;
icmembed 0:100b7ad913c1 54 double DT =0;
icmembed 0:100b7ad913c1 55 double PID =0;
icmembed 0:100b7ad913c1 56 int k=0;
icmembed 0:100b7ad913c1 57 int analogPin = 0;
icmembed 0:100b7ad913c1 58 volatile float valor=0;
icmembed 0:100b7ad913c1 59 int ledPin = 10;
icmembed 0:100b7ad913c1 60 int voo=0;
icmembed 0:100b7ad913c1 61 //float error =0;
icmembed 0:100b7ad913c1 62 double KPin;
icmembed 0:100b7ad913c1 63 double KIin;
icmembed 0:100b7ad913c1 64 double KDin;
icmembed 0:100b7ad913c1 65 double ERROR1;
icmembed 0:100b7ad913c1 66 double PT2;
icmembed 0:100b7ad913c1 67 double E;
icmembed 0:100b7ad913c1 68
icmembed 0:100b7ad913c1 69
icmembed 0:100b7ad913c1 70
icmembed 0:100b7ad913c1 71 int Ient;
icmembed 0:100b7ad913c1 72
icmembed 0:100b7ad913c1 73
icmembed 0:100b7ad913c1 74 DigitalOut led(LED1);
icmembed 0:100b7ad913c1 75
icmembed 0:100b7ad913c1 76 int main() {
icmembed 0:100b7ad913c1 77
icmembed 0:100b7ad913c1 78 PWM.period_ms(17);
icmembed 0:100b7ad913c1 79 PWM1.period_ms(17);
icmembed 0:100b7ad913c1 80 //mypwm.period_ms(10);
icmembed 0:100b7ad913c1 81 //mypwm.pulsewidth_ms(1);
icmembed 0:100b7ad913c1 82 // float meas;
icmembed 0:100b7ad913c1 83
icmembed 0:100b7ad913c1 84 //printf("\nAnalogIn example\n");
icmembed 0:100b7ad913c1 85
icmembed 0:100b7ad913c1 86 pc.printf("This program tiene la referencia %f .\n", REF);
icmembed 0:100b7ad913c1 87
icmembed 0:100b7ad913c1 88
icmembed 0:100b7ad913c1 89
icmembed 0:100b7ad913c1 90
icmembed 0:100b7ad913c1 91
icmembed 0:100b7ad913c1 92 while(1) {
icmembed 0:100b7ad913c1 93 //meas = analog_value.read(); // Converts and read the analog input value (value from 0.0 to 1.0)
icmembed 0:100b7ad913c1 94 //meas = meas * 3300; // Change the value to be in the 0 to 3300 range
icmembed 0:100b7ad913c1 95 //PWM= meas;
icmembed 0:100b7ad913c1 96 //printf("measure = %.0f mV\n", meas);
icmembed 0:100b7ad913c1 97 // if (meas > 2000) { // If the value is greater than 2V then switch the LED on
icmembed 0:100b7ad913c1 98 // led = 1;
icmembed 0:100b7ad913c1 99
icmembed 0:100b7ad913c1 100 //
icmembed 0:100b7ad913c1 101 //KPin = KP.read();
icmembed 0:100b7ad913c1 102 //KPin = KPin*0.9428;
icmembed 0:100b7ad913c1 103 //KPin = KP*0.9;
icmembed 0:100b7ad913c1 104 //KIin = KI.read();
icmembed 0:100b7ad913c1 105 //KIin = KIin*0.9428;
icmembed 0:100b7ad913c1 106 //KIin = KI*0.2;
icmembed 0:100b7ad913c1 107
icmembed 0:100b7ad913c1 108 //KDin = KD.read();
icmembed 0:100b7ad913c1 109 //KDin = KDin*0.9428;
icmembed 0:100b7ad913c1 110 //KDin = KD*0.2;
icmembed 0:100b7ad913c1 111
icmembed 0:100b7ad913c1 112 YT = retro.read();
icmembed 0:100b7ad913c1 113 //YT = YT*1.25;
icmembed 0:100b7ad913c1 114 // UT = REF.read();
icmembed 0:100b7ad913c1 115 //UT = UT;
icmembed 0:100b7ad913c1 116 //
icmembed 0:100b7ad913c1 117 UT = REF;
icmembed 0:100b7ad913c1 118 E = (UT-YT);
icmembed 0:100b7ad913c1 119 PT= KP*(UT-YT);
icmembed 0:100b7ad913c1 120 IT = KI*T*(UT_1-YT_1)+IT_1;
icmembed 0:100b7ad913c1 121 DT= (KD/T)*(UT-YT+YT_1-UT_1);
icmembed 0:100b7ad913c1 122 PID= PT+IT+DT;
icmembed 0:100b7ad913c1 123 YT_1=YT;
icmembed 0:100b7ad913c1 124 UT_1=UT;
icmembed 0:100b7ad913c1 125 IT_1=IT;
icmembed 0:100b7ad913c1 126
icmembed 0:100b7ad913c1 127 // in_value=in_value*10;
icmembed 0:100b7ad913c1 128
icmembed 0:100b7ad913c1 129 // salida = in_value;
icmembed 0:100b7ad913c1 130
icmembed 0:100b7ad913c1 131
icmembed 0:100b7ad913c1 132 Apid = abs(PID);
icmembed 0:100b7ad913c1 133 Ient =(int) Apid;
icmembed 0:100b7ad913c1 134
icmembed 0:100b7ad913c1 135
icmembed 0:100b7ad913c1 136 //PWM = 1;
icmembed 0:100b7ad913c1 137 //PWM1 = 0;
icmembed 0:100b7ad913c1 138 //wait(5);
icmembed 0:100b7ad913c1 139 //}
icmembed 0:100b7ad913c1 140 //else {
icmembed 0:100b7ad913c1 141 //led = 0;
icmembed 0:100b7ad913c1 142 //}
icmembed 0:100b7ad913c1 143 // 200 ms
icmembed 0:100b7ad913c1 144 //PWM =0;
icmembed 0:100b7ad913c1 145 //PWM = 0;
icmembed 0:100b7ad913c1 146 //PWM1 = 1;
icmembed 0:100b7ad913c1 147 //wait(5);
icmembed 0:100b7ad913c1 148 pc.printf("\n\n");
icmembed 0:100b7ad913c1 149 pc.printf("retro %lf.\t", YT);
icmembed 0:100b7ad913c1 150 pc.printf("Error %lf. \n\n", E);
icmembed 0:100b7ad913c1 151 pc.printf("referencia %f.\n", REF);
icmembed 0:100b7ad913c1 152 pc.printf("\r");
icmembed 0:100b7ad913c1 153 //Apid =abs(IT);
icmembed 0:100b7ad913c1 154
icmembed 0:100b7ad913c1 155
icmembed 0:100b7ad913c1 156 // pc.printf("PID %lf. \n \n", PID);
icmembed 0:100b7ad913c1 157
icmembed 0:100b7ad913c1 158 // PT = (UT-YT); // kp es un potenciometro de 0 - 1
icmembed 0:100b7ad913c1 159 // PT2 = KPin*PT;
icmembed 0:100b7ad913c1 160 // IT = KIin*(((T/Ti)*UT_1)-((T/Ti)*YT_1)+IT_1); // ki potenciometro integral de 0 a 1
icmembed 0:100b7ad913c1 161 // DT = KDin*((N*(YT-YT_1))+((1-((N*T)/Td))*DT_1)); // kd ptenciometro derivative de 0 a 1
icmembed 0:100b7ad913c1 162 // PID = PT2+IT+DT;
icmembed 0:100b7ad913c1 163
icmembed 0:100b7ad913c1 164 // IT_1=IT;
icmembed 0:100b7ad913c1 165 // DT_1=DT;
icmembed 0:100b7ad913c1 166 // YT_1=YT;
icmembed 0:100b7ad913c1 167 // Apid = abs(PID);
icmembed 0:100b7ad913c1 168 // pc.printf("PID antes del absoluto %lf. \n \n", PID);
icmembed 0:100b7ad913c1 169 // pc.printf("Absoluto PID %lf. \n \n", Apid);
icmembed 0:100b7ad913c1 170 //salida = (KPin*0.9428); //n aqui es la retroalimentacion
icmembed 0:100b7ad913c1 171 if( E > 0.01 && Apid < 1 ) {
icmembed 0:100b7ad913c1 172 //PWM.pulsewidth_ms(10*PID);
icmembed 0:100b7ad913c1 173 //PWM1=PWM1.pulsewidth_ms(0);
icmembed 0:100b7ad913c1 174
icmembed 0:100b7ad913c1 175 //PWM = 10*PID;
icmembed 0:100b7ad913c1 176
icmembed 0:100b7ad913c1 177 // PWM = abs(PID);
icmembed 0:100b7ad913c1 178 // PMW1 =0;
icmembed 0:100b7ad913c1 179
icmembed 0:100b7ad913c1 180 PWM.write(Apid);
icmembed 0:100b7ad913c1 181 PWM1.write(0.0f);
icmembed 0:100b7ad913c1 182 pc.printf("integrador en ciclo %0.2lf.\n \n", IT);
icmembed 0:100b7ad913c1 183
icmembed 0:100b7ad913c1 184 pc.printf("\r");
icmembed 0:100b7ad913c1 185
icmembed 0:100b7ad913c1 186 //pc.printf("PWM1 %i.\n \n", PWM1);
icmembed 0:100b7ad913c1 187 // pc.printf("PWM %i. \n \n", PWM);
icmembed 0:100b7ad913c1 188
icmembed 0:100b7ad913c1 189 }
icmembed 0:100b7ad913c1 190 else if ( E > 0.01 && Apid >= 1 ) {
icmembed 0:100b7ad913c1 191 //PWM.pulsewidth_ms(10*PID);
icmembed 0:100b7ad913c1 192 //PWM1=PWM1.pulsewidth_ms(0);
icmembed 0:100b7ad913c1 193
icmembed 0:100b7ad913c1 194 //PWM = 10*PID;
icmembed 0:100b7ad913c1 195
icmembed 0:100b7ad913c1 196 // PWM = abs(PID);
icmembed 0:100b7ad913c1 197 // PMW1 =0;
icmembed 0:100b7ad913c1 198 IT_1=2;
icmembed 0:100b7ad913c1 199
icmembed 0:100b7ad913c1 200 PWM.write(Apid/10);
icmembed 0:100b7ad913c1 201 PWM1.write(0.0f);
icmembed 0:100b7ad913c1 202 pc.printf("integrador en ciclo %0.2lf.\n \n", IT);
icmembed 0:100b7ad913c1 203
icmembed 0:100b7ad913c1 204 pc.printf("\r");
icmembed 0:100b7ad913c1 205 //pc.printf("PWM1 %i.\n \n", PWM1);
icmembed 0:100b7ad913c1 206 // pc.printf("PWM %i. \n \n", PWM);
icmembed 0:100b7ad913c1 207
icmembed 0:100b7ad913c1 208 }
icmembed 0:100b7ad913c1 209
icmembed 0:100b7ad913c1 210 else if (E < -0.01 && Apid < 1){
icmembed 0:100b7ad913c1 211 // PWM.pulsewidth_ms(0);
icmembed 0:100b7ad913c1 212 //PWM1.pulsewidth_ms(10*PID);
icmembed 0:100b7ad913c1 213 // PWM =0;
icmembed 0:100b7ad913c1 214
icmembed 0:100b7ad913c1 215 //PWM1 =10*PID;
icmembed 0:100b7ad913c1 216
icmembed 0:100b7ad913c1 217 //PWM1 = abs(PID);
icmembed 0:100b7ad913c1 218
icmembed 0:100b7ad913c1 219 PWM.write(0.0f);
icmembed 0:100b7ad913c1 220 PWM1.write(Apid);
icmembed 0:100b7ad913c1 221
icmembed 0:100b7ad913c1 222 pc.printf("integrador en ciclo %0.2lf.\n \n", IT);
icmembed 0:100b7ad913c1 223 pc.printf("erroren ciclo %lf.\n \n", E);
icmembed 0:100b7ad913c1 224
icmembed 0:100b7ad913c1 225 pc.printf("\r");
icmembed 0:100b7ad913c1 226 // pc.printf("PWM1 %i.\n \n", PWM1);
icmembed 0:100b7ad913c1 227 // pc.printf("PWM %i. \n \n", PWM);
icmembed 0:100b7ad913c1 228
icmembed 0:100b7ad913c1 229 }
icmembed 0:100b7ad913c1 230
icmembed 0:100b7ad913c1 231 else if (E < -0.01 && Apid >= 1){
icmembed 0:100b7ad913c1 232 // PWM.pulsewidth_ms(0);
icmembed 0:100b7ad913c1 233 //PWM1.pulsewidth_ms(10*PID);
icmembed 0:100b7ad913c1 234 // PWM =0;
icmembed 0:100b7ad913c1 235
icmembed 0:100b7ad913c1 236 //PWM1 =10*PID;
icmembed 0:100b7ad913c1 237
icmembed 0:100b7ad913c1 238 //PWM1 = abs(PID);
icmembed 0:100b7ad913c1 239
icmembed 0:100b7ad913c1 240 PWM.write(0.0f);
icmembed 0:100b7ad913c1 241 PWM1.write(Apid/10);
icmembed 0:100b7ad913c1 242
icmembed 0:100b7ad913c1 243 IT_1 =-2;
icmembed 0:100b7ad913c1 244
icmembed 0:100b7ad913c1 245 pc.printf("integrador en ciclo %0.2lf.\n \n", IT);
icmembed 0:100b7ad913c1 246 pc.printf("erroren ciclo %lf.\n \n", E);
icmembed 0:100b7ad913c1 247
icmembed 0:100b7ad913c1 248 pc.printf("\r");
icmembed 0:100b7ad913c1 249 // pc.printf("PWM1 %i.\n \n", PWM1);
icmembed 0:100b7ad913c1 250 // pc.printf("PWM %i. \n \n", PWM);
icmembed 0:100b7ad913c1 251
icmembed 0:100b7ad913c1 252 }
icmembed 0:100b7ad913c1 253
icmembed 0:100b7ad913c1 254 else if (( E < 0.01) || ( E < -0.01)){
icmembed 0:100b7ad913c1 255 PWM =0;
icmembed 0:100b7ad913c1 256 PWM1=0;
icmembed 0:100b7ad913c1 257 PID =0;
icmembed 0:100b7ad913c1 258 }
icmembed 0:100b7ad913c1 259
icmembed 0:100b7ad913c1 260 salida = abs(PT);
icmembed 0:100b7ad913c1 261
icmembed 0:100b7ad913c1 262
icmembed 0:100b7ad913c1 263 //IT_1=IT;
icmembed 0:100b7ad913c1 264 //DT_1=DT;
icmembed 0:100b7ad913c1 265 //YT_1=YT;
icmembed 0:100b7ad913c1 266
icmembed 0:100b7ad913c1 267 //Vo = (T*T)*Vi_2+(T-(T*T)-1)*Vo_2+(2-T)*Vo_1; // mando a salida
icmembed 0:100b7ad913c1 268 //Vi_2 =Vi_1;
icmembed 0:100b7ad913c1 269 //Vi_1 = valor;
icmembed 0:100b7ad913c1 270 //Vo_2 =Vo_1;
icmembed 0:100b7ad913c1 271 //Vo_1=Vo;
icmembed 0:100b7ad913c1 272 //error=valor-Vo;
icmembed 0:100b7ad913c1 273 //voo=Vo*255;
icmembed 0:100b7ad913c1 274
icmembed 0:100b7ad913c1 275
icmembed 0:100b7ad913c1 276
icmembed 0:100b7ad913c1 277
icmembed 0:100b7ad913c1 278 // wait(2);
icmembed 0:100b7ad913c1 279 }
icmembed 0:100b7ad913c1 280 }