Diff: main.cpp

Revision:

1:b38405009bb4

Parent:

0:93650135c3a1

Child:

2:18a1325ac2f2

--- a/main.cpp	Fri Jul 20 12:40:29 2018 +0000
+++ b/main.cpp	Sat Jul 21 02:38:30 2018 +0000
@@ -1,53 +1,58 @@
+//直動機構のPID制御のサンプルプログラム
+//ECライブラリをincludeして、//****//から//****//までの自作関数の群れ(out,move,calomega)をコピーして貼り付けて使用してください
+//main関数の最初にticker.attach(&calOmega,0.05);する必要があります
+
+
 #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; //タイマースタート
-
+Ticker ticker;
 
 //モーター制御関数：-1〜+1の入力に対して、duty比の制限と回転方向の処理をして出力
+//最大出duty_max力は適宜変更してください
 void out (double duty)
 {
+    double duty_max = 0.9;
     if(duty > 0) { //入力duty比が正の場合
-        if( abs(duty) < 0.9 ) { //制限値を超えていない場合
+        if( abs(duty) < duty_max ) { //制限値を超えていない場合
             motorF = abs(duty);
             motorR = 0;
         } else { //制限値を超えている場合
-            motorF = 0.9;
+            motorF = duty_max;
             motorR = 0;
         }
     } else {//入力duty比が負の場合
-        if( abs(duty) < 0.9 ) { //制限値を超えていない場合
+        if( abs(duty) < duty_max) { //制限値を超えていない場合
             motorF = 0;
             motorR = abs(duty);
         } else { //制限値を超えている場合
             motorF = 0;
-            motorR = 0.9;
+            motorR = duty_max;
         }
     }
 }
 
-
 //移動の関数：移動先の座標[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;
+    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]
+            difference = distance_target - distance; //偏差[mm]
             out(difference * Kp - velocity * Kd);
         } else {//目標との距離が10mm未満になったらPID制御を開始
             distance = Ec1.getCount()*(-1)*30*5/6*3.14159265359/2048;
@@ -56,7 +61,7 @@
             pile += difference;
             if(abs(difference)> error) {
                 out(difference * Kp - velocity * Kd + pile *  Ki);
-            } else {//目標との距離がerrorにおさまったら出力を0にしてbreakする
+            } else {//目標との距離がerrorに収まったら出力を0にしてbreak
                 out(0);
                 //pc.printf("x=%fmm, error=%fmm\r\n",distance,difference);
                 break;
@@ -65,12 +70,11 @@
     }
 }
 
-
 void calOmega()
 {
     Ec1.CalOmega();
 }
-//****
+//************************************************************************//
 
 
 int main()
@@ -78,7 +82,6 @@
     ticker.attach(&calOmega,0.05);
     while(1) {
         move(800,1);
-        wait(1);
         move(100,1);
     }
 }
\ No newline at end of file