ロボステ6期
/
NHK2020-arm-sub4
12/20 mainmode3
SpeedController/SpeedController.cpp
- Committer:
- aoikoizumi
- Date:
- 2019-12-20
- Revision:
- 3:9cef6e58a462
- Parent:
- 0:6e2abd0956f1
File content as of revision 3:9cef6e58a462:
#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; }