Revision 1:665ee7703fbe, committed 2016-10-21

Comitter:

fabian101

Date:

Fri Oct 21 09:12:19 2016 +0000

Parent:

0:6e12a3e5af19

Commit message:

result robot control 21-10 11:12

Changed in this revision

diff -r 6e12a3e5af19 -r 665ee7703fbe PID.cpp
--- a/PID.cpp	Thu Sep 02 16:48:10 2010 +0000
+++ /dev/null	Thu Jan 01 00:00:00 1970 +0000
@@ -1,324 +0,0 @@
-/**
- * @author Aaron Berk
- *
- * @section LICENSE
- *
- * Copyright (c) 2010 ARM Limited
- *
- * Permission is hereby granted, free of charge, to any person obtaining a copy
- * of this software and associated documentation files (the "Software"), to deal
- * in the Software without restriction, including without limitation the rights
- * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
- * copies of the Software, and to permit persons to whom the Software is
- * furnished to do so, subject to the following conditions:
- *
- * The above copyright notice and this permission notice shall be included in
- * all copies or substantial portions of the Software.
- *
- * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
- * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
- * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
- * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
- * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
- * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
- * THE SOFTWARE.
- *
- * @section DESCRIPTION
- * 
- * A PID controller is a widely used feedback controller commonly found in
- * industry.
- *
- * This library is a port of Brett Beauregard's Arduino PID library:
- *
- *  http://www.arduino.cc/playground/Code/PIDLibrary
- *
- * The wikipedia article on PID controllers is a good place to start on
- * understanding how they work:
- *
- *  http://en.wikipedia.org/wiki/PID_controller
- *
- * For a clear and elegant explanation of how to implement and tune a
- * controller, the controlguru website by Douglas J. Cooper (who also happened
- * to be Brett's controls professor) is an excellent reference:
- *
- *  http://www.controlguru.com/
- */
-
-/**
- * Includes
- */
-#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) {
-
-    //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() {
-
-    //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 scaledBias = 0.0;
-
-    if (usingFeedForward) {
-        scaledBias = (bias_ - outMin_) / outSpan_;
-    }
-
-    //Perform the PID calculation.
-    controllerOutput_ = scaledBias + 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_;
-
-}

diff -r 6e12a3e5af19 -r 665ee7703fbe PID.h
--- a/PID.h	Thu Sep 02 16:48:10 2010 +0000
+++ /dev/null	Thu Jan 01 00:00:00 1970 +0000
@@ -1,213 +0,0 @@
-/**
- * @author Aaron Berk
- *
- * @section LICENSE
- *
- * Copyright (c) 2010 ARM Limited
- *
- * Permission is hereby granted, free of charge, to any person obtaining a copy
- * of this software and associated documentation files (the "Software"), to deal
- * in the Software without restriction, including without limitation the rights
- * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
- * copies of the Software, and to permit persons to whom the Software is
- * furnished to do so, subject to the following conditions:
- *
- * The above copyright notice and this permission notice shall be included in
- * all copies or substantial portions of the Software.
- *
- * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
- * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
- * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
- * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
- * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
- * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
- * THE SOFTWARE.
- *
- * @section DESCRIPTION
- * 
- * A PID controller is a widely used feedback controller commonly found in
- * industry.
- *
- * This library is a port of Brett Beauregard's Arduino PID library:
- *
- *  http://www.arduino.cc/playground/Code/PIDLibrary
- *
- * The wikipedia article on PID controllers is a good place to start on
- * understanding how they work:
- *
- *  http://en.wikipedia.org/wiki/PID_controller
- *
- * For a clear and elegant explanation of how to implement and tune a
- * controller, the controlguru website by Douglas J. Cooper (who also happened
- * to be Brett's controls professor) is an excellent reference:
- *
- *  http://www.controlguru.com/
- */
-
-#ifndef PID_H
-#define PID_H
-
-/**
- * Includes
- */
-#include "mbed.h"
-
-/**
- * Defines
- */
-#define MANUAL_MODE 0
-#define AUTO_MODE   1
-
-/**
- * Proportional-integral-derivative controller.
- */
-class PID {
-
-public:
-
-    /**
-     * Constructor.
-     *
-     * Sets default limits [0-3.3V], 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 PID to manual or auto mode.
-     *
-     * @param mode        0 -> Manual
-     *             Non-zero -> Auto
-     */
-    void setMode(int mode);
-    
-    /**
-     * Set how fast the PID loop is run.
-     *
-     * @param interval PID calculation peformed every interval seconds.
-     */
-    void setInterval(float interval);
-    
-    /**
-     * Set the set point.
-     *
-     * @param sp The set point as a real world value.
-     */
-    void setSetPoint(float sp);
-    
-    /**
-     * Set the process value.
-     *
-     * @param pv The process value as a real world value.
-     */
-    void setProcessValue(float pv);
-    
-    /**
-     * 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(void);
-
-    //Getters.
-    float getInMin();
-    float getInMax();
-    float getOutMin();
-    float getOutMax();
-    float getInterval();
-    float getPParam();
-    float getIParam();
-    float getDParam();
-
-private:
-
-    bool usingFeedForward;
-    bool inAuto;
-
-    //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 /* PID_H */

diff -r 6e12a3e5af19 -r 665ee7703fbe SigInterpreter.cpp
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/SigInterpreter.cpp	Fri Oct 21 09:12:19 2016 +0000
@@ -0,0 +1,36 @@
+#include "SigInterpreter.h"
+
+AnalogIn emg1(A0);
+AnalogIn emg2(A1);
+AnalogIn emg3(A2);
+AnalogIn emg4(A3);
+
+SigInterpreter::SigInterpreter() {}
+
+double SigInterpreter::readValue(int sig) {
+    // returns the amplitude of EMG signal
+    if(sig > 4 || sig < 1) { // RIGHT
+        // nope
+        return 0.0;     // check later of dit idd goeie shit is
+    } if (sig == 1) {
+        float input = emg1.read();
+        return filter(input);
+    }
+    if (sig == 2) {
+        float input = emg2.read();
+        return filter(input);
+    } 
+    if (sig == 3) {
+        float input = emg3.read();
+        return filter(input);
+    }
+    if (sig == 4) {
+        float input = emg4.read();
+        return filter(input);
+    }     
+    return 0.0;
+}
+double SigInterpreter::filter(float input) {
+    //filter the input)
+    return 0.0;     //default, return the filtered input
+}
\ No newline at end of file

diff -r 6e12a3e5af19 -r 665ee7703fbe SigInterpreter.h
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/SigInterpreter.h	Fri Oct 21 09:12:19 2016 +0000
@@ -0,0 +1,17 @@
+#ifndef SIGINTERPRETER_H
+#define SIGINTERPRETER_H
+
+#include "initialize.h"
+
+class SigInterpreter {
+
+public:
+
+    SigInterpreter();
+    double readValue(int);
+    double filter(float);
+
+private:
+
+};
+#endif /* SIGINTERPRETER_H */