def __init__(self, name, environment=None): super(ARDrone, self).__init__(name, environment) self.battery_level = 1.0 self.diameter = 0.0 self.original_position = np.array([0, 0, 0]) self.position = np.array([0, 0, 0]) self.target = np.array([0, 0, 0]) self.capabilities = [ "move" ] self.is_flying = False self.low_battery_level = 0.1 video = freenect.sync_get_video() self.have_kinect = video is not None self.drone = ARDroneAPI(True, True)
class ARDrone(Drone): """docstring for KinectDrone""" def __init__(self, name, environment=None): super(ARDrone, self).__init__(name, environment) self.battery_level = 1.0 self.diameter = 0.0 self.original_position = np.array([0, 0, 0]) self.position = np.array([0, 0, 0]) self.target = np.array([0, 0, 0]) self.capabilities = [ "move" ] self.is_flying = False self.low_battery_level = 0.1 video = freenect.sync_get_video() self.have_kinect = video is not None self.drone = ARDroneAPI(True, True) def initialize(self): self.target = self.position def isBatteryLow(self): ## DRAINS 0.8 OF BATTERY PER TASK (Intended to run out fast for testing) self.battery_level = self.battery_level - 0.1 if ((self.battery_level - 0.1) >= 0.0) else self.low_battery_level * 2.0 logger.info("%s - %s", self.name, self.battery_level) return self.low_battery_level * 2.0 >= self.battery_level def move(self, position): self.target = position self.original_position = self.position def noop(self): # hover in place pass def getBatteryLevel(self): return self.battery_level def getPosition(self): # return drone's position in the environment's coordinate system return self.position def getDroneDiameter(self): # return drone size in the environment's coordinate system return self.diameter def isCharged(self): return self.battery_level > 0.8 def setState(self, state): pass def takeoff(self): logger.info("Drone takeoff") self.drone.takeoff() time.sleep(5) self.is_flying = True def land(self): logger.info("Drone land") self.drone.land() self.is_flying = False def hover(self): self.drone.hover() def controlLoop(self): try: kp = 0.2 kd = 0.1 ki = 0.0005 x_pid = PID(Kp=kp, Kd=kd, Ki=ki) y_pid = PID(Kp=kp, Kd=kd, Ki=ki) angle_pid = PID(Kp=1.5, Kd=0.5, Ki=0.05) while True: image = freenect.sync_get_video()[0] image = cv2.cvtColor(image, cv2.cv.CV_BGR2RGB) target = (int(self.target[0] + 320), int(self.target[1] + 240)) center = centerFromImage(image, 10, 20) tail = centerFromImage(image, 75, 90) cv2.line(image, target, tuple(center), (255, 0, 0)) cv2.circle(image, tuple(center), 3, (0, 0, 255), -1) cv2.circle(image, tuple(tail), 3, (255, 0, 0), -1) cv2.circle(image, target, 10, (0, 0, 0), 3) distance_x = center[0] - target[0] distance_y = center[1] - target[1] distance_x /= -320.0 distance_y /= -240.0 angle = angleFromCenterAndTail(center, tail) / -180.0 rl = x_pid.GenOut(distance_x) fb = y_pid.GenOut(distance_y) aa = angle_pid.GenOut(angle) if np.isnan(aa): aa = 0.0 self.position = (center[0] - 320.0, center[1] - 240.0, 10) if self.isAtTarget(self.charger.getCoordinates(), 20) and self.battery_level < 1.0: # If at charger, slowly charge self.battery_level += 0.01 if self.is_flying: self.drone.at_cmd(True, rl, fb, 0, aa) # logger.info("Set Drone: %s, %s, %s", rl, fb, aa) except Exception, e: logger.warning("Exception %s", e) finally: