Diff: main.cpp

Revision:

4:77e3082c7cf7

Parent:

3:5d7dca0ca819

Child:

5:97c1b79316c0

diff -r 5d7dca0ca819 -r 77e3082c7cf7 main.cpp
--- a/main.cpp	Tue Jul 24 01:04:10 2018 +0000
+++ b/main.cpp	Tue Jul 24 12:11:43 2018 +0000
@@ -1,8 +1,10 @@
 #include "mbed.h"
 #include "EC.h"
-#define K 0.01
-#define stride 0.01
-#define count 40
+#define K 0.01 //評価をはじめる最小のK値
+#define stride 0.01 //K値を上げるときのきざみ幅
+#define duty_max 0.3 //出力duty比の上限
+#define evaluation_count 40 //定常振動しているとみなす振動回数
+#define evaluation_time 3 //評価時間
 Serial pc(USBTX,USBRX);
 
 PwmOut motorF(PA_3); //モータードライバの接続ピン
@@ -13,19 +15,18 @@
 
 void out (double duty)
 {
-    double duty_max = 0.6;
     if(duty > 0) { //入力duty比が正の場合
-        if( abs(duty) < duty_max ) { //制限値を超えていない場合
-            motorF = abs(duty);
+        if( fabs(duty) < duty_max ) { //制限値を超えていない場合
+            motorF = fabs(duty);
             motorR = 0;
         } else { //制限値を超えている場合
             motorF = duty_max;
             motorR = 0;
         }
     } else {//入力duty比が負の場合
-        if( abs(duty) < duty_max) { //制限値を超えていない場合
+        if( fabs(duty) < duty_max) { //制限値を超えていない場合
             motorF = 0;
-            motorR = abs(duty);
+            motorR = fabs(duty);
         } else { //制限値を超えている場合
             motorF = 0;
             motorR = duty_max;
@@ -40,26 +41,30 @@
 
 int main()
 {
+    motorF.period_us(50);
+    motorR.period_us(50);
     ticker.attach(&calOmega,0.01);
     timer.start();
     double omega_old;
     int i,j;
+    pc.printf("AutoTuning...\r\n");
     while(1) {
-        pc.printf("Kp = %f : ",K+j*stride);
+        //pc.printf("Kp = %f : ",K+j*stride);
         timer.reset();
         i=0;
-        while(i<count && timer.read()<3) {
+        while(i<evaluation_count && timer.read()<evaluation_time) {
             if(omega_old == 0 && Ec1.getOmega() == 0) {}
             else if(omega_old * Ec1.getOmega() <= 0) i++;
             omega_old = Ec1.getOmega();
             out((10*(j%2) - Ec1.getCount()*(-1)*30*5/6*3.14159265359/2048) * (K + j*stride));
         }
-        if(i==count) break;
-        pc.printf("TimeOut\r\n");
+        if(i==evaluation_count) break;
+        //pc.printf("%d periods\r\n");
         j++;
     }
     out(0);
-    double period = timer.read() / count;
-    pc.printf("K = %f, T = %fs\r\n",K+j*stride,period);
-    pc.printf("Kp = %f, Ki = %f, Kd = %f",0.6*(K+j*stride),0.5*period,0.125*period);
+    double period = timer.read() / evaluation_count;
+    pc.printf("Process Completed.\r\n");
+    //pc.printf("K = %f, T = %fs\r\n",K+j*stride,period);
+    pc.printf("Optimum Params : Kp = %f, Ki = %f, Kd = %f",0.6*(K+j*stride),0.5*period,0.125*period);
 }
\ No newline at end of file