Revision 0:d58c1b8d63d9, committed 2014-02-26

Comitter:

arnaudsuire

Date:

Wed Feb 26 08:46:04 2014 +0000

Commit message:

arnaud pid giro acc

Changed in this revision

--- /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

--- /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 
+