File content as of revision 1:b38405009bb4:

//直動機構の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;

//モーター制御関数：-1〜+1の入力に対して、duty比の制限と回転方向の処理をして出力
//最大出duty_max力は適宜変更してください
void out (double duty)
{
    double duty_max = 0.9;
    if(duty > 0) { //入力duty比が正の場合
        if( abs(duty) < duty_max ) { //制限値を超えていない場合
            motorF = abs(duty);
            motorR = 0;
        } else { //制限値を超えている場合
            motorF = duty_max;
            motorR = 0;
        }
    } else {//入力duty比が負の場合
        if( abs(duty) < duty_max) { //制限値を超えていない場合
            motorF = 0;
            motorR = abs(duty);
        } else { //制限値を超えている場合
            motorF = 0;
            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; //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]
            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);
        move(100,1);
    }
}