PIDco.hpp

00001 #ifndef PID_CO_H
00002 #define PID_CO_H
00003 #include "mbed.h"
00004 
00005 #define Kp 0.0001
00006 #define Ki 0.0000014
00007 #define Kd 0.0000001
00008 #define co (108 * 3.1415926535897) / 500
00009 
00010 
00011 class PIDco{
00012     
00013     public://use in main.cpp
00014         PIDco();
00015         double obt_spd();
00016         void pass_val(double PULSE,double TARGET);
00017         void wheel_ctl(PinName PIN_A,PinName PIN_B);
00018     
00019     private:
00020         void cal_spd();
00021         void cal_Error();
00022         void renew_Error();
00023         void cal_Pval();
00024         void cal_Ival();
00025         void cal_Dval();
00026         void cal_pwm();
00027         
00028         double spd;
00029         double Target,Error_a,Error_b;
00030         double Pval,Ival,Dval,Pwm;
00031         double Integral,dt;
00032         double diff,pulse_a,pulse_b;
00033         double v1p_set,v1m_set;
00034 };
00035 
00036 #endif
00037         
00038         
00039         
00040         
00041         
00042