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@4:77e3082c7cf7, 2018-07-24 (annotated)

Committer:
kageyuta
Date:
Tue Jul 24 12:11:43 2018 +0000
Revision:
4:77e3082c7cf7
Parent:
3:5d7dca0ca819
Child:
5:97c1b79316c0
fabs!!!!; ;

Who changed what in which revision?

UserRevisionLine numberNew contents of line
kageyuta 0:93650135c3a1 1 #include "mbed.h"
kageyuta 0:93650135c3a1 2 #include "EC.h"
kageyuta 4:77e3082c7cf7 3 #define K 0.01 //評価をはじめる最小のK値
kageyuta 4:77e3082c7cf7 4 #define stride 0.01 //K値を上げるときのきざみ幅
kageyuta 4:77e3082c7cf7 5 #define duty_max 0.3 //出力duty比の上限
kageyuta 4:77e3082c7cf7 6 #define evaluation_count 40 //定常振動しているとみなす振動回数
kageyuta 4:77e3082c7cf7 7 #define evaluation_time 3 //評価時間
kageyuta 0:93650135c3a1 8 Serial pc(USBTX,USBRX);
kageyuta 0:93650135c3a1 9
kageyuta 0:93650135c3a1 10 PwmOut motorF(PA_3); //モータードライバの接続ピン
kageyuta 0:93650135c3a1 11 PwmOut motorR(PA_4); //モータードライバの接続ピン
kageyuta 2:18a1325ac2f2 12 Ec Ec1(PA_6,PA_7,NC,2048,0.01); //エンコーダの設定（A相、B相、分解能、計算間隔[s]）
kageyuta 1:b38405009bb4 13 Ticker ticker;
kageyuta 2:18a1325ac2f2 14 Timer timer;
kageyuta 0:93650135c3a1 15
kageyuta 0:93650135c3a1 16 void out (double duty)
kageyuta 0:93650135c3a1 17 {
kageyuta 0:93650135c3a1 18 if(duty > 0) { //入力duty比が正の場合
kageyuta 4:77e3082c7cf7 19 if( fabs(duty) < duty_max ) { //制限値を超えていない場合
kageyuta 4:77e3082c7cf7 20 motorF = fabs(duty);
kageyuta 0:93650135c3a1 21 motorR = 0;
kageyuta 0:93650135c3a1 22 } else { //制限値を超えている場合
kageyuta 1:b38405009bb4 23 motorF = duty_max;
kageyuta 0:93650135c3a1 24 motorR = 0;
kageyuta 0:93650135c3a1 25 }
kageyuta 0:93650135c3a1 26 } else {//入力duty比が負の場合
kageyuta 4:77e3082c7cf7 27 if( fabs(duty) < duty_max) { //制限値を超えていない場合
kageyuta 0:93650135c3a1 28 motorF = 0;
kageyuta 4:77e3082c7cf7 29 motorR = fabs(duty);
kageyuta 0:93650135c3a1 30 } else { //制限値を超えている場合
kageyuta 0:93650135c3a1 31 motorF = 0;
kageyuta 1:b38405009bb4 32 motorR = duty_max;
kageyuta 0:93650135c3a1 33 }
kageyuta 0:93650135c3a1 34 }
kageyuta 0:93650135c3a1 35 }
kageyuta 0:93650135c3a1 36
kageyuta 0:93650135c3a1 37 void calOmega()
kageyuta 0:93650135c3a1 38 {
kageyuta 0:93650135c3a1 39 Ec1.CalOmega();
kageyuta 0:93650135c3a1 40 }
kageyuta 0:93650135c3a1 41
kageyuta 0:93650135c3a1 42 int main()
kageyuta 0:93650135c3a1 43 {
kageyuta 4:77e3082c7cf7 44 motorF.period_us(50);
kageyuta 4:77e3082c7cf7 45 motorR.period_us(50);
kageyuta 2:18a1325ac2f2 46 ticker.attach(&calOmega,0.01);
kageyuta 2:18a1325ac2f2 47 timer.start();
kageyuta 2:18a1325ac2f2 48 double omega_old;
kageyuta 2:18a1325ac2f2 49 int i,j;
kageyuta 4:77e3082c7cf7 50 pc.printf("AutoTuning...\r\n");
kageyuta 0:93650135c3a1 51 while(1) {
kageyuta 4:77e3082c7cf7 52 //pc.printf("Kp = %f : ",K+j*stride);
kageyuta 2:18a1325ac2f2 53 timer.reset();
kageyuta 2:18a1325ac2f2 54 i=0;
kageyuta 4:77e3082c7cf7 55 while(i<evaluation_count && timer.read()<evaluation_time) {
kageyuta 3:5d7dca0ca819 56 if(omega_old == 0 && Ec1.getOmega() == 0) {}
kageyuta 2:18a1325ac2f2 57 else if(omega_old * Ec1.getOmega() <= 0) i++;
kageyuta 2:18a1325ac2f2 58 omega_old = Ec1.getOmega();
kageyuta 2:18a1325ac2f2 59 out((10*(j%2) - Ec1.getCount()*(-1)*30*5/6*3.14159265359/2048) * (K + j*stride));
kageyuta 2:18a1325ac2f2 60 }
kageyuta 4:77e3082c7cf7 61 if(i==evaluation_count) break;
kageyuta 4:77e3082c7cf7 62 //pc.printf("%d periods\r\n");
kageyuta 2:18a1325ac2f2 63 j++;
kageyuta 0:93650135c3a1 64 }
kageyuta 3:5d7dca0ca819 65 out(0);
kageyuta 4:77e3082c7cf7 66 double period = timer.read() / evaluation_count;
kageyuta 4:77e3082c7cf7 67 pc.printf("Process Completed.\r\n");
kageyuta 4:77e3082c7cf7 68 //pc.printf("K = %f, T = %fs\r\n",K+j*stride,period);
kageyuta 4:77e3082c7cf7 69 pc.printf("Optimum Params : Kp = %f, Ki = %f, Kd = %f",0.6*(K+j*stride),0.5*period,0.125*period);
kageyuta 0:93650135c3a1 70 }