Revision 0:100b7ad913c1, committed 2017-03-14
- Comitter:
- icmembed
- Date:
- Tue Mar 14 21:46:39 2017 +0000
- Commit message:
- saludos comparto un control PID digital;
Changed in this revision
--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/main.cpp Tue Mar 14 21:46:39 2017 +0000
@@ -0,0 +1,280 @@
+#include "mbed.h"
+
+//AnalogIn analog_value(A0);
+//AnalogIn prop(A1);
+//DigitalOut PWM(D3);
+//DigitalOut PWM1(D4);
+PwmOut mypwm(D5);
+PwmOut PWM(D3);
+PwmOut PWM1(D4);
+Serial pc(SERIAL_TX, SERIAL_RX);
+
+// debemos definir tres entradas analigica para KP, KI, KD
+//AnalogIn KP(A0);
+//AnalogIn KI(A1);
+//AnalogIn KD(A3);
+// declaramos una entrada analogica para la referencia
+//AnalogIn REF(A4);
+// declaramos la salida analogica PID
+AnalogOut salida(PA_4); // esta es A2
+// declaramos la entrada de retroalimentacion
+AnalogIn retro(A5);
+
+// variables
+double KP=0.1;
+double KI =0.1;
+double KD =0.001;
+double REF = 0.5;
+
+float Apid=0;
+
+float Vo_1 = 0;
+float Vo_2 = 0;
+float Vo = 0;
+float Vi_2 = 0;
+float Vi_1 = 0;
+float Vi = 1;
+float R = 1;
+float C = 1;
+float L = 1;
+float T = 0.001;
+// PID
+double IT_1=0;
+double DT_1=0;
+double UT_1=0;
+double YT_1=0;
+double YT;
+double UT;
+double KP1 =0.2;
+double PT;
+double IT = 0;
+double Ti =0.9; //0.017;
+double Td =0.9; //0.017;
+double N =30;
+double DT =0;
+double PID =0;
+int k=0;
+ int analogPin = 0;
+ volatile float valor=0;
+ int ledPin = 10;
+ int voo=0;
+ //float error =0;
+double KPin;
+double KIin;
+double KDin;
+double ERROR1;
+double PT2;
+double E;
+
+
+
+int Ient;
+
+
+DigitalOut led(LED1);
+
+int main() {
+
+ PWM.period_ms(17);
+PWM1.period_ms(17);
+ //mypwm.period_ms(10);
+ //mypwm.pulsewidth_ms(1);
+ // float meas;
+
+ //printf("\nAnalogIn example\n");
+
+ pc.printf("This program tiene la referencia %f .\n", REF);
+
+
+
+
+
+ while(1) {
+ //meas = analog_value.read(); // Converts and read the analog input value (value from 0.0 to 1.0)
+ //meas = meas * 3300; // Change the value to be in the 0 to 3300 range
+ //PWM= meas;
+ //printf("measure = %.0f mV\n", meas);
+ // if (meas > 2000) { // If the value is greater than 2V then switch the LED on
+ // led = 1;
+
+ //
+ //KPin = KP.read();
+ //KPin = KPin*0.9428;
+ //KPin = KP*0.9;
+ //KIin = KI.read();
+ //KIin = KIin*0.9428;
+ //KIin = KI*0.2;
+
+ //KDin = KD.read();
+ //KDin = KDin*0.9428;
+ //KDin = KD*0.2;
+
+ YT = retro.read();
+ //YT = YT*1.25;
+ // UT = REF.read();
+ //UT = UT;
+ //
+ UT = REF;
+ E = (UT-YT);
+ PT= KP*(UT-YT);
+ IT = KI*T*(UT_1-YT_1)+IT_1;
+ DT= (KD/T)*(UT-YT+YT_1-UT_1);
+ PID= PT+IT+DT;
+ YT_1=YT;
+ UT_1=UT;
+ IT_1=IT;
+
+ // in_value=in_value*10;
+
+ // salida = in_value;
+
+
+ Apid = abs(PID);
+ Ient =(int) Apid;
+
+
+ //PWM = 1;
+ //PWM1 = 0;
+ //wait(5);
+ //}
+ //else {
+ //led = 0;
+ //}
+ // 200 ms
+ //PWM =0;
+ //PWM = 0;
+ //PWM1 = 1;
+ //wait(5);
+ pc.printf("\n\n");
+ pc.printf("retro %lf.\t", YT);
+ pc.printf("Error %lf. \n\n", E);
+ pc.printf("referencia %f.\n", REF);
+ pc.printf("\r");
+ //Apid =abs(IT);
+
+
+ // pc.printf("PID %lf. \n \n", PID);
+
+ // PT = (UT-YT); // kp es un potenciometro de 0 - 1
+ // PT2 = KPin*PT;
+ // IT = KIin*(((T/Ti)*UT_1)-((T/Ti)*YT_1)+IT_1); // ki potenciometro integral de 0 a 1
+ // DT = KDin*((N*(YT-YT_1))+((1-((N*T)/Td))*DT_1)); // kd ptenciometro derivative de 0 a 1
+ // PID = PT2+IT+DT;
+
+ // IT_1=IT;
+ // DT_1=DT;
+ // YT_1=YT;
+ // Apid = abs(PID);
+ // pc.printf("PID antes del absoluto %lf. \n \n", PID);
+ // pc.printf("Absoluto PID %lf. \n \n", Apid);
+//salida = (KPin*0.9428); //n aqui es la retroalimentacion
+if( E > 0.01 && Apid < 1 ) {
+ //PWM.pulsewidth_ms(10*PID);
+ //PWM1=PWM1.pulsewidth_ms(0);
+
+ //PWM = 10*PID;
+
+ // PWM = abs(PID);
+ // PMW1 =0;
+
+ PWM.write(Apid);
+ PWM1.write(0.0f);
+ pc.printf("integrador en ciclo %0.2lf.\n \n", IT);
+
+ pc.printf("\r");
+
+ //pc.printf("PWM1 %i.\n \n", PWM1);
+ // pc.printf("PWM %i. \n \n", PWM);
+
+ }
+ else if ( E > 0.01 && Apid >= 1 ) {
+ //PWM.pulsewidth_ms(10*PID);
+ //PWM1=PWM1.pulsewidth_ms(0);
+
+ //PWM = 10*PID;
+
+ // PWM = abs(PID);
+ // PMW1 =0;
+ IT_1=2;
+
+ PWM.write(Apid/10);
+ PWM1.write(0.0f);
+ pc.printf("integrador en ciclo %0.2lf.\n \n", IT);
+
+ pc.printf("\r");
+ //pc.printf("PWM1 %i.\n \n", PWM1);
+ // pc.printf("PWM %i. \n \n", PWM);
+
+ }
+
+ else if (E < -0.01 && Apid < 1){
+ // PWM.pulsewidth_ms(0);
+ //PWM1.pulsewidth_ms(10*PID);
+ // PWM =0;
+
+ //PWM1 =10*PID;
+
+ //PWM1 = abs(PID);
+
+ PWM.write(0.0f);
+ PWM1.write(Apid);
+
+ pc.printf("integrador en ciclo %0.2lf.\n \n", IT);
+ pc.printf("erroren ciclo %lf.\n \n", E);
+
+ pc.printf("\r");
+ // pc.printf("PWM1 %i.\n \n", PWM1);
+ // pc.printf("PWM %i. \n \n", PWM);
+
+ }
+
+ else if (E < -0.01 && Apid >= 1){
+ // PWM.pulsewidth_ms(0);
+ //PWM1.pulsewidth_ms(10*PID);
+ // PWM =0;
+
+ //PWM1 =10*PID;
+
+ //PWM1 = abs(PID);
+
+ PWM.write(0.0f);
+ PWM1.write(Apid/10);
+
+ IT_1 =-2;
+
+ pc.printf("integrador en ciclo %0.2lf.\n \n", IT);
+ pc.printf("erroren ciclo %lf.\n \n", E);
+
+ pc.printf("\r");
+ // pc.printf("PWM1 %i.\n \n", PWM1);
+ // pc.printf("PWM %i. \n \n", PWM);
+
+ }
+
+ else if (( E < 0.01) || ( E < -0.01)){
+ PWM =0;
+ PWM1=0;
+ PID =0;
+ }
+
+salida = abs(PT);
+
+
+ //IT_1=IT;
+ //DT_1=DT;
+ //YT_1=YT;
+
+//Vo = (T*T)*Vi_2+(T-(T*T)-1)*Vo_2+(2-T)*Vo_1; // mando a salida
+//Vi_2 =Vi_1;
+//Vi_1 = valor;
+//Vo_2 =Vo_1;
+//Vo_1=Vo;
+//error=valor-Vo;
+//voo=Vo*255;
+
+
+
+
+ // wait(2);
+ }
+}
--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/mbed.bld Tue Mar 14 21:46:39 2017 +0000
@@ -0,0 +1,1 @@
+http://mbed.org/users/mbed_official/code/mbed/builds/ef9c61f8c49f
\ No newline at end of file