ロボステ6期
/
NHK2020-arm-sub4
12/20 mainmode3
Diff: SpeedController/SpeedController.cpp
- Revision:
- 0:6e2abd0956f1
diff -r 000000000000 -r 6e2abd0956f1 SpeedController/SpeedController.cpp --- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/SpeedController/SpeedController.cpp Sat Dec 14 12:42:13 2019 +0000 @@ -0,0 +1,99 @@ +#include "mbed.h" +#include "EC.h" +#include "SpeedController.h" + +SpeedControl::SpeedControl(PinName pwm_F,PinName pwm_B,int us,Ec &ec) : + 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) +{ + ec_=&ec; + pwm_F_.period_us(us); + pwm_B_.period_us(us); +} +void SpeedControl::period(double s) +{ + pwm_F_.period(s); + pwm_B_.period(s); +} +void SpeedControl::period_ms(int ms) +{ + pwm_F_.period_ms(ms); + pwm_B_.period_ms(ms); +} +void SpeedControl::period_us(int us) +{ + pwm_F_.period_us(us); + pwm_B_.period_us(us); +} + + +void SpeedControl::Sc(double target_omega) //スカンジウムじゃないよ +{ + double t=ec_->timer_.read(); + ec_->calOmega(); + double omega=ec_->getOmega(); + double diff=target_omega-omega; + double pid=Kp_*diff+Kd_*(diff-pre_diff)/(t-ptsc_); + double duty; + if(target_omega>0) { + Df_+=Cf_*pid; + duty=Cf_*target_omega+Df_; + } else if(target_omega<0) { + Db_+=Cb_*pid; + duty=Cb_*target_omega+Db_; + } else duty=0; + turn(duty); + pre_diff=diff; + ptsc_=t; +} + +void SpeedControl::turn(double duty) +{ + if(duty>duty_limit_)duty=duty_limit_; + else if(duty<-duty_limit_)duty=-duty_limit_; + + if(duty>=0) { + pwm_F_=duty; + pwm_B_=0; + } else if(duty<0) { + pwm_F_=0; + pwm_B_=-duty; + } +} + +void SpeedControl::setPDparam(double kp,double kd) +{ + Kp_=kp; + Kd_=kd; +} + + +void SpeedControl::setEquation(double cf,double df,double cb,double db) +{ + Cf_=cf; + if(cb>0)Cb_=cb; + else Cb_=-cb; + initial_Df_=df; + initial_Db_=-db; + Df_=initial_Df_; + Db_=initial_Db_; +} +void SpeedControl::setDutyLimit(double duty_limit) +{ + if(0<=duty_limit && duty_limit<0.95f)duty_limit_=duty_limit; + else duty_limit_=0.95f; +} + +void SpeedControl::reset() +{ + ec_->reset(); + pre_diff=0; + ptsc_=0; + Df_=initial_Df_; + Db_=initial_Db_; +} + +void SpeedControl::stop() +{ + pwm_F_=0; + pwm_B_=0; +}