![](/media/cache/img/default_profile.jpg.50x50_q85.jpg)
fsaf
Fork of MPU6050_Driver_Balanceddddd by
Diff: PID.cpp
- Revision:
- 1:f9658c7309ef
--- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/PID.cpp Fri Apr 20 08:27:50 2018 +0000 @@ -0,0 +1,220 @@ +/************************************************************************************************************************************************************** +// This is a modified version of mbed /users/aberk/code/PID/ for LSM303DLHC +// +// Changes made by Ryan Spillman: +// Fixed a mathematical issue (need to better document said fix) +// +// Need to better document scaling and other mathematical issues +**************************************************************************************************************************************************************/ +#include "PID.h" + +PID::PID(float Kc, float tauI, float tauD, float interval) +{ + + usingFeedForward = false; + inAuto = false; + + //Default the limits to the full range of I/O: 3.3V + //Make sure to set these to more appropriate limits for + //your application. + setInputLimits(0.0, 3.3); + setOutputLimits(0.0, 3.3); + + tSample_ = interval; + + setTunings(Kc, tauI, tauD); + + setPoint_ = 0.0; + processVariable_ = 0.0; + prevProcessVariable_ = 0.0; + controllerOutput_ = 0.0; + prevControllerOutput_ = 0.0; + + accError_ = 0.0; + bias_ = 0.0; + + realOutput_ = 0.0; + +} + +void PID::setInputLimits(float inMin, float inMax) +{ + inMin_ = inMin; + inMax_ = inMax; + inSpan_ = inMax - inMin; + +} + +void PID::setOutputLimits(float outMin, float outMax) +{ + + outMin_ = outMin; + outMax_ = outMax; + outSpan_ = outMax - outMin; + +} + +void PID::setTunings(float Kc, float tauI, float tauD) +{ + //Store raw values to hand back to user on request. + pParam_ = Kc; + iParam_ = tauI; + dParam_ = tauD; + + float tempTauR; + + if (tauI == 0.0f) { + tempTauR = 0.0f; + } else { + tempTauR = (1.0f / tauI) * tSample_; + } + + //For "bumpless transfer" we need to rescale the accumulated error. + if (inAuto) { + if (tempTauR == 0.0f) { + accError_ = 0.0f; + } else { + accError_ *= (Kc_ * tauR_) / (Kc * tempTauR); + } + } + + Kc_ = Kc; + tauR_ = tempTauR; + tauD_ = tauD / tSample_; + +} + +void PID::reset(void) +{ + prevControllerOutput_ = 0.0f; + + //Clear any error in the integral. + accError_ = 0.0f; + +} + +void PID::setMode(int mode) { + + //We were in manual, and we just got set to auto. + //Reset the controller internals. + if (mode != 0 && !inAuto) { + reset(); + } + + inAuto = (mode != 0); + +} + +void PID::setInterval(float interval) { + + if (interval > 0) { + //Convert the time-based tunings to reflect this change. + tauR_ *= (interval / tSample_); + accError_ *= (tSample_ / interval); + tauD_ *= (interval / tSample_); + tSample_ = interval; + } + +} + +void PID::setSetPoint(float sp) { + + setPoint_ = sp; + +} + +void PID::setProcessValue(float pv) { + + processVariable_ = pv; + +} + +void PID::setBias(float bias){ + + bias_ = bias; + usingFeedForward = 1; + +} + +float PID::compute() +{ + float error = setPoint_ - processVariable_; + + //Check and see if the output is pegged at a limit and only + //integrate if it is not. This is to prevent reset-windup. + if (!(prevControllerOutput_ >= outMax_ && error > 0) && !(prevControllerOutput_ <= outMin_ && error < 0)) { + accError_ += error; + } + + //Compute the current slope of the input signal. + float dMeas = (processVariable_ - prevProcessVariable_) / tSample_; + + //Perform the PID calculation. + controllerOutput_ = Kc_ * (error + (tauR_ * accError_) - (tauD_ * dMeas)); + + //scale PID output based on user provided input and output restraints + controllerOutput_ = ((outMax_ - outMin_) * ((Kc_ * controllerOutput_) - inMin_)) / (inMax_ - inMin_) + outMin_; + + //Make sure the computed output is within output constraints. + if (controllerOutput_ < outMin_) { + controllerOutput_ = outMin_; + } else if (controllerOutput_ > outMax_) { + controllerOutput_ = outMax_; + } + + //Remember this output for the windup check next time. + prevControllerOutput_ = controllerOutput_; + //Remember the input for the derivative calculation next time. + prevProcessVariable_ = processVariable_; + + //Scale the output from percent span back out to a real world number. + return controllerOutput_; +} + +float PID::getInMin() { + + return inMin_; + +} + +float PID::getInMax() { + + return inMax_; + +} + +float PID::getOutMin() { + + return outMin_; + +} + +float PID::getOutMax() { + + return outMax_; + +} + +float PID::getInterval() { + + return tSample_; + +} + +float PID::getPParam() { + + return pParam_; + +} + +float PID::getIParam() { + + return iParam_; + +} + +float PID::getDParam() { + + return dParam_; + +}