Diff: main.cpp

Revision:

0:93650135c3a1

Child:

1:b38405009bb4

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/main.cpp	Fri Jul 20 12:40:29 2018 +0000
@@ -0,0 +1,84 @@
+#include "mbed.h"
+#include "EC.h"
+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]）
+Ticker ticker; //タイマースタート
+
+
+//モーター制御関数：-1〜+1の入力に対して、duty比の制限と回転方向の処理をして出力
+void out (double duty)
+{
+    if(duty > 0) { //入力duty比が正の場合
+        if( abs(duty) < 0.9 ) { //制限値を超えていない場合
+            motorF = abs(duty);
+            motorR = 0;
+        } else { //制限値を超えている場合
+            motorF = 0.9;
+            motorR = 0;
+        }
+    } else {//入力duty比が負の場合
+        if( abs(duty) < 0.9 ) { //制限値を超えていない場合
+            motorF = 0;
+            motorR = abs(duty);
+        } else { //制限値を超えている場合
+            motorF = 0;
+            motorR = 0.9;
+        }
+    }
+}
+
+
+//移動の関数：移動先の座標[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;
+    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);
+    while(1) {
+        move(800,1);
+        wait(1);
+        move(100,1);
+    }
+}
\ No newline at end of file