import param as P from slider_input import Sliders # 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 Dynamics from animation import Animation t_start = 0.0 # Start time of simulation t_end = 50.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 user_input = Sliders() # Instantiate Sliders class simAnimation = Animation() # Instantiate Animate class dynam = Dynamics() # Instantiate Dynamics class 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.001) # 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
reference.pitch = ref_input[0] reference.yaw = ref_input[1] ref_input_pub.publish(reference) print("reference_input=%f", ref_input) u = ctrl.getForces(ref_input, states) # Calculate the forces (PWM output) # 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) ref_input_pub = rospy.Publisher('input', Whirlybird, queue_size=5) # Command Message object command = Command()
# 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 fl = 1.0 / 2.0 * F + 1.0 / (2 * P.d) * tau fr = 1.0 / 2.0 * F - 1.0 / (2 * P.d) * tau return [fl, fr] t_start = 0.0 # Start time of simulation t_end = 20.0 # End time of simulation t_Ts = P.Ts # Simulation time step t_elapse = 0.01 # Simulation time elapsed between each iteration t_pause = 0.01 # Pause between each iteration # Instantiate classes 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
import matplotlib.pyplot as plt import param as P # The Animation.py file is kept in the parent directory, # so the parent directory path needs to be added. sys.path.append('..') from animation import WhirlybirdAnimation t_start = 0.0 # Start time of simulation t_end = 20.0 # End time of simulation t_Ts = P.Ts # Simulation time step t_elapse = 0.01 # Simulation time elapsed between each iteration t_pause = 0.01 # Pause between each iteration # Instantiate classes user_input = Sliders() simAnimation = WhirlybirdAnimation() 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 plt.figure( simAnimation.fig.number) # Switch current figure to animation figure simAnimation.drawWhirlybird(user_input.getInputValues()) t = t + t_elapse # Update animation with current user input
# Calculate time elapsed since last function call, s global prev_time now = rospy.Time.now() dt = (now - prev_time).to_sec() prev_time = now # Update prev_time , time F_e = (P.m1 * P.l1 - P.m2 * P.l2) * P.g * np.cos(theta) / P.l1 print('theta: ', theta * 180.0 / np.pi) #print("km: ",F_e/(2.0*PWM) ) print("F_e: ", F_e) if __name__ == '__main__': # Instantiate slider object user_input = Sliders() # Create the node rospy.init_node('whirlybird_calibration', 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() # Used to calculate the time between the callback function calls. global prev_time
def convertForces(input): force = input[0] torque = input[1] fr = force / 2 - torque / 2 / P.d fl = force / 2 + torque / 2 / P.d return [fl, fr] t_start = 0.0 # Start time of simulation t_end = 50.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 user_input = Sliders() # Instantiate Sliders class simAnimation = Animation() # Instantiate Animate class dynam = Dynamics() # Instantiate Dynamics class 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.001) # 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