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