PIDco.cpp

00001 #include "PIDco.hpp"
00002 
00003 PIDco::PIDco(){
00004     spd = 0.0;
00005     Pwm = 0.0;
00006     dt = 0.05;
00007     Integral = 0.0;
00008     Error_b = 0.0;
00009     Error_a = 0.0;
00010     diff = 0.0;
00011     pulse_b = 0.0;
00012     }
00013     
00014 void PIDco::cal_spd(){
00015     diff = pulse_a - pulse_b;
00016     spd = co * diff / dt;
00017     pulse_b = pulse_a;
00018 }
00019 
00020 void PIDco::cal_Ival(){
00021     Integral += (Error_a + Error_b) / 2.0 * dt;
00022     Ival = Ki * Integral;
00023 }
00024 
00025 void PIDco::pass_val(double PULSE,double TARGET){
00026     pulse_a = PULSE;
00027     Target = TARGET;
00028 }
00029 
00030 void PIDco::wheel_ctl(PinName PIN_A,PinName PIN_B){
00031     PwmOut v1p(PIN_A);
00032     PwmOut v1m(PIN_B);
00033     
00034     v1p.period_us(1024);
00035     v1m.period_us(1024);
00036     
00037     cal_spd();
00038     cal_Error();
00039     cal_Pval();
00040     cal_Ival();
00041     cal_Dval();
00042     cal_pwm();
00043     
00044     if(Pwm > 0){
00045             v1p = Pwm > 0.5 ? 0.5 : Pwm;
00046             v1m = 0;
00047         }else if(Pwm < 0){
00048             v1p = 0;
00049             v1m = -Pwm > 0.5 ? 0.5 : -Pwm;
00050         }else{
00051             v1p = 0;
00052             v1m = 0;
00053         }
00054     
00055     renew_Error();
00056 }
00057     
00058 
00059 void PIDco::cal_Error(){ Error_a = Target - spd; }
00060 
00061 void PIDco::cal_Pval(){ Pval = Kp * Error_a; }
00062 
00063 void PIDco::cal_Dval(){ Dval = Kd * (Error_a - Error_b) / dt; }
00064 
00065 void PIDco::cal_pwm(){ Pwm += (Pval + Ival + Dval); }
00066 
00067 void PIDco::renew_Error(){ Error_b = Error_a; }
00068 
00069 double PIDco::obt_spd(){ return spd; }
00070 
00071 
00072 
00073 
00074     
00075