File content as of revision 2:5b2331251b3e:

#include "mbed.h"
#include "MD_PID.h"
#include "QEI.h"
#include "MD.h"

MD_PID::MD_PID()
{
    ref1 = 0;   
}

MD_PID::MD_PID(MD *md_, double kp_, double ki_, double kd_, int pulse_, QEI *qei_)
{
    md = md_;
    qei = qei_;
    
    ref1 = 0;
    kp = kp_;
    ki = ki_;
    kd = kd_;
    pulse = pulse_;
    
    Reset();
}

double MD_PID::Drive(double ref)
{
    /*
    if(ref1 != ref)
    {
        Reset();
        ref1 = ref;
    }
    */
    if(ref == 0)
    {
        duty = 0;
        md->rotate(duty);
        Read_Speed();
    }
    else
    {
        if(kp >= 1.0){
            duty = ref / kp;
            Read_Speed();
        }
        else
            duty -= PID(ref);
            
        if(duty > 1.0)
            duty = 1.0;
        else if(duty < -1.0)
            duty = -1.0;
    }
    
    if( ( (ref >= 0) ^ (Speed() >= 0) ) && fabs(Speed()) > 0.3)
        duty = 0;
    
    md->rotate(duty);
    return duty;
}

double MD_PID::PID(double target)
{
    diff[0] = diff[1];
    //delta = timer.read();
    //qs = (double)(qei->Read()) / delta / (double)pulse ;
    //timer.reset();
    //qei->Reset();
    Read_Speed();
    diff[1] = qs - target;
    integral += ((diff[0] + diff[1]) / 2.0) * delta;
    p = kp * diff[1];
    i = ki * integral;
    d = kd * (diff[1] - diff[0]);
    return p + i + d;
}

void MD_PID::Read_Speed()
{
    delta = timer.read();
    qs = (double)(qei->Read()) / delta / (double)pulse ;
    timer.reset();
    qei->Reset();
}

double MD_PID::Speed()
{
   return qs;
}

void MD_PID::Reset()
{
    qs = 0;
    diff[0] = 0;
    diff[1] = 0.0;
    integral = 0.0;
    timer.reset();
    timer.start();
    qei->Reset();
}

void MD_PID::PID_Step(double interval)
{
    double time[200],speed[200];
    Timer ST;
    ST.reset();
    ST.start();
    md->rotate(-1);
    for(int i = 0; i < 200; i++){
        PID(0);
        time[i] = ST.read(); 
        speed[i] = Speed();
        wait(interval);
    }
    md->rotate(0);
    for(int i = 0; i < 200; i++){
        printf("%f\t%f\r\n",time[i],speed[i]);
    }
}

MD_PID* Create_MD_PID(  int number, int i2c_addr, I2C *i2c,             // QEI
                        double kp_, double ki_, double kd_, int pulse_, // PID
                        PinName pwm, PinName dere)                      // MD
{
    QEI *qei = new QEI(number, i2c_addr, i2c);
    printf("qei\r\n");
    MD *md = new MD(pwm, dere);
    printf("md\r\n");
    MD_PID *md_pid = new MD_PID(md, kp_, ki_, kd_, pulse_, qei);
    printf("md_pid\r\n");
    return md_pid;
}