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()