Fix negative gains issue and create test

test/pid_tests.cpp

@@ -78,9 +78,12 @@ TEST(ParameterTest, integrationClampZeroGainTest)
 
 TEST(ParameterTest, integrationAntiwindupTest)
 {
-  RecordProperty("description","This test succeeds if the integral error is prevented from winding up.");
+  RecordProperty("description","This test succeeds if the integral error is prevented from winding up when i_gain > 0");
 
-  Pid pid(0.0, 2.0, 0.0, 1.0, -1.0, true);
+  double i_gain = 2.0;
+  double i_min = -1.0;
+  double i_max = 1.0;
+  Pid pid(0.0, i_gain, 0.0, i_max, i_min, true);
 
   double cmd = 0.0;
   double pe,ie,de;
@@ -98,6 +101,31 @@ TEST(ParameterTest, integrationAntiwindupTest)
   EXPECT_EQ(-1.0, cmd);
 }
 
+TEST(ParameterTest, negativeIntegrationAntiwindupTest)
+{
+  RecordProperty("description","This test succeeds if the integral error is prevented from winding up when i_gain < 0");
+
+  double i_gain = -2.0;
+  double i_min = -1.0;
+  double i_max = 1.0;
+  Pid pid(0.0, i_gain, 0.0, i_max, i_min, true);
+
+  double cmd = 0.0;
+  double pe,ie,de;
+
+  cmd = pid.computeCommand(-1.0, ros::Duration(1.0));
+  EXPECT_EQ(1.0, cmd);
+
+  cmd = pid.computeCommand(-1.0, ros::Duration(1.0));
+  EXPECT_EQ(1.0, cmd);
+
+  cmd = pid.computeCommand(0.5, ros::Duration(1.0));
+  EXPECT_EQ(0.0, cmd);
+
+  cmd = pid.computeCommand(-1.0, ros::Duration(1.0));
+  EXPECT_EQ(1.0, cmd);
+}
+
 TEST(ParameterTest, gainSettingCopyPIDTest)
 {
   RecordProperty("description","This test succeeds if a PID object has its gain set at different points in time then the values are get-ed and still remain the same, as well as when PID is copied.");