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
pitch=-25,
yaw=0,
vertical_movement=0,
duration=3.25)
mambo.fly_direct(roll=0,
pitch=0,
yaw=0,
vertical_movement=0,
duration=2)
#mambo.fly_direct(roll=-30, pitch=10, yaw=-30, vertical_movement=0, duration=4)
#mambo.fly_direct(roll=-40, pitch=10, yaw=-40, vertical_movement=0, duration=2)
#mambo.fly_direct(roll=0, pitch=0, yaw=0, vertical_movement=0, duration=3)
#mambo.fly_direct(roll=0, pitch=0, yaw=0, vertical_movement=-15, duration=1)
#mambo.fly_direct(roll=0, pitch=50, yaw=0, vertical_movement=0, duration=3)
#mambo.fly_direct(roll=0, pitch=0, yaw=0, vertical_movement=0, duration=1)
#mambo.fly_direct(roll=0, pitch=0, yaw=0, vertical_movement=0, duration=1)
#mambo.fly_direct(roll=0, pitch=0, yaw=0, vertical_movement=0, duration=1)
#mambo.fly_direct(roll=0, pitch=0, yaw=0, vertical_movement=0, duration=1)
#mambo.fly_direct(roll=0, pitch=0, yaw=0, vertical_movement=0, duration=1)
#mambo.fly_direct(roll=0, pitch=0, yaw=0, vertical_movement=0, duration=1)
success = mambo.flip(direction="front")
print("landing")
print("flying state is %s" % mambo.sensors.flying_state)
mambo.safe_land(5)
mambo.smart_sleep(5)
print("disconnect")
mambo.disconnect()
print("taking off!")
mambo.safe_takeoff(5)
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("flip left")
print("flying state is %s" % mambo.sensors.flying_state)
success = mambo.flip(direction="left")
print("mambo flip result %s" % success)
mambo.smart_sleep(5)
print("flip right")
print("flying state is %s" % mambo.sensors.flying_state)
success = mambo.flip(direction="right")
print("mambo flip result %s" % success)
mambo.smart_sleep(5)
print("flip front")
print("flying state is %s" % mambo.sensors.flying_state)
success = mambo.flip(direction="front")
print("mambo flip result %s" % success)
mambo.smart_sleep(5)
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("flip back")
print("flying state is %s" % mambo.sensors.flying_state)
success = mambo.flip(direction="back")
print("mambo flip result %s" % success)
mambo.smart_sleep(5)
#next
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)
if (testFlying):
print("taking off!")
mambo.safe_takeoff(5)
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("flip left")
print("flying state is %s" % mambo.sensors.flying_state)
success = mambo.flip(direction="left")
print("mambo flip result %s" % success)
mambo.smart_sleep(5)
print("landing")
print("flying state is %s" % mambo.sensors.flying_state)
mambo.safe_land(5)
else:
print("Sleeeping for 15 seconds - move the mambo around")
mambo.smart_sleep(15)
# done doing vision demo
print("Ending the sleep and vision")
mamboVision.close_video()
mambo.smart_sleep(5)
print("taking off!")
drone.safe_takeoff(5)
if (drone.sensors.flying_state != "emergency"):
print("flying state is %s" % drone.sensors.flying_state)
print("Flying direct: going up")
drone.fly_direct(roll=0,
pitch=0,
yaw=0,
vertical_movement=20,
duration=1)
print("flip left")
print("flying state is %s" % drone.sensors.flying_state)
success = drone.flip(direction="left")
print("mambo flip result %s" % success)
drone.smart_sleep(2)
print("flip right")
print("flying state is %s" % drone.sensors.flying_state)
success = drone.flip(direction="right")
print("mambo flip result %s" % success)
drone.smart_sleep(2)
print("flip front")
print("flying state is %s" % drone.sensors.flying_state)
success = drone.flip(direction="front")
print("mambo flip result %s" % success)
drone.smart_sleep(2)
print("Flying direct: going up")
mambo.fly_direct(roll=0,
pitch=0,
yaw=0,
vertical_movement=20,
duration=1)
# print("flip left")
# print("flying state is %s" % mambo.sensors.flying_state)
# success = mambo.flip(direction="left")
# print("mambo flip result %s" % success)
# mambo.smart_sleep(5)
#
print("flip right")
print("flying state is %s" % mambo.sensors.flying_state)
success = mambo.flip(direction="right")
print("mambo flip result %s" % success)
mambo.smart_sleep(5)
#
# print("flip front")
# print("flying state is %s" % mambo.sensors.flying_state)
# success = mambo.flip(direction="front")
# print("mambo flip result %s" % success)
# mambo.smart_sleep(5)
#
# print("flip back")
# print("flying state is %s" % mambo.sensors.flying_state)
# success = mambo.flip(direction="back")
# print("mambo flip result %s" % success)
# mambo.smart_sleep(5)
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()
fw(35,2)
#left
mambo.smart_sleep(3)
sw(-20,3.5)
#backwards all the way to chair
mambo.smart_sleep(4)
fw(-35,4.25)
#flying up to go through back
mambo.smart_sleep(2)
mambo.fly_direct(roll=0, pitch=0, yaw=0, vertical_movement=10, duration=3)
mambo.smart_sleep(2)
#going to and through back of chair
sw(20,2.25)
#going through the horizontal part of chair
mambo.smart_sleep(2)
mambo.fly_direct(roll=0, pitch=0, yaw=0, vertical_movement=-7, duration=3)
mambo.smart_sleep(2)
#going backwards through bottom of chair to landing pad
mambo.smart_sleep(2)
fw(-35,3.5)
#flip
mambo.smart_sleep(2)
mambo.flip(direction = "front")
#landing
print("landing")
mambo.safe_land(5)
mambo.smart_sleep(5)
print("disconnect")
mambo.disconnect()
