Diff: SpeedController/SpeedController.cpp

Revision:

5:f9dd9346bfd4

Parent:

4:f1f638a6b6c5

--- a/SpeedController/SpeedController.cpp	Tue Dec 24 10:24:35 2019 +0000
+++ /dev/null	Thu Jan 01 00:00:00 1970 +0000
@@ -1,99 +0,0 @@
-#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;
-}