Diff: main.cpp

Revision:

2:18a1325ac2f2

Parent:

1:b38405009bb4

Child:

3:5d7dca0ca819

--- a/main.cpp	Sat Jul 21 02:38:30 2018 +0000
+++ b/main.cpp	Mon Jul 23 22:57:25 2018 +0000
@@ -1,24 +1,19 @@
-//直動機構のPID制御のサンプルプログラム
-//ECライブラリをincludeして、//****//から//****//までの自作関数の群れ(out,move,calomega)をコピーして貼り付けて使用してください
-//main関数の最初にticker.attach(&calOmega,0.05);する必要があります
-
-
 #include "mbed.h"
 #include "EC.h"
+#define K 0.01
+#define stride 0.01
+#define count 20
 Serial pc(USBTX,USBRX);
 
-
-//************************************************************************//
 PwmOut motorF(PA_3); //モータードライバの接続ピン
 PwmOut motorR(PA_4); //モータードライバの接続ピン
-Ec Ec1(PA_6,PA_7,NC,2048,0.05); //エンコーダの設定（A相、B相、分解能、計算間隔[s]）
+Ec Ec1(PA_6,PA_7,NC,2048,0.01); //エンコーダの設定（A相、B相、分解能、計算間隔[s]）
 Ticker ticker;
+Timer timer;
 
-//モーター制御関数：-1〜+1の入力に対して、duty比の制限と回転方向の処理をして出力
-//最大出duty_max力は適宜変更してください
 void out (double duty)
 {
-    double duty_max = 0.9;
+    double duty_max = 0.3;
     if(duty > 0) { //入力duty比が正の場合
         if( abs(duty) < duty_max ) { //制限値を超えていない場合
             motorF = abs(duty);
@@ -38,50 +33,32 @@
     }
 }
 
-//移動の関数：移動先の座標[mm]を入力すると移動し、許容誤差±errorで停止する
-void move(double distance_target,double error) //(移動先の座標[mm],許容誤差の絶対値[mm])
-{
-    double Kp = 0.01; //P制御（比例制御）の係数
-    double Kd = 0.2; //D制御（微分制御）の係数
-    double Ki = 0.01; //I制御（積分制御）の係数
-    double pile; //I制御のための偏差の積算変数
-    double distance; //現在の座標
-    double velocity; //速度
-    double difference; //偏差
-    while(1) {//
-        if(abs(difference)> 10) {//目標との距離が10mm未満になるまではPD制御のみ
-            distance = Ec1.getCount()*(-1)*30*5/6*3.14159265359/2048; //座標[mm]
-            velocity = Ec1.getOmega()*(-1)*30*5/6*3.14159265359/2048; //速度[mm/s]
-            difference = distance_target - distance; //偏差[mm]
-            out(difference * Kp - velocity * Kd);
-        } else {//目標との距離が10mm未満になったらPID制御を開始
-            distance = Ec1.getCount()*(-1)*30*5/6*3.14159265359/2048;
-            velocity = Ec1.getOmega()*(-1)*30*5/6*3.14159265359/2048;
-            difference = distance_target - distance;
-            pile += difference;
-            if(abs(difference)> error) {
-                out(difference * Kp - velocity * Kd + pile *  Ki);
-            } else {//目標との距離がerrorに収まったら出力を0にしてbreak
-                out(0);
-                //pc.printf("x=%fmm, error=%fmm\r\n",distance,difference);
-                break;
-            }
-        }
-    }
-}
-
 void calOmega()
 {
     Ec1.CalOmega();
 }
-//************************************************************************//
-
 
 int main()
 {
-    ticker.attach(&calOmega,0.05);
+    ticker.attach(&calOmega,0.01);
+    timer.start();
+    double omega_old;
+    int i,j;
     while(1) {
-        move(800,1);
-        move(100,1);
+        pc.printf("Kp = %f : ",K+j*stride);
+        timer.reset();
+        i=0;
+        while(i<count) {
+            if(omega_old == 0 && Ec1.getOmega() == 0) break;
+            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");
+        j++;
     }
+    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);
 }
\ No newline at end of file