Add cpp PIDParameter unit test and fix discovered bug

wpimath/src/main/native/cpp/controller/PIDController.cpp

@@ -18,6 +18,7 @@ using namespace frc;
 PIDController::PIDController(double Kp, double Ki, double Kd,
                              units::second_t period)
     : m_Kp(Kp), m_Ki(Ki), m_Kd(Kd), m_period(period) {
+  CheckGains();
   if (period <= 0_s) {
     wpi::math::MathSharedStore::ReportError(
         "Controller period must be a positive number, got {}!", period.value());

wpimath/src/test/native/cpp/controller/PIDParameterTest.cpp

@@ -0,0 +1,71 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+#include <gtest/gtest.h>
+
+#include "frc/controller/PIDController.h"
+
+static constexpr double kSetpoint = 50.0;
+static constexpr double kRange = 200;
+static constexpr double kTolerance = 10.0;
+
+TEST(PIDParameterTest, Constructor) {
+  frc::PIDController controllerOk{0.1, 0.1, 0.1};
+  EXPECT_TRUE(controllerOk.GetP()==0.1 && controllerOk.GetI()==0.1 && controllerOk.GetD()==0.1);
+  frc::PIDController controllerBadP{-0.1, 0.1, 0.1};
+  EXPECT_TRUE(controllerBadP.GetP()==0.0 && controllerBadP.GetI()==0.0 && controllerBadP.GetD()==0.0);  
+  frc::PIDController controllerBadI{0.1, -0.1, 0.1};
+  EXPECT_TRUE(controllerBadI.GetP()==0.0 && controllerBadI.GetI()==0.0 && controllerBadI.GetD()==0.0);  
+  frc::PIDController controllerBadD{0.1, 0.1, -0.1};
+  EXPECT_TRUE(controllerBadD.GetP()==0.0 && controllerBadD.GetI()==0.0 && controllerBadD.GetD()==0.0);  
+}
+
+TEST(PIDParameterTest, SetPID) {
+  frc::PIDController controller{0.2, 0.2, 0.2};
+  controller.SetPID(0.1, 0.1, 0.1);
+  EXPECT_TRUE(controller.GetP()==0.1 && controller.GetI()==0.1 && controller.GetD()==0.1);
+  controller.SetPID(-0.1, 0.1, 0.1);
+  EXPECT_TRUE(controller.GetP()==0.0 && controller.GetI()==0.0 && controller.GetD()==0.0);  
+  controller.SetPID(0.2, 0.2, 0.2);
+  controller.SetPID(0.1, -0.1, 0.1);
+  EXPECT_TRUE(controller.GetP()==0.0 && controller.GetI()==0.0 && controller.GetD()==0.0);  
+  controller.SetPID(0.2, 0.2, 0.2);
+  controller.SetPID(0.1, 0.1, -0.1);
+  EXPECT_TRUE(controller.GetP()==0.0 && controller.GetI()==0.0 && controller.GetD()==0.0);  
+}
+
+TEST(PIDParameterTest, SetP) {
+  frc::PIDController controller{0.2, 0.2, 0.2};
+  controller.SetP(0.1);
+  EXPECT_TRUE(controller.GetP()==0.1 && controller.GetI()==0.2 && controller.GetD()==0.2);
+  controller.SetP(-0.1);
+  EXPECT_TRUE(controller.GetP()==0.0 && controller.GetI()==0.0 && controller.GetD()==0.0);  
+}
+
+TEST(PIDParameterTest, SetI) {
+  frc::PIDController controller{0.2, 0.2, 0.2};
+  controller.SetI(0.1);
+  EXPECT_TRUE(controller.GetP()==0.2 && controller.GetI()==0.1 && controller.GetD()==0.2);
+  controller.SetI(-0.1);
+  EXPECT_TRUE(controller.GetP()==0.0 && controller.GetI()==0.0 && controller.GetD()==0.0);  
+}
+
+TEST(PIDParameterTest, SetD) {
+  frc::PIDController controller{0.2, 0.2, 0.2};
+  controller.SetD(0.1);
+  EXPECT_TRUE(controller.GetP()==0.2 && controller.GetI()==0.2 && controller.GetD()==0.1);
+  controller.SetI(-0.1);
+  EXPECT_TRUE(controller.GetP()==0.0 && controller.GetI()==0.0 && controller.GetD()==0.0);  
+}
+
+TEST(PIDParameterTest, SetIZone) {
+  frc::PIDController controller{0.2, 0.2, 0.2};
+  controller.SetIZone(10.0);
+  EXPECT_TRUE(controller.GetIZone()==10.0);
+  // This should produce a MathSharedStore::ReportError message but still set it
+  // negative
+  controller.SetIZone(-10.0);
+  EXPECT_TRUE(controller.GetIZone()==-10.0);
+}
+