def testTimeSeriesConstantInput(self): kp = 0.2 ki = 0.02 kd = 0.1 p = PIDController(kp, 0.0, 0.0) i = PIDController(0.0, ki, 0.0) d = PIDController(0.0, 0.0, kd) t = 0.0 dt = 0.12 while t < 10.0: t += dt global_time.updateTime(t) desired = -0.5 measured = 2.0 p_command = p.update(desired, measured) i_command = i.update(desired, measured) d_command = d.update(desired, measured) pid_command = self.pid.update(desired, measured) self.assertAlmostEqual(p_command, kp * -2.5) # Because the input signal is constant, here the discrete-time # integral is exact self.assertAlmostEqual(i_command, ki * -2.5 * t) if (t == dt): # First run self.assertAlmostEqual(d_command, kd * -2.5 / dt) else: self.assertAlmostEqual(d_command, 0.0) self.assertAlmostEqual(pid_command, p_command + i_command + d_command)
def testTimeSeriesRampInput(self): kp = 0.2 ki = 0.02 kd = 0.1 p = PIDController(kp, 0.0, 0.0) i = PIDController(0.0, ki, 0.0) d = PIDController(0.0, 0.0, kd) t = 0.0 dt = 0.008 i_command_1 = 0.0 i_command_2 = 0.0 i_command_3 = 0.0 while t < 10.0: t += dt global_time.updateTime(t) desired = t measured = -2.0 * t p_command = p.update(desired, measured) i_command = i.update(desired, measured) d_command = d.update(desired, measured) pid_command = self.pid.update(desired, measured) self.assertAlmostEqual(p_command, kp * 3.0 * t) # Testing the integral here is hard because of the discrete-time # approximation. # 1st derivative is positive self.assertGreater(i_command, i_command_1) # 2nd derivative is positive i_diff = i_command - i_command_1 i_diff_1 = i_command_1 - i_command_2 self.assertGreater(i_diff, i_diff_1) # 3rd derivative is zero i_diff_2 = i_command_2 - i_command_3 self.assertAlmostEqual(i_diff - i_diff_1, i_diff_1 - i_diff_2, 5) i_command_3 = i_command_2 i_command_2 = i_command_1 i_command_1 = i_command self.assertAlmostEqual(d_command, kd * 3.0) self.assertAlmostEqual(pid_command, p_command + i_command + d_command)