File content as of revision 4:77e3082c7cf7:

#include "mbed.h"
#include "EC.h"
#define K 0.01 //評価をはじめる最小のK値
#define stride 0.01 //K値を上げるときのきざみ幅
#define duty_max 0.3 //出力duty比の上限
#define evaluation_count 40 //定常振動しているとみなす振動回数
#define evaluation_time 3 //評価時間
Serial pc(USBTX,USBRX);

PwmOut motorF(PA_3); //モータードライバの接続ピン
PwmOut motorR(PA_4); //モータードライバの接続ピン
Ec Ec1(PA_6,PA_7,NC,2048,0.01); //エンコーダの設定（A相、B相、分解能、計算間隔[s]）
Ticker ticker;
Timer timer;

void out (double duty)
{
    if(duty > 0) { //入力duty比が正の場合
        if( fabs(duty) < duty_max ) { //制限値を超えていない場合
            motorF = fabs(duty);
            motorR = 0;
        } else { //制限値を超えている場合
            motorF = duty_max;
            motorR = 0;
        }
    } else {//入力duty比が負の場合
        if( fabs(duty) < duty_max) { //制限値を超えていない場合
            motorF = 0;
            motorR = fabs(duty);
        } else { //制限値を超えている場合
            motorF = 0;
            motorR = duty_max;
        }
    }
}

void calOmega()
{
    Ec1.CalOmega();
}

int main()
{
    motorF.period_us(50);
    motorR.period_us(50);
    ticker.attach(&calOmega,0.01);
    timer.start();
    double omega_old;
    int i,j;
    pc.printf("AutoTuning...\r\n");
    while(1) {
        //pc.printf("Kp = %f : ",K+j*stride);
        timer.reset();
        i=0;
        while(i<evaluation_count && timer.read()<evaluation_time) {
            if(omega_old == 0 && Ec1.getOmega() == 0) {}
            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==evaluation_count) break;
        //pc.printf("%d periods\r\n");
        j++;
    }
    out(0);
    double period = timer.read() / evaluation_count;
    pc.printf("Process Completed.\r\n");
    //pc.printf("K = %f, T = %fs\r\n",K+j*stride,period);
    pc.printf("Optimum Params : Kp = %f, Ki = %f, Kd = %f",0.6*(K+j*stride),0.5*period,0.125*period);
}