File content as of revision 2:6173078c34ef:

#include "QEI.h"
#include "mbed.h"
#include "RawSerial.h"
RawSerial pc(P5_0,P5_1); // tx, rx (ポートの指定)
RawSerial pcm(USBTX,USBRX); // tx, rx(USBシリアルポートの指定)


#define ROTATE_PER_REVOLUTIONS  360 //1回転でのパルス数
#define JJ 0.006637 //慣性モーメント
#define KT 0.184088 //モータ定数
#define KD 0.001    //微分ゲイン
#define KP 0.1      //比例ゲイン
#define KI 0        //積分ゲイン
#define T  1000     //割り込み時間(u_sec)

QEI wheel(P1_0, P1_1, NC, ROTATE_PER_REVOLUTIONS, QEI::X4_ENCODING);    //チャンネルAとBの初期状態を決める
PwmOut Servo1(D7);          //Pwm出力Servo1の初期化
PwmOut Servo2(D6);          //Pwm出力Servo2の初期化
//PwmOut Servo1(D4);          //Pwm出力Servo1の初期化
//PwmOut Servo2(D3);          //Pwm出力Servo2の初期化
//BusOut input(D6,D5);

Ticker msec;                //繰り返しタイマ割り込みmsecを定義
Timer time1;                //時間タイマー

double r=0;                 //目標値
double y,e,old_e,u;         //現在値，エラー値，1つ前のエラー値，
double t=0.001;             //割り込み周期


void pid()                                                           //PID制御部
{

    y=(double)wheel.getPulses()*360/(ROTATE_PER_REVOLUTIONS);       //エンコーダの値(角度)の取得
    pc.printf("(%-10f)\0",y);                                       //エンコーダ値(角度)の送信(serial)

    e=r-y;                                                          //誤差値の計算
    //u=KP*e;                                                       //P制御*******ここ書かせる*******
    //u=KP*e+KD*(e-old_e)/t;                                        //PD制御*******ここ書かせる*******
    u=KP*e+KD*(e-old_e)/t+KI*(e+old_e)*t;                           //PID計算(入力値の計算)*******ここ書かせる*******
    old_e=e;                                                        //誤差値の保存
    
    /***PWM値と回転方向の決定***/
    if(u<0) {
        Servo1=-u;
        Servo2=0;
        //input=0x02;
    } else {
        Servo1=0;
        Servo2=u;
        //input=0x01;
    }

}

int main()
{
    pcm.baud(921600);                                    //ボーレートの指定(通信速度)
    pc.baud(921600);                                    //ボーレートの指定
    Servo1.period(0.00001);                                //PWMの周期の指定
    Servo2.period(0.00001);                                //PWMの周期の指定
    msec.attach_us(&pid,T);                             //タイマ割り込みの指定
    time1.start();

    while(1) {

        //pc.printf("(%-10f)¥0",y);                       //エンコーダ値(角度)の送信(serial)
        pcm.printf("%d\t%-10f\t%-10f\r\n",time1.read_ms(),y,u);//時間，エンコーダ値(角度)の送信(serial)(t,y,u)

        /********************************************************************/
        /************************シリアル通信受信部*****************************/
        /********************************************************************/
        char cmd,check_cmd;
        char code[13];
        int i;


        i =0;

        while(1) {
            cmd =pc.getc();
            check_cmd ='m';
            //pc.printf("%c",cmd);
            if(cmd=='(') {
                i =0;
                while(1) {
                    cmd =pc.getc();
                    if(check_cmd!=cmd) {
                        pc.printf("%c",cmd);
                        code[i] =cmd;
                        check_cmd ='m';
                        i++;
                    }
                    if(cmd==')') {
                        i=12;
                        break;

                    }
                }
            }
            if(i>10) {
                break;
            }
        }
//     pc.printf("¥n%s¥n",code);
        r =atof(code);
        i=0;
        while(1) {
            i++;
            if(code[i]==')') {
                break;
            }
        }
        /********************************************************************/
        /*********************シリアル通信受信部ここまで*************************/
        /********************************************************************/

    }
    time1.stop();
    time1.reset();
}