drone = Drone2D()
# INSTANTIATE CONTROLLER
linear_controller = LinearCascadingController(drone.m,
drone.I_x,
z_k_p=z_k_p,
z_k_d=z_k_d,
y_k_p=y_k_p,
y_k_d=y_k_d,
phi_k_p=phi_k_p,
phi_k_d=phi_k_d)
# TRAJECTORY PARAMETERS (you don't need to change these)
total_time = 100.0
omega_z = 1.0 # angular frequency of figure 8
# GENERATE FIGURE 8
z_traj, y_traj, t = trajectories.figure_8(omega_z, total_time, dt=0.02)
z_path, z_dot_path, z_dot_dot_path = z_traj
y_path, y_dot_path, y_dot_dot_path = y_traj
# SIMULATE MOTION
linear_history = simulate.zy_flight(z_traj,
y_traj,
t,
linear_controller,
inner_loop_speed_up=10)
# PLOT RESULTS
plotting.plot_zy_flight_path(z_path, y_path, linear_history)
drone.I_x,
z_k_p=z_k_p,
z_k_d=z_k_d,
y_k_p=y_k_p,
y_k_d=y_k_d,
phi_k_p=phi_k_p,
phi_k_d=phi_k_d)
# GENERATE FIGURE 8
z_traj, y_traj, t = trajectories.figure_8(omega_z, total_time, dt=0.02)
dt = t[1] - t[0]
# SIMULATE MOTION
linear_history = simulate.zy_flight(z_traj,
y_traj,
t,
linear_controller,
inner_loop_speed_up=SPEED_UP)
non_linear_history = simulate.zy_flight(z_traj,
y_traj,
t,
non_linear_controller,
inner_loop_speed_up=SPEED_UP)
# PLOT RESULTS
pylab.rcParams['figure.figsize'] = 10, 10
plotting.compare_flight_paths(z_traj[0], y_traj[0], linear_history,
non_linear_history, "Linear Controller",
"Non-Linear Controller")
# In[3]:
