Revision 0:e3c87254ed02, committed 2015-04-24
- Comitter:
- inst
- Date:
- Fri Apr 24 07:37:55 2015 +0000
- Commit message:
- ?
Changed in this revision
PID.cpp | Show annotated file Show diff for this revision Revisions of this file |
PID.h | Show annotated file Show diff for this revision Revisions of this file |
diff -r 000000000000 -r e3c87254ed02 PID.cpp --- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/PID.cpp Fri Apr 24 07:37:55 2015 +0000 @@ -0,0 +1,45 @@ +#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; +} \ No newline at end of file
diff -r 000000000000 -r e3c87254ed02 PID.h --- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/PID.h Fri Apr 24 07:37:55 2015 +0000 @@ -0,0 +1,25 @@ +#ifndef INCLUDED_PID_H +#define INCLUDED_PID_H + +class PID{ +public: + PID(); + ~PID(); + + void updatePid( float diff ); + + float mOldDiff; + +private: + static const float mPCoeff; + static const float mICoeff; + static const float mDCoeff; + + static const float mIRange; + + virtual void control( float c ) = 0; + + float mI; +}; + +#endif \ No newline at end of file