File content as of revision 0:cf1a10a72034:

#include "mbed.h"
#include "math.h" 


PwmOut mypwm(D5);
PwmOut PWM(D3);
PwmOut PWM1(D4);
Serial pc(SERIAL_TX, SERIAL_RX);


AnalogIn analog_value(A0);
AnalogIn retro(A5); 
DigitalOut led(LED1);

double KP=0.4027;
double KI =0.01;
double KD1 =0.2276;
double KD2 =0.02283;
double REF = 0.6;

float Apid=0;



float T = 0.3;
// 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;
double ET;
double ET1;
double ET2;
double ET3;
double VT;
double VT1;



float precio = 12.34; 
int pesos; 
float centavos; 

int Ient;


int main() {
     PWM.period_ms(17);
     PWM1.period_ms(17);

    pc.printf("This program tiene la referencia %f .\n", REF);
     pc.printf("\r");
     
     
    
  //  printf("\nAnalogIn example\n");
    
    while(1) {
   
        YT =  retro.read();
        UT = REF;
        E = (UT-YT);
        ET = E;
        VT = KP*(ET-ET1)+((KD1/T)*(ET-2*ET1+ET2))+((KD2/(T*T))*(ET-3*ET1+3*ET2-ET3))+KI*T*ET+VT1;
        VT1=VT;
        ET3= ET2;
        ET2=ET1;
        ET1 = ET;
        
        //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;
   
        Apid = abs(VT);
        Ient =(int) Apid;

   
        pc.printf("\n\n");
        pc.printf("retro %lf.\t", YT);
        pc.printf("Error %lf. \t", E);
        pc.printf("referencia %f.\t", REF);
         pc.printf("PID %f.\t", VT);
         pc.printf("Absoluto %f.\t", Apid);
        pc.printf("\r");
   
    if( E > 0.02 && Apid < 1 ) {
    
    PWM.write(Apid);
    PWM1.write(0.0f);
   
     }
    else if ( E < -0.02 && Apid < 1 ) {
        
        
        PWM.write(0.0f);
    PWM1.write(Apid);
   
     }
        
        else if ( E > 0.0 &&  E < 0.02 ){
        
        VT1 = VT;
        KP=0;
        KI=0;
        KD1=0;
        KD1=0;
        PWM.write(Apid);
    //PWM.write(0.21);
    PWM1.write(0.0f);
        }
    
        wait_ms(10); // 200 ms
    }
}