error = z_target - z_actual
error_dot = z_dot_target - z_dot_actual
self.last_error = error
# u_bar is what we want vertical acceleration to be
u_bar = self.k_p * error + self.k_d * error_dot + z_dot_dot_ff
#u is the thrust command which will cause u_bar
u = self.vehicle_mass * (self.g - u_bar)
return u
testing.pd_controller_test(PDController)
def oscillating_target():
'''
returns sampling time, target path, target path velocity, target path acceleration
'''
AMPLITUDE = 0.5
OSCILLATION_FREQUENCY = 5
PERIOD = 2 * np.pi / OSCILLATION_FREQUENCY
t, z_path, z_dot_path, z_dot_dot_path = trajectories.cosine(AMPLITUDE,
PERIOD,
duration=6.0)
# modify the PD control code to incorporate
# the feedforward term.
# position error
err = z_target - z_actual
# velocity error
err_dot = z_dot_target - z_dot_actual
# desired acceleration
u_bar = self.k_p * err + self.k_d * err_dot + z_dot_dot_ff
# commanded thrust
u = self.vehicle_mass * (self.g - u_bar)
return u
testing.pd_controller_test(PDController, feed_forward=True)
# #### TODO 2 - Compare trajectories with and without a feedforward term
#
# The code below generates plots of $z$ vs $t$ for two drones. One uses FF and the other doesn't.
#
# Run the code and compare the two trajectories. What happens if you increase the oscillation frequency to 10? What happens if you decrease it to 2?
#
# You should notice a **lag** in the system response: the trajectory without the feedforward term should lag behind the desired trajectory in time. This effect diminishes as the oscillation frequency decreases.
# In[5]:
# This code simulates TWO drones. One uses the feed forward
# acceleration and the other doesn't. Note the difference in
# trajectories.
