Diff: SpeedController.cpp

Revision:

11:9181694995d2

Parent:

6:d110c276aa5a

--- a/SpeedController.cpp	Sat Aug 10 05:36:57 2019 +0000
+++ b/SpeedController.cpp	Thu Feb 11 13:13:04 2021 +0000
@@ -29,7 +29,7 @@
 void SpeedControl::Sc(double target_omega)        //スカンジウムじゃないよ
 {
     double t=ec_->timer_.read();
-    ec_->calOmega();
+    //ec_->calOmega();
     double omega=ec_->getOmega();
     double diff=target_omega-omega;
     double pid=Kp_*diff+Kd_*(diff-pre_diff)/(t-ptsc_);
@@ -44,13 +44,19 @@
     turn(duty);
     pre_diff=diff;
     ptsc_=t;
+    //printf("omega %f\r\n",omega);
+    //if(omega!=0){printf("yo");}
+    if(target_omega==0&&omega==0) {
+    Df_=initial_Df_;
+    Db_=initial_Db_;
+    }
 }
 
 void SpeedControl::turn(double duty)
 {
     if(duty>duty_limit_)duty=duty_limit_;
     else if(duty<-duty_limit_)duty=-duty_limit_;
-
+//printf("%f\r\n",duty);
     if(duty>=0) {
         pwm_F_=duty;
         pwm_B_=0;