Diff: SpeedController.cpp

Revision:

0:de44976b7670

Child:

1:735c0a48b445

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/SpeedController.cpp	Wed Jul 24 17:11:01 2019 +0000
@@ -0,0 +1,102 @@
+#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;
+    if(duty<-duty_limit_)duty=-duty_limit_;
+    else if(duty>duty_limit_)duty=duty_limit_;
+    turn(duty);
+    pre_diff=diff;
+    ptsc_=t;
+    //printf("tw=%.2f,w=%.2f,diff=%.2f,pid=%.2f,Df=%.2f,Db=%.2f,duty=%.4f\r\n",target_omega,omega,diff,pid,Df_,Db_,duty);
+}
+
+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;
+}