# Get referenced inputs from signal generators
ref_input = sig_gen.getRefInputs(t)
# The dynamics of the model will be propagated in time by t_elapse
# at intervals of t_Ts.
t_temp = t +t_elapse
# import pdb; pdb.set_trace()
while t < t_temp:
states = dynam.Outputs() # Get current states
u = ctrl.getForces(ref_input,states) # Calculate the forces
u_converted = convertForces(u)
temp = convertForcesToPWM(u)
# print("left: %.2f right %.2f" % (temp[0], temp[1]))
dynam.propagateDynamics(u_converted) # Propagate the dynamics of the model in time
t = round(t +t_Ts,2) # Update time elapsed
plt.figure(simAnimation.fig.number) # Switch current figure to animation figure
simAnimation.drawWhirlybird( # Update animation with current user input
dynam.Outputs())
plt.pause(0.0001)
# Organizes the new data to be passed to plotGen
new_data = [[ref_input[0], states[1]],
[ref_input[0], states[0]],
[ref_input[0], states[2]],
[temp[0],temp[1]]]
plotGen.updateDataHistory(t, new_data)
plt.figure(plotGen.fig.number)
while t < t_end:
# Get referenced inputs from signal generators
ref_input = sig_gen.getRefInputs(t)
# The dynamics of the model will be propagated in time by t_elapse
# at intervals of t_Ts.
t_temp = t + t_elapse
while t < t_temp:
states = dynam.Outputs() # Get current states
u = ctrl.getForces(ref_input, states) # Calculate the forces
dynam.propagateDynamics(
[x * P.km
for x in u]) # Propagate the dynamics of the model in time
t = round(t + t_Ts, 2) # Update time elapsed
# plt.figure(simAnimation.fig.number) # Switch current figure to animation figure
# simAnimation.drawSystem( # Update animation with current user input
# dynam.Outputs())
# plt.pause(0.0001)
# Organizes the new data to be passed to plotGen
new_data = [
[ref_input[0], states[1]], # theta_r/theta
[states[0]], # phi
[ref_input[1], states[2]], # psi_r/psi
[u[0], u[1]]
] # u_l,u_r
user_input = Sliders()
simAnimation = WhirlybirdAnimation()
dynam = WhirlybirdDynamics()
t = t_start # Declare time variable to keep track of simulation time elapsed
while t < t_end:
plt.ion() # Make plots interactive
plt.figure(
user_input.fig.number) # Switch current figure to user_input figure
plt.pause(0.0001) # Pause the simulation to detect user input
# The dynamics of the model will be propagated in time by t_elapse
# at intervals of t_Ts.
t_temp = t + t_elapse
while t < t_temp:
u = convertForces( # Convert force and torque to fl and fr
user_input.getInputValues())
dynam.propagateDynamics(
u) # Propagate the dynamics of the model in time
t += t_Ts # Update time elapsed
plt.figure(
simAnimation.fig.number) # Switch current figure to animation figure
simAnimation.drawWhirlybird( # Update animation with current user input
dynam.Outputs())
t = t + t_elapse # Update animation with current user input
# time.sleep(t_pause)