File content as of revision 0:93650135c3a1:

#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比の制限と回転方向の処理をして出力
void out (double duty)
{
    if(duty > 0) { //入力duty比が正の場合
        if( abs(duty) < 0.9 ) { //制限値を超えていない場合
            motorF = abs(duty);
            motorR = 0;
        } else { //制限値を超えている場合
            motorF = 0.9;
            motorR = 0;
        }
    } else {//入力duty比が負の場合
        if( abs(duty) < 0.9 ) { //制限値を超えていない場合
            motorF = 0;
            motorR = abs(duty);
        } else { //制限値を超えている場合
            motorF = 0;
            motorR = 0.9;
        }
    }
}


//移動の関数：移動先の座標[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;
    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);
        wait(1);
        move(100,1);
    }
}