tarou yamada / PID

　

PID.cpp@0:e3c87254ed02, 2015-04-24 (annotated)

Committer:
inst
Date:
Fri Apr 24 07:37:55 2015 +0000
Revision:
0:e3c87254ed02
?

Who changed what in which revision?

UserRevisionLine numberNew contents of line
inst 0:e3c87254ed02 1 #include "mbed.h"
inst 0:e3c87254ed02 2 #include "PID.h"
inst 0:e3c87254ed02 3
inst 0:e3c87254ed02 4 const float PID::mPCoeff = 0.5f;
inst 0:e3c87254ed02 5 const float PID::mICoeff = 0.1f;
inst 0:e3c87254ed02 6 const float PID::mDCoeff = 10.0f;
inst 0:e3c87254ed02 7 const float PID::mIRange = 0.2f;
inst 0:e3c87254ed02 8
inst 0:e3c87254ed02 9 PID::PID(){
inst 0:e3c87254ed02 10 mI = 0.0f;
inst 0:e3c87254ed02 11 mOldDiff = 0.0f;
inst 0:e3c87254ed02 12 }
inst 0:e3c87254ed02 13
inst 0:e3c87254ed02 14 PID::~PID(){
inst 0:e3c87254ed02 15 }
inst 0:e3c87254ed02 16
inst 0:e3c87254ed02 17 void PID::updatePid( float diff ){
inst 0:e3c87254ed02 18 // P制御
inst 0:e3c87254ed02 19 float c = diff * mPCoeff;
inst 0:e3c87254ed02 20
inst 0:e3c87254ed02 21 // I制御
inst 0:e3c87254ed02 22 if ( mOldDiff * diff < 0.0f ){
inst 0:e3c87254ed02 23 mI = 0;
inst 0:e3c87254ed02 24 }
inst 0:e3c87254ed02 25 if ( abs( diff ) < mIRange ){
inst 0:e3c87254ed02 26 c += mI * mICoeff;
inst 0:e3c87254ed02 27 mI += diff;
inst 0:e3c87254ed02 28 } else {
inst 0:e3c87254ed02 29 mI = 0;
inst 0:e3c87254ed02 30 }
inst 0:e3c87254ed02 31
inst 0:e3c87254ed02 32 // D制御
inst 0:e3c87254ed02 33 c += ( diff - mOldDiff ) * mDCoeff;
inst 0:e3c87254ed02 34
inst 0:e3c87254ed02 35 // 値の範囲を-1.0~1.0にする
inst 0:e3c87254ed02 36 if ( c > 1.0f ){
inst 0:e3c87254ed02 37 c = 1.0f;
inst 0:e3c87254ed02 38 } else if ( c < -1.0f ){
inst 0:e3c87254ed02 39 c = -1.0f;
inst 0:e3c87254ed02 40 }
inst 0:e3c87254ed02 41
inst 0:e3c87254ed02 42 control( c );
inst 0:e3c87254ed02 43
inst 0:e3c87254ed02 44 mOldDiff = diff;
inst 0:e3c87254ed02 45 }