Merge pull request #136 from BCLab-UNM/hotfix-PID_changes

Hotfix pid changes

Author: gmfricke

src/behaviours/src/PID.cpp

@@ -11,10 +11,13 @@ PID::PID(PIDConfig config)
 float PID::PIDOut(float calculatedError, float setPoint)
 {
 
+  //cout << "ErrorSize:  " << Error.size() << endl;
+
   if (Error.size() >= config.errorHistLength)
   {
     Error.pop_back();
   }
+
   Error.insert(Error.begin(), calculatedError); //insert new error into vector for history purposes.
 
   float P = 0; //proportional yaw output
@@ -31,17 +34,48 @@ float PID::PIDOut(float calculatedError, float setPoint)
   }
 
   //feed forward
-  float FF = config.feedForwardMultiplier * setPoint;
+  //float FF = config.feedForwardMultiplier * setPoint;
+    float FF = (pow(setPoint, 3) * config.feedForwardMultiplier) + (setPoint * (config.feedForwardMultiplier / 4.6));
+
+    if (!config.alwaysIntegral && Error.size() > 1)
+    {
+        //check the change of sign to see if the rover overshot its goal
+        float sign_change = Error[0] / Error[1];
+        //if the sign has changed between the previous error and the current error
+        if(sign_change < 0)
+        {
+            //reset the integral history and values
+            integralErrorHistArray.clear();
+            integralErrorHistArray.resize(config.integralErrorHistoryLength, 0.0);
+            I = 0;
+            step = 0;
+
+
+            //Store error zero into temp variable
+            float error_zero = Error[0];
+
+            //clear the error history in order to prevent movement in the incorrect direction because of the sign change
+            Error.clear();
+
+            //put back into vector
+            Error.push_back(error_zero);
+        }
+    }
+
+
 
   //error averager
   float avgError = 0;
-  if (Error.size() >= config.errorHistLength) {
-    for (int i = 0; i < config.errorHistLength; i++) {
+  if (Error.size() >= config.errorHistLength)
+  {
+    for (int i = 0; i < config.errorHistLength; i++)
+    {
       avgError += Error[i];
     }
     avgError /= config.errorHistLength;//config.errorHistLength;
   }
-  else {
+  else
+  {
     avgError = calculatedError;
   }
 
@@ -105,7 +139,7 @@ float PID::PIDOut(float calculatedError, float setPoint)
   }
 
   //Derivative
-  if (fabs(P) < config.antiWindup)
+  if (Error.size() < 4 )//(fabs(P) < config.antiWindup)
   {
     float avgPrevError = 0;
     for (int i = 1; i < Error.size(); i++)
@@ -121,11 +155,17 @@ float PID::PIDOut(float calculatedError, float setPoint)
       avgPrevError = Error[0];
     }
 
-    D = config.Kd * (Error[0] - Error[1]) * hz;
+
+    D = config.Kd * ((Error[0]+Error[1])/2 - (Error[2]+Error[3])/2) * hz;
+
+    //cout << "PID Error[0]:  " << Error[0] << ", Error[1]:  " << Error[1] << ", Error[2]:  " << Error[2] << ", Error[3]:  " << Error[3] << endl;
+
   }
 
   float PIDOut = P + I + D + FF;
 
+  //cout << "PID P:  " << P << ",  I:  " << I << ", D:  " << D << ", FF:  " << FF << endl;
+
   if (PIDOut > config.satUpper) //cap vel command
   {
     PIDOut = config.satUpper;
@@ -135,7 +175,5 @@ float PID::PIDOut(float calculatedError, float setPoint)
     PIDOut = config.satLower;
   }
 
-  cout << "PID OUTPUT:  " << PIDOut << endl;
-
   return PIDOut;
 }

src/behaviours/src/PickUpController.cpp

@@ -227,7 +227,7 @@ Result PickUpController::DoWork()
     // If we don't see any blocks or cubes turn towards the location of the last cube we saw.
     // I.E., try to re-aquire the last cube we saw.
 
-    float grasp_time_begin = 1.1;
+    float grasp_time_begin = 1.5;
     float raise_time_begin = 2.0;
     float lower_gripper_time_begin = 4.0;
     float target_reaquire_begin= 4.2;

src/sbridge/src/sbridge.cpp

@@ -24,7 +24,7 @@ void sbridge::cmdHandler(const geometry_msgs::Twist::ConstPtr& message) {
     double right = (message->angular.z);
 
     float max_turn_rate = 4.5; //radians per second
-    float max_linear_velocity = 0.6; // meters per second
+    float max_linear_velocity = 0.65; // meters per second
 
     float turn = 0;
     float forward = 0;
@@ -33,7 +33,7 @@ void sbridge::cmdHandler(const geometry_msgs::Twist::ConstPtr& message) {
     float angularVel = (right-left)/2;
 
     turn = angularVel/55;
-    forward = linearVel/425;
+    forward = linearVel/390;
     if (forward >= 150){
       forward -= (abs(turn)/5);
     }