from controllerPD import controllerPD
# The Animation.py file is kept in the parent directory,
# so the parent directory path needs to be added.
sys.path.append('..')
from dynamics import WhirlybirdDynamics
from animation import WhirlybirdAnimation
t_start = 0.0 # Start time of simulation
t_end = 40.0 # End time of simulation
t_Ts = P.Ts # Simulation time step
t_elapse = 0.1 # Simulation time elapsed between each iteration
t_pause = 0.01 # Pause between each iteration
sig_gen = Signals() # Instantiate Signals class
plotGen = plotGenerator() # Instantiate plotGenerator class
ctrl = controllerPD() # Instantiate controllerPD class
simAnimation = WhirlybirdAnimation() # Instantiate Animate class
dynam = WhirlybirdDynamics() # Instantiate Dynamics class
t = t_start # Declare time variable to keep track of simulation time elapsed
# Converts force and torque into the left and
# right forces produced by the propellers.
def convertForces(u):
F = u[0] # Force, N
tau = u[1] # Torque, Nm
# Convert Force and Torque to fl and fr
# fl is the force created by the left propeller
# fr is the force created by the right propeller
pwml = (1/(2*P.km))*(F + tau / P.d)
pwmr = (1/(2*P.km))*(F - tau / P.d)
if pwml >= 0.6:
pwml = 0.6
if pwmr >= 0.6:
pwmr = 0.6
return [pwml,pwmr]
t_start = 0.0 # Start time of simulation
t_end = 15.0 # End time of simulation
t_Ts = P.Ts # Simulation time step
t_elapse = 0.1 # Simulation time elapsed between each iteration
t_pause = 0.01 # Pause between each iteration
plotGen = plotGenerator()
sig_gen = Signals() # Instantiate Signals class
simAnimation = WhirlybirdAnimation() # Instantiate Animate class
dynam = WhirlybirdDynamics() # Instantiate Dynamics class
ctrl = controllerPID()
t = t_start # Declare time variable to keep track of simulation time elapsed
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
# import pdb; pdb.set_trace()
