# This code simulates TWO drones. One uses the feed forward # acceleration and the other doesn't. Note the difference in # trajectories. MASS_ERROR = 1.0 K_P = 20.0 K_D = 8.0 AMPLITUDE = 0.5 OSCILLATION_FREQUENCY = 5 PERIOD = 2 * np.pi / OSCILLATION_FREQUENCY # preparation (TWO drones to compare) drone = Monorotor() ff_drone = Monorotor() perceived_mass = drone.m * MASS_ERROR # instantiate TWO controllers controller = PDController(K_P, K_D, perceived_mass) ff_controller = PDController(K_P, K_D, perceived_mass) # get trajectories t, z_path, z_dot_path, z_dot_dot_path = trajectories.cosine(AMPLITUDE, PERIOD, duration=6.0) dt = t[1] - t[0] # run simulation history = [] ff_history = []
t = np.linspace(0.0, total_time, 1000) dt = t[1] - t[0] z_path = 0.5 * np.cos(2 * t) - 0.5 plt.figure(figsize=(5, 5)) plt.ylabel("z (meters)") plt.xlabel("time (seconds)") plt.gca().invert_yaxis() plt.plot(t, z_path) plt.show() # 1. Preparation for simulation MASS_ERROR = 1.01 drone = Monorotor() drone_start_state = drone.X drone_mass = drone.m # The mass that the controller believes is not necessarily the # true mass of the drone. This reflects the real world more accurately. perceived_mass = drone_mass * MASS_ERROR controller = OpenLoopController(perceived_mass, drone_start_state) # 2. Run the simulation drone_state_history = [] for target_z in z_path: drone_state_history.append(drone.X) thrust = controller.thrust_control(target_z, dt) drone.thrust = thrust drone.advance_state(dt)