File content as of revision 0:6ea9b36d7630:

#include "mbed.h"
#include "QEI.h"
#define DELTA_T 0.004   //処理周期4msec
#define KP 0.0          //比例定数
#define KI 0.0          //積分定数
#define KD 0.0          //微分定数
#define n  100          //最大値最小値設定

mbed::PwmOut mypwm011(D11);
PwmOut mypwm012(D10);    //モーター１

PwmOut mypwm021(D6);
PwmOut mypwm022(D5);    //モーター２

PwmOut mypwm030(D3);    //アームのモータ？
PwmOut mypwm031(D4);

#define ROTATE_PER_REVOLUTIONS  24
QEI wheel01(   D7,D8,     //A相のピン、B相のピン
             NC,                             //回転数数えるピン（任意）
             ROTATE_PER_REVOLUTIONS,                          //1回転につき何パルスの出力をするか？
             QEI::X2_ENCODING );         //モード設定、X4_ENCODING→分解能が細かくなるが精度低い
             
QEI wheel02(   D9,D10,     //A相のピン、B相のピン
             NC,                             //回転数数えるピン（任意）
             ROTATE_PER_REVOLUTIONS,                          //1回転につき何パルスの出力をするか？
             QEI::X2_ENCODING );         //モード設定、X4_ENCODING→分解能が細かくなるが精度低い

DigitalOut myled(LED1);     //特に必要性のないLED定義
typedef signed long S32;
typedef float F32;
static S32 diff[2];
static F32 integral;

F32 pid_sample(int a,int b)
{
    int turn;
    F32 p,i,d;
    
    diff[0] = diff[1];
    diff[1]= a - b;
    integral += (diff[1]+diff[0])/2.0*DELTA_T;
    
    p = KP*diff[1];
    i = KI*integral;
    d = KD *(diff[1]-diff[0])/DELTA_T;
    
    turn = p + i + d;
    
    if(n<turn)turn = n;
    if(n>turn)turn = -n;    //最大値最小値の設定
    
    return turn;
    }

int main() {    while(true){
    int c =0;                //cはスイッチによって値うける、ｃの値を３パターン規定し、n,mに加算減算することで使えるものにする（または単純に定義してもいいか）
    int s = c;
    int m = c;
    mypwm011 = 1.0;
    mypwm012 = 0.0;
    
    mypwm021 = 1.0;
    mypwm022 = 0.0;
    
/*    mypwm.period_ms(10);
    mypwm.pulsewidth_ms(1);  */
 for(int i = s; i < wheel01.getPulses();)
        {       if(wheel01.getPulses()>wheel02.getPulses())
                        {mypwm012 = 1 - pid_sample(wheel01.getPulses(),wheel02.getPulses());}
                if(wheel01.getPulses()<wheel02.getPulses())
                        {mypwm022 = 1 - pid_sample(wheel02.getPulses(),wheel01.getPulses());}
                                                            }
    mypwm030 = 1.0;
    mypwm031 = 1.0;
    
    //アーム部の駆動
    
wheel01.reset();
wheel02.reset();   //パルス値の初期化

    mypwm011 = 0.0;
    mypwm012 = 1.0;         //逆転
    
    mypwm021 = 0.0;
    mypwm022 = 1.0;         //逆転 


for(int i = m; i > wheel01.getPulses();)
        {if(wheel01.getPulses()>wheel02.getPulses())
                        {mypwm011 = 1 - pid_sample(wheel01.getPulses(),wheel02.getPulses());}
                if(wheel01.getPulses()<wheel02.getPulses())
                        {mypwm021 = 1 - pid_sample(wheel02.getPulses(),wheel01.getPulses());}
                                                            }
            
    myled = 1;   //パルスがnの値にいくまで
    
    mypwm011 = 1.0;
    mypwm012 = 1.0;

    mypwm021 = 1.0;
    mypwm022 = 1.0;
    


wheel01.reset();
wheel02.reset();    //パルス値の初期化


}
}