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)