tarou yamada / PID

　

PID.cpp

Committer:
inst
Date:
2015-04-24
Revision:
0:e3c87254ed02

File content as of revision 0:e3c87254ed02:

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

const float PID::mPCoeff = 0.5f;
const float PID::mICoeff = 0.1f;
const float PID::mDCoeff = 10.0f;
const float PID::mIRange = 0.2f;

PID::PID(){
    mI = 0.0f;
    mOldDiff = 0.0f;
}

PID::~PID(){
}

void PID::updatePid( float diff ){
    // P制御
    float c = diff * mPCoeff;
    
    // I制御
    if ( mOldDiff * diff < 0.0f ){
        mI = 0;
    }
    if ( abs( diff ) < mIRange ){
        c += mI * mICoeff;
        mI += diff;
    } else {
        mI = 0;
    }
    
    // D制御
    c += ( diff - mOldDiff ) * mDCoeff;
    
    // 値の範囲を-1.0~1.0にする
    if ( c > 1.0f ){
        c = 1.0f;
    } else if ( c < -1.0f ){
        c = -1.0f;
    }
    
    control( c );
    
    mOldDiff = diff;
}