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     //printf("omega %f\r\n",omega);
00048     //if(omega!=0){printf("yo");}
00049     if(target_omega==0&&omega==0) {
00050     Df_=initial_Df_;
00051     Db_=initial_Db_;
00052     }
00053 }
00054 
00055 void SpeedControl::turn(double duty)
00056 {
00057     if(duty>duty_limit_)duty=duty_limit_;
00058     else if(duty<-duty_limit_)duty=-duty_limit_;
00059 //printf("%f\r\n",duty);
00060     if(duty>=0) {
00061         pwm_F_=duty;
00062         pwm_B_=0;
00063     } else if(duty<0) {
00064         pwm_F_=0;
00065         pwm_B_=-duty;
00066     }
00067 }
00068 
00069 void SpeedControl::setPDparam(double kp,double kd)
00070 {
00071     Kp_=kp;
00072     Kd_=kd;
00073 }
00074 
00075 
00076 void SpeedControl::setEquation(double cf,double df,double cb,double db)
00077 {
00078     Cf_=cf;
00079     if(cb>0)Cb_=cb;
00080     else Cb_=-cb;
00081     initial_Df_=df;
00082     initial_Db_=-db;
00083     Df_=initial_Df_;
00084     Db_=initial_Db_;
00085 }
00086 void SpeedControl::setDutyLimit(double duty_limit)
00087 {
00088     if(0<=duty_limit && duty_limit<0.95f)duty_limit_=duty_limit;
00089     else duty_limit_=0.95f;
00090 }
00091 
00092 void SpeedControl::reset()
00093 {
00094     ec_->reset();
00095     pre_diff=0;
00096     ptsc_=0;
00097     Df_=initial_Df_;
00098     Db_=initial_Db_;
00099 }
00100 
00101 void SpeedControl::stop()
00102 {
00103     pwm_F_=0;
00104     pwm_B_=0;
00105 }