Diff: controller.cpp

Revision:

7:f04bde0ca846

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/controller.cpp	Mon Jul 07 15:23:50 2014 +0000
@@ -0,0 +1,179 @@
+#include "mbed.h"
+#include "controller.h"
+
+
+
+
+PID::PID(float in_min,float in_max,float out_min,float out_max,float Kc, float tauI, float tauD, float interval) {
+ 
+ //BX tune
+    Kp = 0.0004;
+    Ki = 0.0;
+    Kd = 0.0;
+    setInputLimits(in_min,in_max);
+    setOutputLimits(out_min,out_max);
+ 
+    tSample_ = interval;
+ 
+    setTunings(Kc, tauI, tauD);
+ 
+    setPoint_ = 0.0;
+    processVariable_ = 0.0;
+    prevProcessVariable_ = 0.0;
+    controllerOutput_ = 0.0;
+    prevControllerOutput_ = 0.0;
+ 
+    
+ 
+}
+ 
+void PID::setInputLimits(float inMin, float inMax) {
+ 
+    //Make sure we haven't been given impossible values.
+    if (inMin >= inMax) {
+        return;
+    }
+ 
+   
+ 
+   
+ 
+    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;
+    }
+ 
+    
+ 
+   //ppp
+ 
+    outMin_ = outMin;
+    outMax_ = outMax;
+    outMid_ = (outMin+outMax)/2;
+    
+    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;
+ 
+   
+ 
+  
+ 
+    Kc_ = Kc;
+    tauI_=tauI;
+    tauD_=tauD;
+ 
+}
+
+
+
+ 
+
+        
+
+
+
+
+   
+   
+   
+   
+     
+        
+      
+   
+   
+ 
+    
+     
+float PID::compute(float center ,  float sp) {
+    //turn right 122~64   122
+    //turn left  64~6     8
+    float C = center;
+    float goal = sp;  // center of black
+    float error = goal - C;//
+    return 0.073+Kp*error;
+ 
+}
+ 
+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_;
+ 
+}