pid

Dependents:   Labo_TRSE_Drone

Files at this revision

API Documentation at this revision

Comitter:
arnaudsuire
Date:
Wed Feb 26 08:46:04 2014 +0000
Commit message:
arnaud pid giro acc

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 d58c1b8d63d9 PID.cpp
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/PID.cpp	Wed Feb 26 08:46:04 2014 +0000
@@ -0,0 +1,292 @@
+/**
+* Includes
+*/
+#include "stdafx.h"
+#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.
+    //Make sure to set these to more appropriate limits for your application.
+    setInputLimits(0.0, 100.0);
+    setOutputLimits(0.0,100.0);
+
+    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) {
+
+    //Make sure we haven't been given impossible values.
+    if (inMin >= inMax) {
+        return;
+    }
+
+    //Rescale the working variables to reflect the changes.
+    prevProcessVariable_ *= (inMax - inMin) / inSpan_;
+    accError_ *= (inMax - inMin) / inSpan_;
+
+    //Make sure the working variables are within the new limits.
+    if (prevProcessVariable_ > 1) {
+        prevProcessVariable_ = 1;
+    }
+    else if (prevProcessVariable_ < 0) {
+        prevProcessVariable_ = 0;
+    }
+
+    inMin_ = inMin;
+    inMax_ = inMax;
+    inSpan_ = inMax - inMin;
+
+}
+
+void PID::setOutputLimits(float outMin, float outMax) {
+
+    //Make sure we haven't been given impossible values.
+    if (outMin >= outMax) {
+        return;
+    }
+
+    //Rescale the working variables to reflect the changes.
+    prevControllerOutput_ *= (outMax - outMin) / outSpan_;
+
+    //Make sure the working variables are within the new limits.
+    if (prevControllerOutput_ > 1) {
+        prevControllerOutput_ = 1;
+    }
+    else if (prevControllerOutput_ < 0) {
+        prevControllerOutput_ = 0;
+    }
+
+    outMin_ = outMin;
+    outMax_ = outMax;
+    outSpan_ = outMax - outMin;
+
+}
+
+void PID::setTunings(float Kc, float tauI, float tauD) {
+
+    //Verify that the tunings make sense.
+    if (Kc == 0.0 || tauI < 0.0 || tauD < 0.0) {
+        return;
+    }
+
+    //Store raw values to hand back to user on request.
+    pParam_ = Kc;
+    iParam_ = tauI;
+    dParam_ = tauD;
+
+    float tempTauR;
+
+    if (tauI == 0.0) {
+        tempTauR = 0.0;
+    }
+    else {
+        tempTauR = (1.0 / tauI) * tSample_;
+    }
+
+    //For "bumpless transfer" we need to rescale the accumulated error.
+    //if (inAuto) {
+        //if (tempTauR == 0.0) {
+            //accError_ = 0.0;
+        //}
+        //else {
+            accError_ *= (Kc_ * tauR_) / (Kc * tempTauR);
+        //}
+    //}
+
+    Kc_ = Kc;
+    tauR_ = tempTauR;
+    tauD_ = tauD / tSample_;
+
+}
+
+void PID::reset(void) {
+
+    float scaledBias = 0.0;
+
+    if (usingFeedForward) {
+        scaledBias = (bias_ - outMin_) / outSpan_;
+    }
+    else {
+        scaledBias = (realOutput_ - outMin_) / outSpan_;
+    }
+
+    prevControllerOutput_ = scaledBias;
+    prevProcessVariable_ = (processVariable_ - inMin_) / inSpan_;
+
+    //Clear any error in the integral.
+    accError_ = 0;
+
+}
+/*
+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 pv,  float sp) {
+
+    //enregistrer variables dans var interne
+    processVariable_ = pv; //ce que l'on mesure
+    setPoint_ = sp;  // ce que l'on veut atteindre
+
+    //Pull in the input and setpoint, and scale them into percent span.
+    float scaledPV = (processVariable_ - inMin_) / inSpan_;
+
+    if (scaledPV > 1.0) {
+        scaledPV = 1.0;
+    }
+    else if (scaledPV < 0.0) {
+        scaledPV = 0.0;
+    }
+
+    float scaledSP = (setPoint_ - inMin_) / inSpan_;
+    if (scaledSP > 1.0) {
+        scaledSP = 1;
+    }
+    else if (scaledSP < 0.0) {
+        scaledSP = 0;
+    }
+
+    float error = scaledSP - scaledPV;
+
+    //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_ >= 1 && error > 0) && !(prevControllerOutput_ <= 0 && error < 0)) {
+        accError_ += error;
+    }
+
+    //Compute the current slope of the input signal.
+    float dMeas = (scaledPV - prevProcessVariable_) / tSample_;
+    //float dMeas = (scaledPV - prevProcessVariable_);
+
+    float scaledBias = 0.0;
+
+    if (usingFeedForward) {
+        scaledBias = (bias_ - outMin_) / outSpan_;
+    }
+
+    //Perform the PID calculation.
+    controllerOutput_ = scaledBias + Kc_ * (error + (tauR_ * accError_) - (tauD_ * dMeas));
+    //controllerOutput_ = Kc_ * error + tauR_ * accError_ + tauD_ * dMeas;
+
+    //Make sure the computed output is within output constraints.
+    if (controllerOutput_ < 0.0) {
+        controllerOutput_ = 0.0;
+    }
+    else if (controllerOutput_ > 1.0) {
+        controllerOutput_ = 1.0;
+    }
+
+    //Remember this output for the windup check next time.
+    prevControllerOutput_ = controllerOutput_;
+    //Remember the input for the derivative calculation next time.
+    prevProcessVariable_ = scaledPV;
+
+    //Scale the output from percent span back out to a real world number.
+    return ((controllerOutput_ * outSpan_) + outMin_);
+
+}
+
+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_;
+
+}
\ No newline at end of file
diff -r 000000000000 -r d58c1b8d63d9 PID.h
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/PID.h	Wed Feb 26 08:46:04 2014 +0000
@@ -0,0 +1,123 @@
+#pragma once
+
+#ifndef PID_H
+#define PID_H
+
+class PID
+{
+public:
+
+    /*
+    * Constructeur
+    * Sets default limits, calculates tuning parameters, and sets manual mode with no bias.
+    * @param Kc - Tuning parameter
+    * @param tauI - Tuning parameter
+    * @param tauD - Tuning parameter
+    * @param interval PID calculation performed every interval seconds.
+    */
+    PID(float Kc, float tauI, float tauD, float interval);
+
+    /*
+    * Scale from inputs to 0-100%.
+    * @param InMin The real world value corresponding to 0%.
+    * @param InMax The real world value corresponding to 100%.
+    */
+    void setInputLimits(float inMin, float inMax);
+
+    /*
+    * Scale from outputs to 0-100%.
+    * @param outMin The real world value corresponding to 0%.
+    * @param outMax The real world value corresponding to 100%.
+    */
+    void setOutputLimits(float outMin, float outMax);
+
+    /*
+    * Calculate PID constants.
+    * Allows parameters to be changed on the fly without ruining calculations.
+    * @param Kc - Tuning parameter
+    * @param tauI - Tuning parameter
+    * @param tauD - Tuning parameter
+    */
+    void setTunings(float Kc, float tauI, float tauD);
+
+    /*
+    * Reinitializes controller internals. Automatically
+    * called on a manual to auto transition.
+    */
+    void reset(void);
+
+    /*
+    * Set how fast the PID loop is run.
+    * @param interval PID calculation peformed every interval seconds.
+    */
+    void setInterval(float interval);
+
+    /*
+    * Set the bias.
+    * @param bias The bias for the controller output.
+    */
+    void setBias(float bias);
+
+    /*
+    * PID calculation.
+    * @return The controller output as a float between outMin and outMax.
+    */
+    float compute(float pv, float sp);
+
+    //Getters.
+    float getInMin();
+    float getInMax();
+    float getOutMin();
+    float getOutMax();
+    float getInterval();
+    float getPParam();
+    float getIParam();
+    float getDParam();
+
+private:
+    
+    bool usingFeedForward;
+
+    //Actual tuning parameters used in PID calculation.
+    float Kc_;
+    float tauR_;
+    float tauD_;
+
+    //Raw tuning parameters.
+    float pParam_;
+    float iParam_;
+    float dParam_;
+
+    //The point we want to reach.
+    float setPoint_;
+    //The thing we measure.
+    float processVariable_;
+    float prevProcessVariable_;
+    //The output that affects the process variable.
+    float controllerOutput_;
+    float prevControllerOutput_;
+
+    //We work in % for calculations so these will scale from
+    //real world values to 0-100% and back again.
+    float inMin_;
+    float inMax_;
+    float inSpan_;
+    float outMin_;
+    float outMax_;
+    float outSpan_;
+
+    //The accumulated error, i.e. integral.
+    float accError_;
+    //The controller output bias.
+    float bias_;
+
+    //The interval between samples.
+    float tSample_;
+
+    //Controller output as a real world value.
+    volatile float realOutput_;
+
+};
+
+#endif 
+