monorotor.advance_state(dt)
history.append(monorotor.X)
actual_path = [h[0] for h in history]
return t, z_path, actual_path
if __name__ == '__main__':
MASS_ERROR = 1.0
K_P = 20.0
K_D = 2.0
drone = Monorotor()
controller = PDController(K_P, K_D, drone.m * MASS_ERROR)
t, target_path, actual_path = proceed(drone, controller,
oscillating_target)
drone = Monorotor()
controller = PDController(K_P, K_D, drone.m * MASS_ERROR)
t, target_path, actual_path_ff = proceed(drone, controller,
oscillating_target, True)
plotting.compare_planned_to_actual(actual_path, target_path, t,
actual_path_ff)
# This code simulates TWO drones. One uses the feed forward
# acceleration and the other doesn't. Note the difference in
# trajectories.
history = []
ff_history = []
for z_target, z_dot_target, z_dot_dot_ff in zip(z_path, z_dot_path,
z_dot_dot_path):
z_actual = drone.z
z_dot_actual = drone.z_dot
ff_z_actual = ff_drone.z
ff_z_dot_actual = ff_drone.z_dot
u_ff = controller.thrust_control(z_target, ff_z_actual, z_dot_target,
ff_z_dot_actual, z_dot_dot_ff)
u = controller.thrust_control(z_target, z_actual, z_dot_target,
z_dot_actual)
drone.thrust = u
ff_drone.thrust = u_ff
drone.advance_state(dt)
ff_drone.advance_state(dt)
history.append(drone.X)
ff_history.append(ff_drone.X)
# generate plots
z_actual = [h[0] for h in history]
z_ff_actual = [h[0] for h in ff_history]
plotting.compare_planned_to_actual(z_actual, z_path, t, z_ff_actual)
# [Solution](/notebooks/PD%20with%20FF%20Solution.ipynb)
