Catch the GIANT Caplico! / Mbed 2 deprecated PID_AutoTuning

Important changes to repositories hosted on mbed.com

Mbed hosted mercurial repositories are deprecated and are due to be permanently deleted in July 2026.

To keep a copy of this software download the repository Zip archive or clone locally using Mercurial.

It is also possible to export all your personal repositories from the account settings page.

Dependencies:   EC mbed

Fork of PID by Catch the GIANT Caplico!

main.cpp@1:b38405009bb4, 2018-07-21 (annotated)

Committer:
kageyuta
Date:
Sat Jul 21 02:38:30 2018 +0000
Revision:
1:b38405009bb4
Parent:
0:93650135c3a1
Child:
2:18a1325ac2f2
?????PID????????????

Who changed what in which revision?

UserRevisionLine numberNew contents of line
kageyuta 1:b38405009bb4 1 //直動機構のPID制御のサンプルプログラム
kageyuta 1:b38405009bb4 2 //ECライブラリをincludeして、//****//から//****//までの自作関数の群れ(out,move,calomega)をコピーして貼り付けて使用してください
kageyuta 1:b38405009bb4 3 //main関数の最初にticker.attach(&calOmega,0.05);する必要があります
kageyuta 1:b38405009bb4 4
kageyuta 1:b38405009bb4 5
kageyuta 0:93650135c3a1 6 #include "mbed.h"
kageyuta 0:93650135c3a1 7 #include "EC.h"
kageyuta 0:93650135c3a1 8 Serial pc(USBTX,USBRX);
kageyuta 0:93650135c3a1 9
kageyuta 0:93650135c3a1 10
kageyuta 1:b38405009bb4 11 //************************************************************************//
kageyuta 0:93650135c3a1 12 PwmOut motorF(PA_3); //モータードライバの接続ピン
kageyuta 0:93650135c3a1 13 PwmOut motorR(PA_4); //モータードライバの接続ピン
kageyuta 0:93650135c3a1 14 Ec Ec1(PA_6,PA_7,NC,2048,0.05); //エンコーダの設定（A相、B相、分解能、計算間隔[s]）
kageyuta 1:b38405009bb4 15 Ticker ticker;
kageyuta 0:93650135c3a1 16
kageyuta 0:93650135c3a1 17 //モーター制御関数：-1〜+1の入力に対して、duty比の制限と回転方向の処理をして出力
kageyuta 1:b38405009bb4 18 //最大出duty_max力は適宜変更してください
kageyuta 0:93650135c3a1 19 void out (double duty)
kageyuta 0:93650135c3a1 20 {
kageyuta 1:b38405009bb4 21 double duty_max = 0.9;
kageyuta 0:93650135c3a1 22 if(duty > 0) { //入力duty比が正の場合
kageyuta 1:b38405009bb4 23 if( abs(duty) < duty_max ) { //制限値を超えていない場合
kageyuta 0:93650135c3a1 24 motorF = abs(duty);
kageyuta 0:93650135c3a1 25 motorR = 0;
kageyuta 0:93650135c3a1 26 } else { //制限値を超えている場合
kageyuta 1:b38405009bb4 27 motorF = duty_max;
kageyuta 0:93650135c3a1 28 motorR = 0;
kageyuta 0:93650135c3a1 29 }
kageyuta 0:93650135c3a1 30 } else {//入力duty比が負の場合
kageyuta 1:b38405009bb4 31 if( abs(duty) < duty_max) { //制限値を超えていない場合
kageyuta 0:93650135c3a1 32 motorF = 0;
kageyuta 0:93650135c3a1 33 motorR = abs(duty);
kageyuta 0:93650135c3a1 34 } else { //制限値を超えている場合
kageyuta 0:93650135c3a1 35 motorF = 0;
kageyuta 1:b38405009bb4 36 motorR = duty_max;
kageyuta 0:93650135c3a1 37 }
kageyuta 0:93650135c3a1 38 }
kageyuta 0:93650135c3a1 39 }
kageyuta 0:93650135c3a1 40
kageyuta 0:93650135c3a1 41 //移動の関数：移動先の座標[mm]を入力すると移動し、許容誤差±errorで停止する
kageyuta 0:93650135c3a1 42 void move(double distance_target,double error) //(移動先の座標[mm],許容誤差の絶対値[mm])
kageyuta 0:93650135c3a1 43 {
kageyuta 0:93650135c3a1 44 double Kp = 0.01; //P制御（比例制御）の係数
kageyuta 0:93650135c3a1 45 double Kd = 0.2; //D制御（微分制御）の係数
kageyuta 0:93650135c3a1 46 double Ki = 0.01; //I制御（積分制御）の係数
kageyuta 1:b38405009bb4 47 double pile; //I制御のための偏差の積算変数
kageyuta 1:b38405009bb4 48 double distance; //現在の座標
kageyuta 1:b38405009bb4 49 double velocity; //速度
kageyuta 1:b38405009bb4 50 double difference; //偏差
kageyuta 0:93650135c3a1 51 while(1) {//
kageyuta 0:93650135c3a1 52 if(abs(difference)> 10) {//目標との距離が10mm未満になるまではPD制御のみ
kageyuta 0:93650135c3a1 53 distance = Ec1.getCount()*(-1)*30*5/6*3.14159265359/2048; //座標[mm]
kageyuta 0:93650135c3a1 54 velocity = Ec1.getOmega()*(-1)*30*5/6*3.14159265359/2048; //速度[mm/s]
kageyuta 1:b38405009bb4 55 difference = distance_target - distance; //偏差[mm]
kageyuta 0:93650135c3a1 56 out(difference * Kp - velocity * Kd);
kageyuta 0:93650135c3a1 57 } else {//目標との距離が10mm未満になったらPID制御を開始
kageyuta 0:93650135c3a1 58 distance = Ec1.getCount()*(-1)*30*5/6*3.14159265359/2048;
kageyuta 0:93650135c3a1 59 velocity = Ec1.getOmega()*(-1)*30*5/6*3.14159265359/2048;
kageyuta 0:93650135c3a1 60 difference = distance_target - distance;
kageyuta 0:93650135c3a1 61 pile += difference;
kageyuta 0:93650135c3a1 62 if(abs(difference)> error) {
kageyuta 0:93650135c3a1 63 out(difference * Kp - velocity * Kd + pile * Ki);
kageyuta 1:b38405009bb4 64 } else {//目標との距離がerrorに収まったら出力を0にしてbreak
kageyuta 0:93650135c3a1 65 out(0);
kageyuta 0:93650135c3a1 66 //pc.printf("x=%fmm, error=%fmm\r\n",distance,difference);
kageyuta 0:93650135c3a1 67 break;
kageyuta 0:93650135c3a1 68 }
kageyuta 0:93650135c3a1 69 }
kageyuta 0:93650135c3a1 70 }
kageyuta 0:93650135c3a1 71 }
kageyuta 0:93650135c3a1 72
kageyuta 0:93650135c3a1 73 void calOmega()
kageyuta 0:93650135c3a1 74 {
kageyuta 0:93650135c3a1 75 Ec1.CalOmega();
kageyuta 0:93650135c3a1 76 }
kageyuta 1:b38405009bb4 77 //************************************************************************//
kageyuta 0:93650135c3a1 78
kageyuta 0:93650135c3a1 79
kageyuta 0:93650135c3a1 80 int main()
kageyuta 0:93650135c3a1 81 {
kageyuta 0:93650135c3a1 82 ticker.attach(&calOmega,0.05);
kageyuta 0:93650135c3a1 83 while(1) {
kageyuta 0:93650135c3a1 84 move(800,1);
kageyuta 0:93650135c3a1 85 move(100,1);
kageyuta 0:93650135c3a1 86 }
kageyuta 0:93650135c3a1 87 }