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)
