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),Kd_(0),pre_diff(0),ptsc_(0),Cf_(1/45),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 {
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 diff=target_omega-omega;
00035     double pid=Kp_*diff+Kd_*(diff-pre_diff)/(t-ptsc_);
00036     double duty;
00037     if(target_omega>0) {
00038         Df_+=Cf_*pid;
00039         duty=Cf_*target_omega+Df_;
00040     } else if(target_omega<0) {
00041         Db_+=Cb_*pid;
00042         duty=Cb_*target_omega+Db_;
00043     } else duty=0;
00044     turn(duty);
00045     pre_diff=diff;
00046     ptsc_=t;
00047 }
00048 
00049 void SpeedControl::turn(double duty)
00050 {
00051     if(duty>duty_limit_)duty=duty_limit_;
00052     else if(duty<-duty_limit_)duty=-duty_limit_;
00053 
00054     if(duty>=0) {
00055         pwm_F_=duty;
00056         pwm_B_=0;
00057     } else if(duty<0) {
00058         pwm_F_=0;
00059         pwm_B_=-duty;
00060     }
00061 }
00062 
00063 void SpeedControl::setPDparam(double kp,double kd)
00064 {
00065     Kp_=kp;
00066     Kd_=kd;
00067 }
00068 
00069 
00070 void SpeedControl::setEquation(double cf,double df,double cb,double db)
00071 {
00072     Cf_=cf;
00073     if(cb>0)Cb_=cb;
00074     else Cb_=-cb;
00075     initial_Df_=df;
00076     initial_Db_=-db;
00077     Df_=initial_Df_;
00078     Db_=initial_Db_;
00079 }
00080 void SpeedControl::setDutyLimit(double duty_limit)
00081 {
00082     if(0<=duty_limit && duty_limit<0.95f)duty_limit_=duty_limit;
00083     else duty_limit_=0.95f;
00084 }
00085 
00086 void SpeedControl::reset()
00087 {
00088     ec_->reset();
00089     pre_diff=0;
00090     ptsc_=0;
00091     Df_=initial_Df_;
00092     Db_=initial_Db_;
00093 }
00094 
00095 void SpeedControl::stop()
00096 {
00097     pwm_F_=0;
00098     pwm_B_=0;
00099 }