Diff: controller.cpp

Revision:

12:f7acda4545ba

Parent:

10:d2401a243e8d

Child:

15:3fa780990a6a

diff -r 6189b2fc618a -r f7acda4545ba controller.cpp
--- a/controller.cpp	Wed Jun 25 05:50:03 2014 +0000
+++ b/controller.cpp	Mon Jun 30 02:07:57 2014 +0000
@@ -2,8 +2,10 @@
 #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) {
- 
+PID::PID(float in_min,float in_max,float out_min,float out_max,float Kp, float Ki, float Kd) {
+//in_min in_out  camera array
+//out_min out_max servo range
+//
     usingFeedForward = false;
     //inAuto = false;
  
@@ -11,10 +13,10 @@
     //Make sure to set these to more appropriate limits for your application.
  
  //BX tune
-    setInputLimits(in_min,in_max);
+    /*setInputLimits(in_min,in_max);
     setOutputLimits(out_min,out_max);
  
-    tSample_ = interval;
+    tSample_ = interval;// init is 10
  
     setTunings(Kc, tauI, tauD);
  
@@ -27,7 +29,7 @@
     accError_ = 0.0;
     bias_ = 0.0;
  
-    realOutput_ = 0.0;
+    realOutput_ = 0.0;*/
  
 }
  
@@ -185,68 +187,23 @@
  
 }
  
-float PID::compute(float pv,  float sp) {
+float PID::compute(float R ,float L,  float sp) {
     //pv centerR centerL
     //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;
+    float centerR = R;  //(right + left) / 2
+    float centerL = L; 
+    float centerB = sp;  // center of black
+    float error = centerB - centerR;
+    if(error < -8 && error >8){
+        return 0.085;
     }
- 
-    //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_;
+    else if((error < -8 && error > -20)||(error > 8 && error < 40)){
+        return 0.085 + Kp*error;
     }
- 
-    //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_ < 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_ = scaledPV;
+    else return 0.085;
     
-     
-    //Scale the output from percent span back out to a real world number.
-    return (controllerOutput_ );
+    
+   
  
 }