def run(self):
# you will need to change this to the address of YOUR mambo
mamboAddr = "e0:14:d0:63:3d:d0"
# make my mambo object
mambo = Mambo(mamboAddr, use_wifi=True)
print("trying to connect")
success = mambo.connect(num_retries=3)
print("connected: %s" % success)
print("sleeping")
mambo.smart_sleep(2)
mambo.ask_for_state_update()
mambo.smart_sleep(2)
print("taking off!")
mambo.safe_takeoff(3)
# from IPython import embed;embed()
# mambo.fly_direct(roll=0, pitch=0, yaw=0, vertical_movement=50, duration=1)
while self._running:
message = self.read()
self.logger.info("message:%s", str(message))
data = message.get("data")
if data:
if data == "turn around":
mambo.turn_degrees(90)
if data == "up":
# self.logger.info("up!!") # ok
# https://jingtaozf.gitbooks.io/crazepony-gitbook/content/wiki/pitch-yaw-roll.html
mambo.fly_direct(
roll=0,
pitch=0,
yaw=0,
vertical_movement=50,
duration=0.5)
if data == "down":
# self.logger.info("down!!") # ok
mambo.fly_direct(
roll=0,
pitch=0,
yaw=0,
vertical_movement=-50,
duration=0.5)
if data == "flip":
success = mambo.flip(direction="back")
mambo.safe_land(5)
class DroneController():
def __init__(self, mac_address, debug=False, mock=False):
self.flying = False
self.commands = Queue()
if not mock:
self.drone = Mambo(mac_address)
self.debug = debug
self.mock = mock
def done():
self.drone.safe_land(5)
self.flying = False
self.code_to_command = {
0: (lambda: done()),
1: (lambda: self.drone.safe_takeoff(5)), # takeoff
2: (lambda: self.drone.safe_land(5)), # land
3: (lambda: self.drone.fly_direct(0, 10, 0, 0, 1)), # forward
4: (lambda: self.drone.fly_direct(0, -10, 0, 0, 1)), # backward
5: (lambda: self.drone.fly_direct(-10, 0, 0, 0, 1)), # left
6: (lambda: self.drone.fly_direct(10, 0, 0, 0, 1)), # right
7: (lambda: self.drone.fly_direct(0, 0, 0, 10, 1)), # up
8: (lambda: self.drone.fly_direct(0, 0, 0, -10, 1)), # down
9: (lambda: self.drone.turn_degrees(-45)), # turn left
10: (lambda: self.drone.turn_degrees(45)), # turn right
11: (lambda: self.drone.flip("front")), # flip forward
12: (lambda: self.drone.flip("back")), # flip backward
13: (lambda: self.drone.flip("right")), # flip right
14: (lambda: self.drone.flip("left")), # flip left
}
if mock:
self.process_command = lambda c: print(F"Mock execute: {c}")
else:
self.process_command = lambda c: self.code_to_command[
c.command_code]()
def start_flight(self):
self.flying = True
def fly():
self.flying = True
if not self.mock:
self.drone.connect(5)
self.drone.smart_sleep(5)
while self.flying:
try:
c = self.commands.get(block=False)
except:
if not self.mock:
#self.drone.hover()
self.drone.smart_sleep(2)
continue
if self.debug:
print(F"Debug: {c}")
self.process_command(c)
if not self.mock:
self.drone.smart_sleep(3)
if not self.mock:
self.drone.disconnect()
t = Thread(target=fly)
t.start()
def send_command(self, command):
if not self.flying:
return False
self.commands.put(command)
return True
def stop_flight(self):
# self.send_command(<STOP COMMAND>)
pass
print("Flying direct: going forward (positive pitch)")
mambo.fly_direct(roll=0, pitch=100, yaw=0, vertical_movement=0, duration=2)
print("Flying direct: going backwards (negative pitch)")
mambo.fly_direct(roll=0,
pitch=-200,
yaw=0,
vertical_movement=0,
duration=2)
print("Flying direct: going forward (positive pitch)")
mambo.fly_direct(roll=0, pitch=100, yaw=0, vertical_movement=0, duration=2)
print("Showing turning (in place) using turn_degrees")
mambo.turn_degrees(180)
mambo.smart_sleep(2)
mambo.turn_degrees(-180)
mambo.smart_sleep(2)
print("Flying direct: going forward (positive pitch)")
mambo.fly_direct(roll=0, pitch=100, yaw=0, vertical_movement=0, duration=2)
print("Flying direct: going backwards (negative pitch)")
mambo.fly_direct(roll=0,
pitch=-200,
yaw=0,
vertical_movement=0,
duration=2)
print("Flying direct: going forward (positive pitch)")
print("sleeping")
mambo.smart_sleep(5)
print("taking off!")
mambo.safe_takeoff(5)
print("Flying direct: going forward (positive pitch)")
mambo.fly_direct(roll=0, pitch=0, yaw=0, vertical_movement=-10, duration=2)
mambo.smart_sleep(0)
print("Flying direct: going forward (positive pitch)")
mambo.fly_direct(roll=0, pitch=20, yaw=0, vertical_movement=0, duration=4)
mambo.smart_sleep(0)
print("Showing turning (in place) using turn_degrees")
mambo.turn_degrees(-45)
mambo.smart_sleep(0)
print("Showing turning (in place) using turn_degrees")
mambo.turn_degrees(-45)
mambo.smart_sleep(0)
print("Showing turning (in place) using turn_degrees")
mambo.turn_degrees(-45)
mambo.smart_sleep(0)
print("Showing turning (in place) using turn_degrees")
mambo.turn_degrees(-45)
mambo.smart_sleep(2)
print("Flying direct: going forward (positive pitch)")
class Fly(Visitor):
def __init__(self, mac_addr, rs):
self.mambo = Mambo(mac_addr, use_wifi=False)
self.requirements = rs
# positive is east
self.x = 0
# positive is up
self.y = 0
# positive is south
self.z = 0
# Angle of drone from the x axis, so starts north
self.theta = 90
info("Trying to connect")
success = self.mambo.connect(num_retries=3)
info("Connected: %s" % success)
def __del__(self):
info("Disconnecting")
self.mambo.safe_land(5)
self.mambo.disconnect()
def takeoff(self, tree):
info('Taking off')
self.mambo.safe_takeoff(5)
self.mambo.smart_sleep(1)
self.y = TAKE_OFF_HEIGHT
def land(self, tree):
info('Landing at x={}, y={}, ={}'.format(self.x, self.y, self.z))
self.mambo.safe_land(5)
self.y = 0
for r in self.requirements:
r.update_on_land(self.x, self.y, self.z)
def up(self, tree):
duration = toFloat(tree)
self.mambo.fly_direct(roll=0,
pitch=0,
yaw=0,
vertical_movement=10,
duration=duration)
self.mambo.smart_sleep(2)
self.y += VERTICAL_CALIBRATION * duration
def down(self, tree):
duration = toFloat(tree)
self.mambo.fly_direct(roll=0,
pitch=0,
yaw=0,
vertical_movement=-10,
duration=duration)
self.mambo.smart_sleep(2)
self.y -= VERTICAL_CALIBRATION * duration
def move_in_steps(self, roll, pitch, yaw, v_m, duration):
for _ in range(floor(duration)):
self.mambo.fly_direct(roll, pitch, yaw, v_m, 1)
self.mambo.smart_sleep(2)
if floor(duration) is not duration:
self.mambo.fly_direct(roll, pitch, yaw, v_m,
duration - floor(duration))
self.mambo.smart_sleep(2)
def left(self, tree):
duration = toFloat(tree)
self.move_in_steps(roll=-10, pitch=0, yaw=0, v_m=0, duration=duration)
self.x += HORIZONTAL_CALIBRATION * duration * cos(
radians(self.theta) + pi / 2)
self.z -= HORIZONTAL_CALIBRATION * duration * sin(
radians(self.theta) + pi / 2)
for r in self.requirements:
r.update_on_move(self.x, self.y, self.z)
def right(self, tree):
duration = toFloat(tree)
self.move_in_steps(roll=10, pitch=0, yaw=0, v_m=0, duration=duration)
self.x += HORIZONTAL_CALIBRATION * duration * cos(
radians(self.theta) - pi / 2)
self.z -= HORIZONTAL_CALIBRATION * duration * sin(
radians(self.theta) - pi / 2)
for r in self.requirements:
r.update_on_move(self.x, self.y, self.z)
def forward(self, tree):
duration = toFloat(tree)
self.move_in_steps(roll=0, pitch=10, yaw=0, v_m=0, duration=duration)
self.x += HORIZONTAL_CALIBRATION * duration * cos(radians(self.theta))
self.z -= HORIZONTAL_CALIBRATION * duration * sin(radians(self.theta))
for r in self.requirements:
r.update_on_move(self.x, self.y, self.z)
def backward(self, tree):
duration = toFloat(tree)
self.move_in_steps(roll=0, pitch=-10, yaw=0, v_m=0, duration=duration)
self.x += HORIZONTAL_CALIBRATION * duration * cos(
radians(self.theta) + pi)
self.z -= HORIZONTAL_CALIBRATION * duration * sin(
radians(self.theta) + pi)
for r in self.requirements:
r.update_on_move(self.x, self.y, self.z)
def rotatel(self, tree):
degrees = toFloat(tree)
self.mambo.turn_degrees(-degrees)
self.mambo.smart_sleep(3)
self.theta += degrees
def rotater(self, tree):
degrees = toFloat(tree)
self.mambo.turn_degrees(degrees)
self.mambo.smart_sleep(3)
self.theta -= degrees
def wait(self, tree):
duration = toFloat(tree)
info('Waiting {} seconds'.format(duration))
self.mambo.smart_sleep(duration)
def action(self, tree):
self.mambo.smart_sleep(2)
info('Performed action at ({}, {}, {})'.format(self.x, self.y, self.z))
for r in self.requirements:
r.update_on_action(self.x, self.y, self.z)
def abort(self):
self.mambo.safe_land(5)
i = 0
while True:
i +=1
print(mambo.sensors.flying_state)
if mambo.sensors.flying_state != "landed":
print("flying forward")
mambo.smart_sleep(3)
mambo.fly_direct(roll=0, pitch=50, yaw=0, vertical_movement=0, duration=0.5)
print("flying backwards")
mambo.smart_sleep(2)
mambo.fly_direct(roll=0, pitch=-50, yaw=0, vertical_movement=0, duration=0.5)
print("turning 360")
mambo.smart_sleep(2)
mambo.turn_degrees(180)
mambo.smart_sleep(2)
mambo.turn_degrees(180)
print("landing")
mambo.safe_land(5)
mambo.smart_sleep(5)
print("disconnect")
mambo.disconnect()
elif i >= 500:
print("TIMEOUT ERROR")
mambo.safe_land(5)
mambo.smart_sleep(5)
for i in range(200):
mambo.smart_sleep(2)
pic_success = mambo.take_picture()
mambo.smart_sleep(1)
ftp_download()
ground_coordinates_value = ground_vision()
if ground_coordinates_value is None:
# There are not houghes circles in the pic. No hay problema, I will cotinue on my way
pass
else:
(angle_r_s, x_vector_g_m,
y_vector_g_m) = ground_coordinates_value
print("Drone Rotate of degree " + str(angle_r_s))
if angle_r_s > 180:
mambo.turn_degrees(angle_r_s - 360)
else:
mambo.turn_degrees(angle_r_s)
# uav looks left
mambo.smart_sleep(2)
mambo.turn_degrees(-90)
# uav looks straight
mambo.smart_sleep(1)
mambo.turn_degrees(90)
# uav looks right
mambo.smart_sleep(1)
mambo.turn_degrees(90)
print('rotate angle is: ' + str(rotate_angle))
#setting drone speed
tilt = 12
drone_speed = 20
#take-off
print("taking off!")
mambo.takeoff()
mambo.smart_sleep(2)
#rotate to face corrent position
mambo.turn_degrees(rotate_angle - 5)
#get distance that needs to be travelled
straight_dist = np.sqrt(final_x**2 + final_y**2)
print('distance is: ' + str(straight_dist))
current_dist = 0
# getting to the location required
print('getting to location')
while(current_dist <= straight_dist):
mambo.fly_direct(roll=0, pitch=tilt, yaw=0, vertical_movement=0, duration=0.5)
current_dist += drone_speed
#stabilizing
mambo.smart_sleep(2)
#mambo.fly_direct(roll=0, pitch=0, yaw=0,vertical_movement=0)
print("connected: %s" % success1)
print("connected: %s" % success2)
if (success1 and success2 == True):
# get the state information
print("sleeping")
mambo1.smart_sleep(2)
mambo2.smart_sleep(2)
mambo1.ask_for_state_update()
mambo2.ask_for_state_update()
mambo1.smart_sleep(2)
mambo2.smart_sleep(2)
print("taking off!")
mambo1.safe_takeoff(1)
mambo2.safe_takeoff(1)
mambo1.turn_degrees(180)
mambo2.turn_degrees(180)
mambo1.safe_land(5)
mambo2.safe_land(5)
mambo1.smart_sleep(5)
mambo2.smart_sleep(5)
mambo1.disconnect()
mambo2.disconnect()
"""
if (mambo.sensors.flying_state != "emergency"):
print("flying state is %s" % mambo.sensors.flying_state)
print("Flying direct: going up")
mambo.fly_direct(roll=0, pitch=0, yaw=0, vertical_movement=20, duration=1)
print("Showing turning (in place) using turn_degrees")
mambo.turn_degrees(180)
class DroneColorSegTest:
def __init__(self, testFlying, mamboAddr, use_wifi):
self.bb = [0, 0, 0, 0]
self.bb_threshold = 4000
self.bb_trigger = False
self.testFlying = testFlying
self.mamboAddr = mamboAddr
self.use_wifi = use_wifi
self.mambo = Mambo(self.mamboAddr, self.use_wifi)
self.mamboVision = DroneVisionGUI(
self.mambo,
is_bebop=False,
buffer_size=200,
user_code_to_run=self.mambo_fly_function,
user_args=None)
def color_segmentation(self, args):
img = self.mamboVision.get_latest_valid_picture()
if img is not None:
[((x1, y1), (x2, y2)), ln_color] = cd_color_segmentation(img)
self.bb = [x1, y1, x2, y2]
bb_size = self.get_bb_size()
print('bb_size:', bb_size)
# cv2.imwrite('test_file.png', img) # uncomment to save latest pic
if bb_size >= self.bb_threshold:
print('orange detected')
self.bb_trigger = True
# else:
# self.bb_trigger = False
else:
print('image is None')
def get_bb_size(self):
''' returns area of self.bb (bounding box) '''
return (self.bb[2] - self.bb[0]) * (self.bb[3] - self.bb[1])
def mambo_fly_function(self, mamboVision, args):
"""
self.mambo takes off and yaws slowly in a circle until the vision processing
detects a large patch of orange. It then performs a flip and lands.
"""
if (self.testFlying):
print("taking off!")
self.mambo.safe_takeoff(5)
if (self.mambo.sensors.flying_state != "emergency"):
print("flying state is %s" % self.mambo.sensors.flying_state)
print("Flying direct: going up")
self.mambo.fly_direct(roll=0,
pitch=0,
yaw=0,
vertical_movement=15,
duration=2)
print("flying state is %s" % self.mambo.sensors.flying_state)
print("yawing slowly")
for deg in range(36):
self.mambo.turn_degrees(10)
if self.bb_trigger:
break
self.mambo.smart_sleep(1)
print("flying state is %s" % self.mambo.sensors.flying_state)
print("flip left")
success = self.mambo.flip(direction="left")
print("self.mambo flip result %s" % success)
self.mambo.smart_sleep(2)
print("landing")
print("flying state is %s" % self.mambo.sensors.flying_state)
self.mambo.safe_land(5)
else:
print("Sleeeping for 15 seconds - move the self.mambo around")
self.mambo.smart_sleep(15)
# done doing vision demo
print("Ending the sleep and vision")
self.mamboVision.close_video()
self.mambo.smart_sleep(5)
print("disconnecting")
self.mambo.disconnect()
def run_test(self):
print("trying to connect to self.mambo now")
success = self.mambo.connect(num_retries=3)
print("connected: %s" % success)
if (success):
# get the state information
print("sleeping")
self.mambo.smart_sleep(1)
self.mambo.ask_for_state_update()
self.mambo.smart_sleep(1)
print("Preparing to open vision")
self.mamboVision.set_user_callback_function(
self.color_segmentation, user_callback_args=None)
self.mamboVision.open_video()
