from sim_plot import plotGenerator
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
# Get referenced inputs from signal generators or sliders
ref_input = usr_input.getInputValues(
) if SLIDERS else usr_input.getRefInputs(sim_time)
# ref_input = [0.0, 0.0]
u = ctrl.getForces(ref_input, states) # Calculate the forces
u = convertForces(u) # Convert forces to PWM
command.left_motor = u[0]
command.right_motor = u[1]
command_pub.publish(command)
if __name__ == '__main__':
usr_input = Sliders() if SLIDERS else Signals()
ctrl = ctrl() # Instantiate controllerPD class
# Create the node
rospy.init_node('whirlybird_controller', anonymous=False)
# Subscriber to topic 'whirlybird'
rospy.Subscriber('whirlybird', Whirlybird, callback)
# Publisher to topic 'command'
command_pub = rospy.Publisher('command', Command, queue_size=5)
# Command Message object
command = Command()