File content as of revision 6:4427687e6dbe:

#ifndef INCLUDED_PID_IMPL_H
#define INCLUDED_PID_IMPL_H

#include "PID.hpp"
#include "mbed.h"

namespace mbed_stl {

template <typename ErrType, typename MVType, typename KType>
PID<ErrType, MVType, KType>::PID(KType kp, KType ki, KType kd) :
        kp_(kp),
        ki_(ki),
        kd_(kd),
        integral_(),
        prev_error_(),
        prev_time_us_() {
    timer_.reset();
}

template <typename ErrType, typename MVType, typename KType>
inline MVType PID<ErrType, MVType, KType>::update(ErrType error) {
    float time_us = timer_.read_us() - prev_time_us_;
    
    // P制御(比例)
    MVType mv = kp_ * error;
    
    // I制御(積分)
    mv += ki_ * integral_;
    integral_ += time_us * (error + prev_error_) / (2.0f * 1000.0f * 1000.0f);
    
    // D制御(微分)
    if (time_us > 0.0f) {
        mv += kd_ * (error - prev_error_) * 1000.0f * 1000.0f / time_us;
    }
    
    prev_error_ = error;
    prev_time_us_ = time_us;
    timer_.start();
    
    return mv;
}

} /* namespace mbed_stl */

#endif