def constant_target():
'''
returns sampling time, target path, target path velocity
'''
t, z_path, z_dot_path, z_dot_dot_path = trajectories.step()
return t, z_path, z_dot_path, z_dot_dot_path
testing.pid_controller_test(PIDController)
MASS_ERROR = 1.5
K_P = 20.0
K_D = 10.0
K_I = 0.0 # TODO - increase to 0.5, 1, 2, etc...
AMPLITUDE = 0.5
PERIOD = 0.4
# preparation
drone = Monorotor()
perceived_mass = drone.m * MASS_ERROR
controller = PIDController(K_P, K_D, K_I, perceived_mass)
t, z_path, z_dot_path = trajectories.step(duration=10.0)
dt = t[1] - t[0]
# run simulation
history = []
for z_target, z_dot_target in zip(z_path, z_dot_path):
z_actual = drone.z
z_dot_actual = drone.z_dot
u = controller.thrust_control(z_target, z_actual,
z_dot_target, z_dot_actual,
dt, z_dot_dot_ff)
drone.thrust = u