Diff: MotorController.cpp

Revision:

4:6bff84284ecb

Parent:

3:8668176943ae

Child:

5:2e5f40e761bd

--- a/MotorController.cpp	Tue Jun 22 08:35:02 2021 +0000
+++ b/MotorController.cpp	Fri Sep 10 09:00:54 2021 +0000
@@ -97,7 +97,7 @@
 }
 void MotorController::Sc(double target_omega)
 {
-    ec_->rewriteCount();
+    ec_->rewriteCount_2();
     ec_->calOmega();
 
     double duty=calSc(target_omega);
@@ -106,45 +106,66 @@
 }
 void MotorController::AcDuty(double target_angle)
 {
-    ec_->rewriteCount();
-    double angle = ec_->getRad();
+    ec_->rewriteCount_2();
+    //double angle = 62*3.14159265359f/180 - ec_->getRad()/3;
+    double angle = ec_->getRad()*36/60 - 6.4*3.14159265359f/180;
     double devia = target_angle - angle;
     double duty_omega = ac_duty_pid->calP_D(devia,ec_->getOmega());
+    printf("%f\t%f\t%f\r\n",duty_omega, target_angle, angle);
     turn(duty_omega);
 }
-void MotorController::AcOmega_1(double target_angle, double angle_)
+void MotorController::AcOmega_1(double target_angle,double max_duty)
 {
-    ec_->rewriteCount();
+    ec_->rewriteCount_1();
     ec_->calOmega();
-    //double angle = ec_->getRad();
-    double devia = target_angle - angle_;
+    double angle = ec_->getRad()/3 + 159.72*3.14159265359f/180;
+    double devia = target_angle - angle;
     double omega = ac_omega_pid->calP_D(devia,ec_->getOmega());
-    //printf("%f  ",omega);
     double duty=calSc(omega);
     if(fabs(devia)<ANGLE_ERROR) {//収束幅の設定
         omega=0;
     }
-    printf("%f  ",-duty);
-    if(duty > 0.2) duty = 0.2;
-    if(duty < -0.2)duty = -0.2;
-    printf("%f\t%f\t%f\r\n",-duty, target_angle, angle_);
-    turn(-duty);
+    printf("duty_1:%f\t",duty);
+    if(duty > max_duty) duty = max_duty;
+    if(duty < -max_duty)duty = -max_duty;
+    turn(duty);
 }
-void MotorController::AcOmega_2(double target_angle, double angle_)
+void MotorController::AcOmega_2(double target_angle,double angleA,double max_duty)
 {
-    ec_->rewriteCount();
+    ec_->rewriteCount_2();
     ec_->calOmega();
-    //double angle = ec_->getRad();
-    double devia = target_angle - angle_;
+    double angle;
+    if(angleA >= 0){
+            angle = ec_->getRad()*36/60 + (-102 - angleA/3 - angleA*16.63/90)*3.14159265359f/180;
+        }else if(angleA < 0){
+            angle = ec_->getRad()*36/60 + (-102 - angleA/3 + angleA*16.63/90)*3.14159265359f/180;
+        }
+    double devia = target_angle - angle;
     double omega = ac_omega_pid->calP_D(devia,ec_->getOmega());
-    printf("%f  ",omega);
     double duty=calSc(omega);
     if(fabs(devia)<ANGLE_ERROR) {//収束幅の設定
         omega=0;
     }
-    printf("%f  ",duty);
-    if(duty > 0.3) duty = 0.3;
-    if(duty < -0.3)duty = -0.3;
+    printf("duty_2:%f\t",duty);
+    if(duty > max_duty) duty = max_duty;
+    if(duty < -max_duty)duty = -max_duty;
+    turn(duty);
+}
+
+void MotorController::AcOmega(double target_angle)
+{
+    ec_->rewriteCount_2();
+    ec_->calOmega();
+    double angle = ec_->getRad()*36/60 - 6.4*3.14159265359f/180;
+    double devia = target_angle - angle;
+    double omega = ac_omega_pid->calP_D(devia,ec_->getOmega());
+    double duty=calSc(omega);
+    if(fabs(devia)<ANGLE_ERROR) {//収束幅の設定
+        omega=0;
+    }
+    printf("%f\t%f\t%f\t\r\n",duty, target_angle, angle);
+    if(duty > 0.2) duty = 0.2;
+    if(duty < -0.2)duty = -0.2;
     turn(duty);
 }