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()