SpeedController.cpp

00001 #include "mbed.h"
00002 #include "EC.h"
00003 #include "SpeedController.h"
00004 
00005 SpeedControl::SpeedControl(PinName pwm_F,PinName pwm_B,int us,Ec &ec) :
00006     Kp_(0),Ki_(0),Kd_(0),pre_diff(0),ptsc_(0),Cf_(1/300),Cb_(1/45),Df_(0),Db_(0),initial_Df_(0),initial_Db_(0),duty_limit_(0.95),pwm_F_(pwm_F),pwm_B_(pwm_B)
00007 {//Cf_(1/300)
00008     ec_=&ec;
00009     pwm_F_.period_us(us);
00010     pwm_B_.period_us(us);
00011 }
00012 void SpeedControl::period(double s)
00013 {
00014     pwm_F_.period(s);
00015     pwm_B_.period(s);
00016 }
00017 void SpeedControl::period_ms(int ms)
00018 {
00019     pwm_F_.period_ms(ms);
00020     pwm_B_.period_ms(ms);
00021 }
00022 void SpeedControl::period_us(int us)
00023 {
00024     pwm_F_.period_us(us);
00025     pwm_B_.period_us(us);
00026 }
00027 
00028 
00029 void SpeedControl::Sc(double target_omega)        //スカンジウムじゃないよ
00030 {
00031     double t=ec_->timer_.read();
00032     ec_->calOmega();
00033     double omega=ec_->getOmega();
00034    /* double is_skip=0;
00035     if(omega=target_omega) {
00036         is_skip=1;
00037     }
00038     if(is_skip=1) {
00039         if(omega<(target_omega-15)) {
00040             omega=target_omega-15;
00041         }
00042     }*/
00043     double diff=target_omega-omega;
00044     double integral=0;
00045     integral+=diff;
00046     double pid=Kp_*diff+Ki_*integral+Kd_*(diff-pre_diff)/(t-ptsc_);
00047     double duty;
00048     if(target_omega>0) {
00049         Df_+=Cf_*pid;
00050         duty=Cf_*target_omega+Df_;
00051         //duty=Cf_*target_omega+pid;
00052     } else if(target_omega<0) {
00053         Db_+=Cb_*pid;
00054         duty=Cb_*target_omega+Db_;
00055     } else duty=0;
00056     turn(duty);
00057     pre_diff=diff;
00058     ptsc_=t;
00059     //printf("%f\r\n",duty);
00060 }
00061 
00062 void SpeedControl::turn(double duty)
00063 {
00064     if(duty>duty_limit_)duty=duty_limit_;
00065     else if(duty<-duty_limit_)duty=-duty_limit_;
00066 
00067     if(duty>=0) {
00068         pwm_F_=duty;
00069         pwm_B_=0;
00070     } else if(duty<0) {
00071         pwm_F_=0;
00072         pwm_B_=-(duty);
00073     }
00074 }
00075 
00076 void SpeedControl::setPIDparam(double kp,double ki,double kd)
00077 {
00078     Kp_=kp;
00079     Ki_=ki;
00080     Kd_=kd;
00081 }
00082 
00083 
00084 void SpeedControl::setEquation(double cf,double df,double cb,double db)
00085 {
00086     Cf_=cf;
00087     if(cb>0)Cb_=cb;
00088     else Cb_=-cb;
00089     initial_Df_=df;
00090     initial_Db_=-db;
00091     Df_=initial_Df_;
00092     Db_=initial_Db_;
00093 }
00094 void SpeedControl::setDutyLimit(double duty_limit)
00095 {
00096     if(0<=duty_limit && duty_limit<0.95f)duty_limit_=duty_limit;
00097     else duty_limit_=0.95f;
00098 }
00099 
00100 void SpeedControl::reset()
00101 {
00102     ec_->reset();
00103     pre_diff=0;
00104     ptsc_=0;
00105     Df_=initial_Df_;
00106     Db_=initial_Db_;
00107 }
00108 
00109 void SpeedControl::stop()
00110 {
00111     pwm_F_=0;
00112     pwm_B_=0;
00113 }