# 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)