Esempio n. 1
0
def f_7_d():
    pvt = Sim.PlanarVTOL()
    proportional_gain = 0.75
    derivative_gain = 0.09
    pvt.add_controller(PD.PlanarVTOL, proportional_gain, derivative_gain, 0, 0, 0, 0)
    request = zip(
        sg.generator(sg.constant, y_offset=1, t_step=pvt.seconds_per_sim_step, t_final=10),
        sg.generator(sg.constant, y_offset=1, t_step=pvt.seconds_per_sim_step, t_final=10)
    )
    handle = pvt.view_animation(request)
    Sim.Animations.plt.waitforbuttonpress()
    Sim.Animations.plt.close()
    return handle
Esempio n. 2
0
def e_8_f():
    bnb = Sim.BallAndBeam()
    max_force = 15
    rise_time_theta = 1

    beam_mass = bnb.dynamics.beam_mass
    ball_mass = bnb.dynamics.ball_mass
    beam_length = bnb.dynamics.beam_length
    gravity = bnb.dynamics.gravity

    c_theta = 1 / (beam_length * (ball_mass / 4 + beam_mass / 3))
    natural_frequency_theta = np.pi / (2 * rise_time_theta * (1 - damping_ratio**2)**(1/2))
    proportional_gain_theta = natural_frequency_theta**2 / c_theta
    derivative_gain_theta = 2 * damping_ratio * natural_frequency_theta / c_theta

    rise_time_z = 10 * rise_time_theta
    natural_frequency_z = np.pi / (2 * rise_time_z * (1 - damping_ratio**2)**(1/2))
    proportional_gain_z = natural_frequency_z**2 / -gravity
    derivative_gain_z = 2 * damping_ratio * natural_frequency_z / -gravity

    bnb.add_controller(PD.BallAndBeam,
                       proportional_gain_z, derivative_gain_z,
                       proportional_gain_theta, derivative_gain_theta, max_force)
    # a square wave with magnitude 0.25±0.15 meters and frequency 0.01 Hz
    requests = sg.generator(sg.constant, amplitude=0.0, t_step=bnb.seconds_per_sim_step, t_final=10)
    handle = bnb.view_animation(requests)
    Sim.Animations.plt.waitforbuttonpress()
    Sim.Animations.plt.close()
    return handle
Esempio n. 3
0
def f_8_a():
    pvt = Sim.PlanarVTOL()
    rise_time = 8
    natural_frequency = np.pi / (2 * rise_time * (1 - damping_ratio**2)**(1/2))
    c_h = 1 / (pvt.dynamics.center_mass + 2 * pvt.dynamics.wing_mass)
    proportional_gain_h = natural_frequency**2 / c_h
    derivative_gain_h = 2 * damping_ratio * natural_frequency / c_h
    pvt.add_controller(PD.PlanarVTOL, proportional_gain_h, derivative_gain_h, 0, 0, 0, 0)
    request = zip(
        sg.generator(sg.constant, y_offset=1, t_step=pvt.seconds_per_sim_step, t_final=10),
        sg.generator(sg.sin, frequency=0.01, t_step=pvt.seconds_per_sim_step, t_final=10),
    )
    handle = pvt.view_animation(request)
    Sim.Animations.plt.waitforbuttonpress()
    Sim.Animations.plt.close()
    return handle
Esempio n. 4
0
def d_7_d():
    msd = Sim.MassSpringDamper()
    proportional_gain = 4.5
    derivative_gain = 12
    msd.add_controller(PD.MassSpringDamper, proportional_gain, derivative_gain)
    request = sg.generator(sg.constant, t_step=msd.seconds_per_sim_step, t_final=5)
    handle = msd.view_animation(request)
    Sim.Animations.plt.waitforbuttonpress()
    Sim.Animations.plt.close()
    return handle
Esempio n. 5
0
def e_12():
    print('\n--E--\n')
    bnb = Sim.BallAndBeam()
    max_force = 15
    damping_ratio = 1 / (2**(1 / 2))

    #  Tuning variables
    rise_time_theta = .5
    bandwidth_separation = 2.4
    integrator_pole = 2.

    natural_frequency_theta = np.pi / (2 * rise_time_theta *
                                       (1 - damping_ratio**2)**(1 / 2))
    rise_time_z = rise_time_theta * bandwidth_separation
    natural_frequency_z = np.pi / (2 * rise_time_z *
                                   (1 - damping_ratio**2)**(1 / 2))

    coefs_theta = [
        1, 2 * damping_ratio * natural_frequency_theta,
        natural_frequency_theta**2
    ]
    coefs_z = [
        1, 2 * damping_ratio * natural_frequency_z, natural_frequency_z**2
    ]
    coefs_I = [1, integrator_pole]
    roots = np.roots(np.convolve(coefs_I, np.convolve(coefs_theta, coefs_z)))

    A = bnb.dynamics.A
    B = bnb.dynamics.B
    C = bnb.dynamics.C

    A_I, B_I = augment_matrices(A, B, C[0:1, :])

    gains = ctrl.place(A_I, B_I, roots)
    dim = A.shape[1]
    feedback_gain = gains[0:1, 0:dim]
    integrator_gain = gains[0, dim]
    print(
        f'Feedback Gain: {feedback_gain}\nIntegrator Gain: {integrator_gain}')

    bnb.add_controller(Control.BallAndBeam,
                       feedback_gain,
                       integrator_gain,
                       max_force=max_force)
    requests = sg.generator(sg.square,
                            amplitude=0.15,
                            y_offset=0.25,
                            frequency=0.1,
                            t_step=bnb.seconds_per_sim_step,
                            t_final=90)

    handle = bnb.view_animation(requests)
    Sim.Animations.plt.waitforbuttonpress()
    Sim.Animations.plt.close()
    return handle
Esempio n. 6
0
def f_8_g():
    pvt = Sim.PlanarVTOL()
    rise_time_h = 8
    max_force = 10
    natural_frequency_h = np.pi / (2 * rise_time_h * (1 - damping_ratio**2)**(1/2))
    c_h = 1 / (pvt.dynamics.center_mass + 2 * pvt.dynamics.wing_mass)
    proportional_gain_h = natural_frequency_h**2 / c_h
    derivative_gain_h = 2 * damping_ratio * natural_frequency_h / c_h

    rise_time_theta = 0.8
    natural_frequency_theta = np.pi / (2 * rise_time_theta * (1 - damping_ratio**2)**(1/2))
    j_center = pvt.dynamics.center_moi
    wing_mass = pvt.dynamics.wing_mass
    wing_spacing = pvt.dynamics.wing_spacing
    c_theta = 1 / (j_center + 2 * wing_mass * wing_spacing**2)

    proportional_gain_theta = natural_frequency_theta**2 / c_theta
    derivative_gain_theta = 2 * damping_ratio * natural_frequency_theta / c_theta

    rise_time_z = 10 * rise_time_theta
    natural_frequency_z = np.pi / (2 * rise_time_z * (1 - damping_ratio**2)**(1/2))
    mu = pvt.dynamics.drag
    gravity = pvt.dynamics.gravity

    proportional_gain_z = natural_frequency_z**2 / -gravity
    derivative_gain_z = (2 * damping_ratio * natural_frequency_z - mu * c_h) / -gravity

    pvt.add_controller(PD.PlanarVTOL,
                       proportional_gain_h, derivative_gain_h,
                       proportional_gain_z, derivative_gain_z,
                       proportional_gain_theta, derivative_gain_theta,
                       max_force)
    request = zip(
        sg.generator(sg.constant, y_offset=0, t_step=pvt.seconds_per_sim_step, t_final=10),
        # a square wave with magnitude 3 ± 2.5 meters and frequency 0.08 Hz
        sg.generator(sg.sin, frequency=0.08, y_offset=3, amplitude=2.5, t_step=pvt.seconds_per_sim_step, t_final=10),
    )
    handle = pvt.view_animation(request)
    Sim.Animations.plt.waitforbuttonpress()
    Sim.Animations.plt.close()
    return handle
Esempio n. 7
0
def d_8_a():
    msd = Sim.MassSpringDamper()
    rise_time = 2
    natural_frequency = np.pi / (2 * rise_time * (1 - damping_ratio**2)**(1/2))
    proportional_gain = 5 * (natural_frequency**2 - 0.6)
    derivative_gain = 5 * (2 * damping_ratio * natural_frequency - 0.1)
    msd.add_controller(PD.MassSpringDamper, proportional_gain, derivative_gain)
    request = sg.generator(sg.constant, t_step=msd.seconds_per_sim_step, t_final=5)
    handle = msd.view_animation(request)
    Sim.Animations.plt.waitforbuttonpress()
    Sim.Animations.plt.close()
    return handle
Esempio n. 8
0
def d_11():
    print('\n--D--\n')
    msd = Sim.MassSpringDamper()
    max_force = 2.0
    damping_ratio = 1 / (2**(1 / 2))

    rise_time = 2.0

    # mass = msd.dynamics.mass
    # spring_const = msd.dynamics.spring_const
    # natural_frequency = ((spring_const + max_force) / mass) ** (1 / 2)
    natural_frequency = np.pi / (2 * rise_time *
                                 (1 - damping_ratio**2)**(1 / 2))

    # Part A
    coefs = [1, 2 * damping_ratio * natural_frequency,
             natural_frequency**2]  # Small error
    roots = np.roots(coefs)
    print(f"Roots: {roots}")

    # Part B
    A = msd.dynamics.A
    B = msd.dynamics.B
    C = msd.dynamics.C

    # Part C
    control_mat = ctrl.ctrb(A, B)
    rank = np.linalg.matrix_rank(control_mat)
    order = A.shape[0]
    print(f"controllability matrix rank: {rank}")
    print(f"System order: {order}")
    print(f"{'Controllable' if rank == order else 'Not Controllable'}")

    # Part D
    feedback_gain = ctrl.place(A, B, roots)
    print(f'Feedback Gain: {feedback_gain}')

    reference_gain = -1 / (C @ np.linalg.inv(A - B @ feedback_gain) @ B)
    print(f"Reference gain: {reference_gain}")

    # Part E
    msd.add_controller(SSController.MassSpringDamper, feedback_gain,
                       reference_gain, max_force)
    request = sg.generator(sg.constant,
                           amplitude=2 / 3,
                           t_step=msd.seconds_per_sim_step,
                           t_final=20)
    handle = msd.view_animation(request)
    Sim.Animations.plt.waitforbuttonpress()
    Sim.Animations.plt.close()

    return handle
Esempio n. 9
0
def e_10():
    bnb = Sim.BallAndBeam()
    max_force = 15.0
    damping_ratio = 1 / (2**(1 / 2)) + 0.2

    length = bnb.dynamics.beam_length
    ball_mass = bnb.dynamics.ball_mass
    beam_mass = bnb.dynamics.beam_mass
    z_equilibrium = length / 2
    gravity = bnb.dynamics.gravity
    b_theta = length / (
        (beam_mass * length**2) / 3 + ball_mass * z_equilibrium**2)

    #  Tuning variables
    rise_time_theta = .5
    bandwidth_separation = 10
    integration_gain_theta = .01
    integration_gain_z = 0.001
    threshold_theta = 0.0005
    threshold_z = 0.02

    natural_frequency_theta = np.pi / (2 * rise_time_theta *
                                       (1 - damping_ratio**2)**(1 / 2))
    proportional_gain_theta = natural_frequency_theta**2 / b_theta
    derivative_gain_theta = 2 * damping_ratio * natural_frequency_theta / b_theta

    rise_time_z = rise_time_theta * bandwidth_separation
    natural_frequency_z = np.pi / (2 * rise_time_z *
                                   (1 - damping_ratio**2)**(1 / 2))
    proportional_gain_z = -natural_frequency_z**2 / gravity
    derivative_gain_z = -2 * damping_ratio * natural_frequency_z / gravity

    bnb.add_controller(PID.BallAndBeam, proportional_gain_z, derivative_gain_z,
                       integration_gain_z, proportional_gain_theta,
                       derivative_gain_theta, integration_gain_theta,
                       threshold_z, threshold_theta, max_force)

    # a square wave with magnitude 0.25±0.15 meters and frequency 0.05 Hz
    requests = sg.generator(sg.square,
                            amplitude=0.15,
                            y_offset=0.25,
                            frequency=0.05,
                            t_step=bnb.seconds_per_sim_step,
                            t_final=20)
    handle = bnb.save_movie(requests, 'out/HW7E.mp4')
    # Sim.Animations.plt.waitforbuttonpress()
    # Sim.Animations.plt.close()
    return handle
Esempio n. 10
0
def d_12():
    print('\n--D--\n')
    msd = Sim.MassSpringDamper()
    max_force = 2.0
    damping_ratio = 1 / (2**(1 / 2))

    rise_time = 2.0

    # mass = msd.dynamics.mass
    # spring_const = msd.dynamics.spring_const
    # natural_frequency = ((spring_const + max_force) / mass) ** (1 / 2)
    natural_frequency = np.pi / (2 * rise_time *
                                 (1 - damping_ratio**2)**(1 / 2))

    coefs = [1, 2 * damping_ratio * natural_frequency, natural_frequency**2]
    integrator_root = 1.0
    roots = np.roots(np.convolve(coefs, [1, integrator_root]))
    print(f"Roots: {roots}")

    # Part B
    A = msd.dynamics.A
    B = msd.dynamics.B
    C = msd.dynamics.C

    A_I, B_I = augment_matrices(A, B, C)

    gains = ctrl.place(A_I, B_I, roots)
    feedback_gain = gains[:, 0:2]
    integrator_gain = gains[0, 2]
    print(
        f'Feedback Gain: {feedback_gain}\nIntegrator Gain: {integrator_gain}')

    msd.add_controller(Control.MassSpringDamper, feedback_gain,
                       integrator_gain, max_force)
    request = sg.generator(sg.constant,
                           amplitude=2 / 3,
                           t_step=msd.seconds_per_sim_step,
                           t_final=20)
    handle = msd.view_animation(request)
    Sim.Animations.plt.waitforbuttonpress()
    Sim.Animations.plt.close()

    return handle
Esempio n. 11
0
def d_10():
    msd = Sim.MassSpringDamper()
    max_force = 2.0
    damping_ratio = 1 / (2**(1 / 2))

    proportional_gain = max_force
    natural_frequency = ((msd.dynamics.spring_const + proportional_gain) /
                         msd.dynamics.mass)**(1 / 2)
    derivative_gain = msd.dynamics.mass * 2 * damping_ratio * natural_frequency - msd.dynamics.damping

    integrator_gain = 1

    msd.add_controller(PID.MassSpringDamper, proportional_gain,
                       derivative_gain, integrator_gain, max_force)

    request = sg.generator(sg.constant,
                           amplitude=2 / 3,
                           t_step=msd.seconds_per_sim_step,
                           t_final=20)

    handle = msd.save_movie(request, 'out/HW7D.mp4')
    # Sim.Animations.plt.waitforbuttonpress()
    # Sim.Animations.plt.close()
    return handle
Esempio n. 12
0
def f_12():
    print('\n--F--\n')
    pvt = Sim.PlanarVTOL()
    max_force = 10
    damping_ratio = 1 / (2**(1 / 2)) + 0.2

    # Tuning variable
    rise_time_h = 2.5
    rise_time_theta = 0.1
    bandwidth_separation = 7
    integrator_pole_lat = 1.5
    integrator_pole_lon = 1

    natural_frequency_h = np.pi / (2 * rise_time_h *
                                   (1 - damping_ratio**2)**(1 / 2))
    natural_frequency_theta = np.pi / (2 * rise_time_theta *
                                       (1 - damping_ratio**2)**(1 / 2))
    rise_time_z = bandwidth_separation * rise_time_theta
    natural_frequency_z = np.pi / (2 * rise_time_z *
                                   (1 - damping_ratio**2)**(1 / 2))

    coefs_theta = [
        1, 2 * damping_ratio * natural_frequency_theta,
        natural_frequency_theta**2
    ]
    coefs_z = [
        1, 2 * damping_ratio * natural_frequency_z, natural_frequency_z**2
    ]
    coefs_I_lat = [1, integrator_pole_lat]
    roots_lat = np.roots(
        np.convolve(coefs_I_lat, np.convolve(coefs_theta, coefs_z)))
    print(f"Latitudinal roots: {roots_lat}")

    coefs_h = [
        1, 2 * damping_ratio * natural_frequency_h, natural_frequency_h * 2
    ]
    coefs_I_lon = [1, integrator_pole_lon]
    roots_lon = np.roots(np.convolve(coefs_h, coefs_I_lon))
    print(f"Longitudinal roots: {roots_lon}")

    # Part B
    A_lat = pvt.dynamics.A_lat
    B_lat = pvt.dynamics.B_lat
    C_lat = pvt.dynamics.C_lat

    A_I_lat, B_I_lat = augment_matrices(A_lat, B_lat, C_lat[0:1, :])

    A_lon = pvt.dynamics.A_lon
    B_lon = pvt.dynamics.B_lon
    C_lon = pvt.dynamics.C_lon

    A_I_lon, B_I_lon = augment_matrices(A_lon, B_lon, C_lon)

    gains_lat = ctrl.place(A_I_lat, B_I_lat, roots_lat)
    dim_lat = A_lat.shape[1]
    feedback_gain_lat = gains_lat[0:1, 0:dim_lat]
    int_gain_lat = gains_lat[0, dim_lat]
    print(
        f'Latitudinal feedback Gain: {feedback_gain_lat}\nLatitudinal integrator gain: {int_gain_lat}'
    )

    gains_lon = ctrl.place(A_I_lon, B_I_lon, roots_lon)
    dim_lon = A_lon.shape[1]
    feedback_gain_lon = gains_lon[0:1, 0:dim_lon]
    int_gain_lon = gains_lon[0, dim_lon]
    print(
        f'Longitudinal feedback Gain: {feedback_gain_lon}\nLongitudinal integrator gain: {int_gain_lon}'
    )

    request = zip(
        sg.generator(sg.constant,
                     y_offset=1,
                     t_step=pvt.seconds_per_sim_step,
                     t_final=20),
        sg.generator(sg.sin,
                     frequency=0.08,
                     y_offset=0,
                     amplitude=2.5,
                     t_step=pvt.seconds_per_sim_step,
                     t_final=20),
    )

    pvt.add_controller(Control.PlanarVTOL, feedback_gain_lat, int_gain_lat,
                       feedback_gain_lon, int_gain_lon, max_force)

    handel = pvt.view_animation(request)
    Sim.Animations.plt.waitforbuttonpress()
    Sim.Animations.plt.close()
    return handel
Esempio n. 13
0
def e_11():
    print('\n--E--\n')
    bnb = Sim.BallAndBeam()
    max_force = 15
    damping_ratio = 1 / (2**(1 / 2))

    #  Tuning variables
    rise_time_theta = .5
    bandwidth_separation = 2.4

    natural_frequency_theta = np.pi / (2 * rise_time_theta *
                                       (1 - damping_ratio**2)**(1 / 2))
    rise_time_z = rise_time_theta * bandwidth_separation
    natural_frequency_z = np.pi / (2 * rise_time_z *
                                   (1 - damping_ratio**2)**(1 / 2))

    # Part A
    coefs_theta = [
        1, 2 * damping_ratio * natural_frequency_theta,
        natural_frequency_theta**2
    ]
    coefs_z = [
        1, 2 * damping_ratio * natural_frequency_z, natural_frequency_z**2
    ]
    roots = np.concatenate((
        np.roots(coefs_theta),
        np.roots(coefs_z),
    ))

    # Part B
    A = bnb.dynamics.A
    B = bnb.dynamics.B
    C = bnb.dynamics.C

    # Part C
    control_mat = ctrl.ctrb(A, B)
    rank = np.linalg.matrix_rank(control_mat)
    order = A.shape[0]
    print(f"controllability matrix rank: {rank}")
    print(f"System order: {order}")
    print(f"{'Controllable' if rank == order else 'Not Controllable'}")

    # Part D
    feedback_gain = ctrl.place(A, B, roots)
    print(f'Feedback Gain: {feedback_gain}')

    reference_gain = -1 / (C[0] @ np.linalg.inv(A - B @ feedback_gain) @ B)
    print(f"Reference gain: {reference_gain}")

    # Part E
    bnb.add_controller(SSController.BallAndBeam, feedback_gain, reference_gain,
                       max_force)
    requests = sg.generator(sg.square,
                            amplitude=0.15,
                            y_offset=0.25,
                            frequency=0.05,
                            t_step=bnb.seconds_per_sim_step,
                            t_final=20)

    handle = bnb.view_animation(requests)
    Sim.Animations.plt.waitforbuttonpress()
    Sim.Animations.plt.close()
    return handle
Esempio n. 14
0
def f_11():
    print('\n--F--\n')
    pvt = Sim.PlanarVTOL()
    max_force = 10
    damping_ratio = 1 / (2**(1 / 2)) + 0.2

    # Tuning variable
    rise_time_h = 2.5
    rise_time_theta = 0.1
    bandwidth_separation = 7

    natural_frequency_h = np.pi / (2 * rise_time_h *
                                   (1 - damping_ratio**2)**(1 / 2))
    natural_frequency_theta = np.pi / (2 * rise_time_theta *
                                       (1 - damping_ratio**2)**(1 / 2))
    rise_time_z = bandwidth_separation * rise_time_theta
    natural_frequency_z = np.pi / (2 * rise_time_z *
                                   (1 - damping_ratio**2)**(1 / 2))

    # Part A
    coefs_theta = [
        1, 2 * damping_ratio * natural_frequency_theta,
        natural_frequency_theta**2
    ]
    coefs_z = [
        1, 2 * damping_ratio * natural_frequency_z, natural_frequency_z**2
    ]
    roots_lat = np.concatenate((
        np.roots(coefs_theta),
        np.roots(coefs_z),
    ))
    print(f"Latitudinal roots: {roots_lat}")

    coefs_h = [
        1, 2 * damping_ratio * natural_frequency_h, natural_frequency_h * 2
    ]
    roots_lon = np.roots(coefs_h)
    print(f"Longitudinal roots: {roots_lon}")

    # Part B
    A_lat = pvt.dynamics.A_lat
    B_lat = pvt.dynamics.B_lat
    C_lat = pvt.dynamics.C_lat

    A_lon = pvt.dynamics.A_lon
    B_lon = pvt.dynamics.B_lon
    C_lon = pvt.dynamics.C_lon

    # Part C
    control_mat_lat = ctrl.ctrb(A_lat, B_lat)
    rank = np.linalg.matrix_rank(control_mat_lat)
    order = A_lat.shape[0]
    print(f"Latitudinal controllability matrix rank: {rank}")
    print(f"Latitudinal system order: {order}")
    print(f"{'Controllable' if rank == order else 'Not Controllable'}")

    control_mat_lon = ctrl.ctrb(A_lon, B_lon)
    rank = np.linalg.matrix_rank(control_mat_lon)
    order = A_lon.shape[0]
    print(f"Longitudinal Controllability matrix rank: {rank}")
    print(f"Longitudinal system order: {order}")
    print(f"{'Controllable' if rank == order else 'Not Controllable'}")

    # Part D
    feedback_gain_lat = ctrl.place(A_lat, B_lat, roots_lat)
    print(f'Latitudinal feedback Gain: {feedback_gain_lat}')

    reference_gain_lat = -1 / (
        C_lat[0] @ np.linalg.inv(A_lat - B_lat @ feedback_gain_lat) @ B_lat)
    print(f"Latitudinal reference gain: {reference_gain_lat}")

    feedback_gain_lon = ctrl.place(A_lon, B_lon, roots_lon)
    print(f'Longitudinal feedback Gain: {feedback_gain_lon}')

    reference_gain_lon = -1 / (
        C_lon @ np.linalg.inv(A_lon - B_lon @ feedback_gain_lon) @ B_lon)
    print(f"Longitudinal reference gain: {reference_gain_lon}")

    # Part E
    request = zip(
        sg.generator(sg.constant,
                     y_offset=1,
                     t_step=pvt.seconds_per_sim_step,
                     t_final=20),
        sg.generator(sg.sin,
                     frequency=0.08,
                     y_offset=0,
                     amplitude=2.5,
                     t_step=pvt.seconds_per_sim_step,
                     t_final=20),
    )

    pvt.add_controller(SSController.PlanarVTOL, feedback_gain_lat,
                       reference_gain_lat, feedback_gain_lon,
                       reference_gain_lon, max_force)

    handel = pvt.view_animation(request)
    Sim.Animations.plt.waitforbuttonpress()
    Sim.Animations.plt.close()
    return handel
Esempio n. 15
0
def f_10():
    pvt = Sim.PlanarVTOL()
    max_force = 10
    damping_ratio = 1 / (2**(1 / 2)) + 0.2

    # Tuning variable
    rise_time_h = 2.5
    rise_time_theta = 0.1
    bandwidth_separation = 7
    integration_gain_h = 0.0001
    integration_gain_z = 0.01
    integration_gain_theta = 0.001
    threshold_h = 0.01
    threshold_z = 0.01
    threshold_theta = 0.01

    natural_frequency_h = np.pi / (2 * rise_time_h *
                                   (1 - damping_ratio**2)**(1 / 2))
    c_h = 1 / (pvt.dynamics.center_mass + 2 * pvt.dynamics.wing_mass)
    proportional_gain_h = natural_frequency_h**2 / c_h
    derivative_gain_h = 2 * damping_ratio * natural_frequency_h / c_h

    natural_frequency_theta = np.pi / (2 * rise_time_theta *
                                       (1 - damping_ratio**2)**(1 / 2))
    j_center = pvt.dynamics.center_moi
    wing_mass = pvt.dynamics.wing_mass
    wing_spacing = pvt.dynamics.wing_spacing
    c_theta = 1 / (j_center + 2 * wing_mass * wing_spacing**2)

    proportional_gain_theta = natural_frequency_theta**2 / c_theta
    derivative_gain_theta = 2 * damping_ratio * natural_frequency_theta / c_theta

    rise_time_z = bandwidth_separation * rise_time_theta
    natural_frequency_z = np.pi / (2 * rise_time_z *
                                   (1 - damping_ratio**2)**(1 / 2))
    mu = pvt.dynamics.drag
    gravity = pvt.dynamics.gravity

    proportional_gain_z = natural_frequency_z**2 / -gravity
    derivative_gain_z = (2 * damping_ratio * natural_frequency_z -
                         mu * c_h) / -gravity

    pvt.add_controller(PID.PlanarVTOL, proportional_gain_h, derivative_gain_h,
                       integration_gain_h, proportional_gain_z,
                       derivative_gain_z, integration_gain_z,
                       proportional_gain_theta, derivative_gain_theta,
                       integration_gain_theta, threshold_h, threshold_z,
                       threshold_theta, max_force)
    request = zip(
        sg.generator(sg.constant,
                     y_offset=1,
                     t_step=pvt.seconds_per_sim_step,
                     t_final=20),
        sg.generator(sg.sin,
                     frequency=0.08,
                     y_offset=0,
                     amplitude=2.5,
                     t_step=pvt.seconds_per_sim_step,
                     t_final=20),
    )
    handle = pvt.save_movie(request, 'out/HW7F.mp4')
    # Sim.Animations.plt.waitforbuttonpress()
    # Sim.Animations.plt.close()
    return handle