def main():
speed = -.5 # m/s
min_altitude = 0.8 # mm
max_altitudeGoal = 2.5# mm
height_diff = 0 #mm
timeout = 300# seconds
# pid_i = (kp, ki, kd, integrator, derivator, set_point)
pid_x = (6, 0.625, 2.5, 0, 0, 500)
pid_y = (6, 0.875, 3.75, 0, 0, 500)
pid_z = (1,0,0,0,0,2.5)
pid_theta = (1/260,0,0,0,0,0)
# set the bounding box
bbx = (375, 625)
bby = (375, 625)
bounding_box = True
bbx_min, bbx_max = bbx
bby_min, bby_max = bby
yaw_min = 10
yaw_max = 350
# controller update values
yaw_update = 0
x_update = 0
y_update = 0
z_update = 0
navdata = nd()
# Twist commands
qc = Twist()
land = Landing(speed, min_altitude, height_diff, max_altitudeGoal, \
timeout, bbx, bby, pid_x, pid_y, pid_z, pid_theta)
rate = rospy.Rate(200)
while((land.state != 'land')):
# print takeoff.state
rate.sleep()
while not rospy.is_shutdown():
# always update the altitude
# z_update = land.pid_z.update(z_update)
# print("Theta %.2f" % navdata.theta)
# print("(%d, %d)" % (navdata.tag_x, navdata.tag_y))
if (navdata.tag_acquired):
# If 10 < theta < 350 then let's rotate
# if ((yaw_min < navdata.theta) and (navdata.theta < yaw_max)):
if ((yaw_min < navdata.theta < yaw_max)):
print "Yaw!"
yaw_update = land.pid_theta.update(navdata.theta)
else:
print "No yaw!"
yaw_update = 0
# is_in_box(minimum, maximum, position)
if (land.is_in_box(bbx_min, bbx_max, navdata.tag_y) and land.is_in_box(bby_min, bby_max, navdata.tag_x)):
# If the QC is in the bounding box then we should enter 'Hover'
# mode and just hang there
x_update = 0
y_update = 0
# # qc.angular.x = 0.0
# qc.angular.y = 0.0
# print("In the Box")
else:
# It's not in the bounding box therefore we should update the PIDs
x_update = land.pid_x.update(navdata.tag_x)
y_update = land.pid_y.update(navdata.tag_y)
# print("%.3f" % land.pid_x.getError())
# always update the altitude
height_diff = land.max_altitudeGoal - land.altitude
# We only want to execute these manuevers if we're in land mode
if land.state == 'land':
# print("%.3f m" % land.altitude)
if(land.altitude > land.min_altitude):
print("Go down")
# qc.linear.z = speed
if(land.height_diff > land.min_altitude):
qc.linear.z = speed
rate.sleep()
print("Descending")
elif(land.height_diff <= land.min_altitude):
qc.linear.z = 0
else:
# elif((land.altitude < land.min_altitude)):
if not (land.altitude == 0):
print("Eagle one has descended!")
land.land()
# To change states, we publish the fact that we've
# reached our land altitude
rospy.loginfo("Going to secure mode")
land.goto_secure()
# qc.angular.z = yaw_update
qc.linear.x = x_update
qc.linear.y = y_update
# qc.linear.z = z_update
land.pub_altitude.publish(qc)
rate.sleep()
def state_cb(msg):
global state
state = msg.data
# pub_transition = rospy.Publisher('/smach/transition', String, queue_size=1)
sub_state = rospy.Subscriber('/smach/state', String, state_cb, queue_size=1000)
# Instantiate all of the modes
# Set all of the necessary parameters
# TODO instantiate these correctly i.e. correct parameters
# Land Mode Instantiation
land_speed = -1 # m/s
land_min_alt = 1000 # mm
land_max_alt = 2000 # mm
land_height_diff = 0 #mm
land_timeout = 3 # seconds
land = Landing(land_speed, land_min_alt, land_height_diff, land_max_alt, land_timeout)
# Takeoff Mode Instantiation
takeoff_speed = 1 # percentage of max speed (kind of)
takeoff_max_alt = 2500 # mm
takeoff_timeout = 10 # seconds
takeoff_counter = 0 # times
takeoff_to_reac_timeout = 15 # seconds
takeoff = Takeoff(takeoff_speed, takeoff_max_alt, takeoff_timeout)
# Takeoff Mode Instantiation
emergency = Emergency()
# Take Picture Mode Instantiation
takepicture_time = 300 # seconds
# takepicture_max_time = 3 # seconds
def main():
speed = -.5 # m/s
min_altitude = 0.8 # mm
max_altitudeGoal = 2.5 # mm
height_diff = 0 #mm
timeout = 300 # seconds
# pid_i = (kp, ki, kd, integrator, derivator, set_point)
pid_x = (6, 0.625, 2.5, 0, 0, 500)
pid_y = (6, 0.875, 3.75, 0, 0, 500)
pid_z = (1, 0, 0, 0, 0, 2.5)
pid_theta = (1 / 260, 0, 0, 0, 0, 0)
# set the bounding box
bbx = (375, 625)
bby = (375, 625)
bounding_box = True
bbx_min, bbx_max = bbx
bby_min, bby_max = bby
yaw_min = 10
yaw_max = 350
# controller update values
yaw_update = 0
x_update = 0
y_update = 0
z_update = 0
navdata = nd()
# Twist commands
qc = Twist()
land = Landing(speed, min_altitude, height_diff, max_altitudeGoal, \
timeout, bbx, bby, pid_x, pid_y, pid_z, pid_theta)
rate = rospy.Rate(200)
while ((land.state != 'land')):
# print takeoff.state
rate.sleep()
while not rospy.is_shutdown():
# always update the altitude
# z_update = land.pid_z.update(z_update)
# print("Theta %.2f" % navdata.theta)
# print("(%d, %d)" % (navdata.tag_x, navdata.tag_y))
if (navdata.tag_acquired):
# If 10 < theta < 350 then let's rotate
# if ((yaw_min < navdata.theta) and (navdata.theta < yaw_max)):
if ((yaw_min < navdata.theta < yaw_max)):
print "Yaw!"
yaw_update = land.pid_theta.update(navdata.theta)
else:
print "No yaw!"
yaw_update = 0
# is_in_box(minimum, maximum, position)
if (land.is_in_box(bbx_min, bbx_max, navdata.tag_y)
and land.is_in_box(bby_min, bby_max, navdata.tag_x)):
# If the QC is in the bounding box then we should enter 'Hover'
# mode and just hang there
x_update = 0
y_update = 0
# # qc.angular.x = 0.0
# qc.angular.y = 0.0
# print("In the Box")
else:
# It's not in the bounding box therefore we should update the PIDs
x_update = land.pid_x.update(navdata.tag_x)
y_update = land.pid_y.update(navdata.tag_y)
# print("%.3f" % land.pid_x.getError())
# always update the altitude
height_diff = land.max_altitudeGoal - land.altitude
# We only want to execute these manuevers if we're in land mode
if land.state == 'land':
# print("%.3f m" % land.altitude)
if (land.altitude > land.min_altitude):
print("Go down")
# qc.linear.z = speed
if (land.height_diff > land.min_altitude):
qc.linear.z = speed
rate.sleep()
print("Descending")
elif (land.height_diff <= land.min_altitude):
qc.linear.z = 0
else:
# elif((land.altitude < land.min_altitude)):
if not (land.altitude == 0):
print("Eagle one has descended!")
land.land()
# To change states, we publish the fact that we've
# reached our land altitude
rospy.loginfo("Going to secure mode")
land.goto_secure()
# qc.angular.z = yaw_update
qc.linear.x = x_update
qc.linear.y = y_update
# qc.linear.z = z_update
land.pub_altitude.publish(qc)
rate.sleep()
global state
state = msg.data
# pub_transition = rospy.Publisher('/smach/transition', String, queue_size=1)
sub_state = rospy.Subscriber('/smach/state', String, state_cb, queue_size=1000)
# Instantiate all of the modes
# Set all of the necessary parameters
# TODO instantiate these correctly i.e. correct parameters
# Land Mode Instantiation
land_speed = -1 # m/s
land_min_alt = 1000 # mm
land_max_alt = 2000 # mm
land_height_diff = 0 #mm
land_timeout = 3 # seconds
land = Landing(land_speed, land_min_alt, land_height_diff, land_max_alt,
land_timeout)
# Takeoff Mode Instantiation
takeoff_speed = 1 # percentage of max speed (kind of)
takeoff_max_alt = 2500 # mm
takeoff_timeout = 10 # seconds
takeoff_counter = 0 # times
takeoff_to_reac_timeout = 15 # seconds
takeoff = Takeoff(takeoff_speed, takeoff_max_alt, takeoff_timeout)
# Takeoff Mode Instantiation
emergency = Emergency()
# Take Picture Mode Instantiation
takepicture_time = 300 # seconds
# takepicture_max_time = 3 # seconds