class CrazyflieThread(threading.Thread): def __init__(self, ctrl_message, state_message, message_lock, barrier): threading.Thread.__init__(self) self.ctrl = ctrl_message self.state = state_message self.lock = message_lock self.cf = None self.mc = None self.scf = None self.runSM = True self.cmd_height_old = uglyConst.TAKEOFF_HEIGHT self.b = barrier self.descCounter = 0 def raise_exception(self): self.runSM = False #-- FSM condition funcitons def isCloseXYP(self, r): """Determines if drone is within radius r and aligned.""" x = self.ctrl.errorx y = self.ctrl.errory if (np.sqrt(x*x+y*y) > r) or (np.abs(self.ctrl.erroryaw) > uglyConst.FAR_ANGL): return False else: return True def isCloseCone(self): """Determines if drone within an inverted cone (defined in constants).""" x = self.ctrl.errorx y = self.ctrl.errory r = (self.mc._thread.get_height()-uglyConst.DIST_IGE)*uglyConst.FAR_CONE if (np.sqrt(x*x+y*y) > r): # or (np.abs(self.ctrl.erroryaw) > uglyConst.FAR_ANGL): return False else: return True def isClosePix(self): """Determines if drone is within radius (in pixels, defined in constants).""" x = self.ctrl.errorpixx y = self.ctrl.errorpixy if (np.sqrt(x*x+y*y) > uglyConst.CLOSE_PIX): return False else: return True def limOutputVel(self, vx, vy, vz): """Limits output of velocity controller.""" return min(vx, uglyConst.MAX_XVEL), min(vy, uglyConst.MAX_YVEL), min(vz, uglyConst.MAX_ZVEL) #-- FSM state functions def stateInit(self): """Initialize CF (scan, open link) and logging framework""" #--- Scan for cf cflib.crtp.init_drivers(enable_debug_driver=False) print('Scanning interfaces for Crazyflies...') available = cflib.crtp.scan_interfaces() if len(available) == 0: print("No cf found, aborting cf code.") self.cf = None else: print('Crazyflies found:') for i in available: print(str(i[0])) self.URI = 'radio://0/80/2M' #available[0][0] self.cf = Crazyflie(rw_cache='./cache') if self.cf is None: self.b.wait() print('Not running cf code.') return None self.scf = SyncCrazyflie(self.URI,cf=Crazyflie(rw_cache='./cache')) self.scf.open_link() self.mc = MotionCommander(self.scf) self.file = open("ugly_log.txt","a") #-- Barrier to wait for CV thread self.b.wait() self.lgr = UglyLogger(self.URI, self.scf, self.file) self.enterTakingOff() return self.stateTakingOff def enterTakingOff(self): """Entry to state: Take off""" print("enterTakingOff") self.mc.take_off(uglyConst.TAKEOFF_HEIGHT,uglyConst.TAKEOFF_ZVEL) def stateTakingOff(self): """State: Taking off""" print("stateTakingOff") if self.mc._thread.get_height() >= uglyConst.TAKEOFF_HEIGHT: return self.stateSeeking else: time.sleep(0.05) return self.stateTakingOff def stateSeeking(self): """State: Seeking. Slowly ascend until marker is detected. TODO: Implement some kind of search algorithm (circle outward?)""" self.mc._set_vel_setpoint(0.0, 0.0, 0.01, 0.0) print("stateSeeking") if self.state.isMarkerDetected: return self.stateNearing else: time.sleep(0.05) return self.stateSeeking def stateNearing(self): """ State: Nearing Control in pixel frame. Takes in error in pixels (note: camera coordinates), outputs velocity command in x,y,z. Z vel inversely proportional to radius. """ x = self.ctrl.errorpixx y = self.ctrl.errorpixy cmdx = y*uglyConst.PIX_Kx cmdy = x*uglyConst.PIX_Ky r = np.sqrt(x*x+y*y) if r > 0.0: cmdz = (1/r)*uglyConst.PIX_Kz else: cmdz = 1 cmdx, cmdy, cmdz = self.limOutputVel(cmdx, cmdy, cmdz) self.mc._set_vel_setpoint(cmdx, cmdy, -cmdz, 0.0) print("stateNearing") if self.isClosePix() and self.mc._thread.get_height() < uglyConst.NEARING2APPROACH_HEIGHT: self.state.cv_mode = uglyConst.CVMODE_POSE return self.stateApproachin else: time.sleep(0.05) return self.stateNearing def stateApproachingXY(self): """ State: Approaching (XY plane) Control in the horizontal XY plane and yaw angle. """ #self.mc._set_vel_setpoint(self.ctrl.errorx*(uglyConst.Kx+self.ctrl.K), self.ctrl.errory*(uglyConst.Ky+self.ctrl.K), 0.0, 30.0) self.mc._set_vel_setpoint(self.ctrl.errorx*uglyConst.Kx, self.ctrl.errory*uglyConst.Ky, 0.0, -self.ctrl.erroryaw*uglyConst.Kyaw) print("stateApproachingXY") if not self.isClosePix: return self.stateNearing if self.isCloseCone() and np.abs(self.ctrl.erroryaw) < uglyConst.FAR_ANGL: return self.stateApproachingXYZ else: time.sleep(0.05) return self.stateApproachingXY def stateApproachingXYZ(self): """ State: Approaching Control in world frame. Takes in error in meters, outputs velocity command in x,y. Constant vel cmd in z. """ self.mc._set_vel_setpoint(self.ctrl.errorx*uglyConst.Kx, self.ctrl.errory*uglyConst.Ky, -uglyConst.APPROACH_ZVEL, -self.ctrl.erroryaw*uglyConst.Kyaw) print("stateApproachingXYZ") if not self.isCloseCone: return self.stateApproachingXY elif self.mc._thread.get_height() < uglyConst.APPROACH2LANDING_HEIGHT: return self.stateDescending else: time.sleep(0.05) return self.stateApproachingXYZ def stateDescending(self): """ State: Landing Procedure: Descend to a set height, then stop and land. """ self.mc._set_vel_setpoint(self.ctrl.errorx*uglyConst.Kx*2.0, self.ctrl.errory*uglyConst.Ky*2.0, -uglyConst.LANDING_ZVEL, -self.ctrl.erroryaw*uglyConst.Kyaw) print("stateDescending") if self.mc._thread.get_height() > uglyConst.APPROACH2LANDING_HEIGHT: return self.stateApproaching elif self.mc._thread.get_height() < uglyConst.LANDING2LANDED_HEIGHT: #self.exitLanding() #return self.stateTerminating return self.stateLanding else: time.sleep(0.01) return self.stateDescending def stateLanding(self): """""" self.mc._set_vel_setpoint(self.ctrl.errorx*uglyConst.Kx*4.0, self.ctrl.errory*uglyConst.Ky*4.0, -uglyConst.MAX_ZVEL, -self.ctrl.erroryaw*uglyConst.Kyaw*2.0) self.descCounter += 1 print("stateLanding") if self.descCounter > 10: self.mc.land() return self.stateTerminating else: time.sleep(0.01) return self.stateLanding def exitLanding(self): """ Exit from state: Landing Stop movement (vel cmds=0), then descend. """ self.mc.land() print("exitLandning") def stateTerminating(self): """ State: Landed Dummy state. """ print("stateTerminating") return None def run(self): """Main loop, state machine""" try: state = self.stateInit # initial state self.stateNum = uglyConst.S0_INIT while state and self.runSM: state = state() finally: #-- Clean up print('Stopping cf_thread') if self.cf is not None: if self.mc._is_flying: # if cf has not landed, do so self.mc.stop() print('Finally landing') self.mc.land() #-- Close link and logging self.scf.close_link() self.file.close()
class CF: def __init__(self, scf, available): # get yaw-angle and battery level of crazyflie self.vbat, self.blevel, self.m1_pwm, self.temp = getCFparams(scf, available) self.scf = scf self.available = available self.mc = MotionCommander(scf) self.psi_limit = 0.7 # Don't cmd an angle less than this [deg] self.des_angle = 0 # Set to zero initially self.psi = 0 self.roll = 0 self.pitch = 0 self.theta = 0 self.phi = 0 def update(self, des_angle, eul, turnRate): if 'psi' in eul and abs(des_angle) > self.psi_limit: self.des_angle = des_angle if des_angle > 0: if not self.mc.turn_right(abs(self.des_angle), turnRate): pass else: if not self.mc.turn_left(abs(self.des_angle), turnRate): pass # Compute current angle (yaw) of CF self.vbat, self.blevel, self.m1_pwm, self.temp = getCFparams(self.scf, self.available) return(self.vbat, self.blevel, self.m1_pwm, self.temp) # Move cf left or right def update_x(self, dist): if dist is not None and dist != 0: if not self.move_distance(0, -dist, 0): pass elif dist is not None and dist == 0 and self.des_angle == 0: self.mc.stop() def takeoff(self): self.mc.take_off() def land(self): self.mc.land() def move_distance(self, x, y, z): ''' Move in a straight line, {CF frame}. positive X is forward [cm] positive Y is left [cm] positive Z is up [cm] ''' velocity = 0.08 x = x/100 y = y/100 z = z/100 distance = math.sqrt(x**2 + y**2 + z**2) if x != 0: flight_time = distance / velocity velocity_x = velocity * x / distance else: velocity_x = 0 if y != 0: velocity_y = velocity * y / distance else: velocity_y = 0 if z != 0: velocity_z = velocity * z / distance else: velocity_z = 0 self.mc.start_linear_motion(velocity_x, velocity_y, velocity_z) if x != 0: time.sleep(flight_time) return(False)
thread.setDaemon(True) thread.start() thread.join() e2 = cv2.getTickCount() t = (e2 - e1) / cv2.getTickFrequency() exeTime.append(t) calculator.writeDistance(frame) cv2.imshow('frame', frame) # print(calculator.distance_x) if (calculator.distance_x == 0): print("zero") elif (calculator.distance_x > 30): if (mc.isRunning): mc.stop() mc.start_linear_motion(0.1, 0.0, 0.0) mc.setIsRunning(True) print("para frente") elif (calculator.distance_x < 25): if (mc.isRunning): mc.stop() mc.start_linear_motion(-0.1, 0.0, 0.0) mc.setIsRunning(True) print("para tras") else: if (mc.isRunning): mc.stop() cont += 1 if cv2.waitKey(10) & 0xFF == ord('q'):
class Comm: def __init__(self, link_uri): """ Initialize and run the test with the specified link_uri """ self._cf = Crazyflie() self.mc = MotionCommander(self._cf, default_height=1.05) self._cf.connected.add_callback(self._connected) self._cf.disconnected.add_callback(self._disconnected) self._cf.connection_failed.add_callback(self._connection_failed) self._cf.connection_lost.add_callback(self._connection_lost) self._cf.open_link(link_uri) print('Connecting to %s' % link_uri) self.is_connected = True def _connected(self, link_uri): """ This callback is called from the Crazyflie API when a Crazyflie has been connected and the TOCs have been downloaded.""" print('Connected to %s' % link_uri) # The definition of the logconfig can be made before connecting self._lg_stab = LogConfig(name='Stabilizer', period_in_ms=10) self._lg_stab.add_variable('stabilizer.roll', 'float') self._lg_stab.add_variable('stabilizer.pitch', 'float') self._lg_stab.add_variable('stabilizer.yaw', 'float') self._log_conf = LogConfig(name="Accel", period_in_ms=10) self._log_conf.add_variable('acc.x', 'float') self._log_conf.add_variable('acc.y', 'float') self._log_conf.add_variable('acc.z', 'float') # Adding the configuration cannot be done until a Crazyflie is # connected, since we need to check that the variables we # would like to log are in the TOC. try: self._cf.log.add_config(self._lg_stab) # This callback will receive the data self._lg_stab.data_received_cb.add_callback(self._stab_log_data) # This callback will be called on errors self._lg_stab.error_cb.add_callback(self._stab_log_error) # Start the logging self._lg_stab.start() self._log = self._cf.log.add_config(self._log_conf) if self._log_conf is not None: self._log_conf.data_received_cb.add_callback( self._log_accel_data) self._log_conf.start() else: print("acc.x/y/z not found in log TOC") except KeyError as e: print('Could not start log configuration,' '{} not found in TOC'.format(str(e))) except AttributeError: print('Could not add log config, bad configuration.') # Start a separate thread to do the motor test. # Do not hijack the calling thread! self.is_connected = True worker = Thread(target=self.flip_test) worker.start() def _log_accel_data(self, timestamp, data, logconf): #print('[%d] Accelerometer: x=%.2f, y=%.2f, z=%.2f' % # (timestamp, data['acc.x'], data['acc.y'], data['acc.z'])) """This is from the basiclog file""" def _stab_log_error(self, logconf, msg): """Callback from the log API when an error occurs""" #print('Error when logging %s: %s' % (logconf.name, msg)) def _stab_log_data(self, timestamp, data, logconf): """Callback froma the log API when data arrives""" #print('[%d][%s]: %s' % (timestamp, logconf.name, data)) def _connection_failed(self, link_uri, msg): """Callback when connection initial connection fails (i.e no Crazyflie at the specified address)""" print('Connection to %s failed: %s' % (link_uri, msg)) def _connection_lost(self, link_uri, msg): """Callback when disconnected after a connection has been made (i.e Crazyflie moves out of range)""" print('Connection to %s lost: %s' % (link_uri, msg)) def _disconnected(self, link_uri): """Callback when the Crazyflie is disconnected (called in all cases)""" print('Disconnected from %s' % link_uri) def flip_test(self): # Below is attempt to implememnt algorithm found in https://www.research-collection.ethz.ch/bitstream/handle/20.500.11850/151399/eth-165-01.pdf?sequence=1 # Currently unknown how the provided initial values can make physical sense given the parameters of the Crazyflie (or any drone?) # U1 = 0.9 * beta_max # U2 = 0.5 * (beta_max + beta_min) # U3 = beta_min # U4 = U2 # U5 = U1 # theta_dd1 = -(beta_max - U1) / (alpha * L) # theta_dd2 = (beta_max - beta_min) / (2.0 * alpha * L) # theta_dd3 = 0.0 # theta_dd4 = -theta_dd2 # theta_dd5 = (beta_max - U5) / (alpha * L) # T3 = (2 * pi - (theta_d_max ** 2) / (2 * theta_dd2)) # T2 = (theta_d_max - theta_dd1 * T1) print 'Enter to take off' c = raw_input() self.mc.take_off() print 'Enter to flip' c = raw_input() print 'Beginning test...' # Accelerate up tic = time.time() while time.time() - tic < 0.1: self.mc._cf.commander.send_setpoint(0, 0, 0, max_thrust) time.sleep(0.01) # Max differential thrust tic = time.time() while time.time() - tic < 0.14: self.mc._cf.commander.send_setpoint(360 * 8, 0, 0, max_thrust) time.sleep(0.01) # Coast time.sleep(0.01) # Stop rotate tic = time.time() while time.time() - tic < 0.15: self.mc._cf.commander.send_setpoint(-360 * 8, 0, 0, max_thrust) time.sleep(0.01) # Accelerate up tic = time.time() while time.time() - tic < 0.9: self.mc._cf.commander.send_setpoint(0, 0, 0, max_thrust) time.sleep(0.01) while True: self.mc.stop() time.sleep(0.01)
class CrazyflieControl: def __init__(self, name, crazyflie): # Instantiate motion commander self._cf = crazyflie self._name = name self._mc = MotionCommander(self._cf) # Topic Publishers self._velocity_setpoint_pub = rospy.Publisher(self._name + '/velocity_setpoint', Vector3, queue_size=10) """ Services hosted for this crazyflie controller """ self._reset_position_estimator_srv = rospy.Service( self._name + '/reset_position_estimator', ResetPositionEstimator, self._reset_position_estimator_cb) self._send_hover_setpoint_srv = rospy.Service( self._name + '/send_hover_setpoint', SendHoverSetpoint, self._send_hover_setpoint_cb) self._set_param_srv = rospy.Service(self._name + '/set_param', SetParam, self._set_param_cb) self._velocity_control_srv = rospy.Service( self._name + '/velocity_control', VelocityControl, self._velocity_control_cb) """ Action servers for this crazyflie controller """ self._position_control_as = actionlib.SimpleActionServer( self._name + '/position_control', PositionControlAction, self._position_control_cb, False) self._position_control_as.start() """ Service Callbacks """ def _reset_position_estimator_cb(self, req): pass def _send_hover_setpoint_cb(self, req): vx = req.vx vy = req.vy z = req.z yaw_rate = req.yaw_rate self._cf.commander.send_hover_setpoint(vx, vy, yaw_rate, z) return [] def _set_param_cb(self, req): self._cf.param.set_value(req.param, req.value) print("set %s to %s" % (req.param, req.value)) return SetParamResponse() def _velocity_control_cb(self, req): try: obj = pickle.loads(req.pickle) print(self._mc) if isinstance(obj, SetVelSetpoint): self._mc._set_vel_setpoint(obj.vx, obj.vy, obj.vz, obj.rate_yaw) elif isinstance(obj, StartBack): self._mc.start_back(velocity=obj.velocity) elif isinstance(obj, StartCircleLeft): self._mc.start_circle_left(obj.radius_m, velocity=obj.velocity) elif isinstance(obj, StartCircleRight): self._mc.start_turn_right(obj.radius_m, velocity=obj.velocity) elif isinstance(obj, StartDown): self._mc.start_down(velocity=obj.velocity) elif isinstance(obj, StartForward): self._mc.start_forward(velocity=obj.velocity) elif isinstance(obj, StartLeft): self._mc.start_left(velocity=obj.velocity) elif isinstance(obj, StartLinearMotion): self._mc.start_linear_motion(obj.vx, obj.vy, obj.vz) elif isinstance(obj, StartRight): self._mc.start_right(velocity=obj.velocity) elif isinstance(obj, StartTurnLeft): self._mc.start_turn_left(rate=obj.rate) elif isinstance(obj, StartTurnRight): self._mc.start_turn_right(rate=obj.rate) elif isinstance(obj, StartUp): self._mc.start_up(velocity=obj.velocity) elif isinstance(obj, Stop): self._mc.stop() else: return 'Object is not a valid velocity command' except Exception as e: print(str(e)) raise e return 'ok' """ Action Implementations """ def _position_control_cb(self, goal): try: obj = pickle.loads(goal.pickle) if isinstance(obj, Back): self._mc.back(obj.distance_m, velocity=obj.velocity) elif isinstance(obj, CircleLeft): self._mc.circle_left(obj.radius_m, velocity=obj.velocity, angle_degrees=obj.angle_degrees) elif isinstance(obj, CircleRight): self._mc.circle_right(obj.radius_m, velocity=obj.velocity, angle_degrees=obj.angle_degrees) elif isinstance(obj, Down): self._mc.down(obj.distance_m, velocity=obj.velocity) elif isinstance(obj, Forward): self._mc.forward(obj.distance_m, velocity=obj.velocity) elif isinstance(obj, Land): self._mc.land(velocity=obj.velocity) elif isinstance(obj, Left): self._mc.left(obj.distance_m, velocity=obj.velocity) elif isinstance(obj, MoveDistance): self._mc.move_distance(obj.x, obj.y, obj.z, velocity=obj.velocity) elif isinstance(obj, Right): self._mc.right(obj.distance_m, velocity=obj.velocity) elif isinstance(obj, TakeOff): self._mc.take_off(height=obj.height, velocity=obj.velocity) elif isinstance(obj, TurnLeft): self._mc.turn_left(obj.angle_degrees, rate=obj.rate) elif isinstance(obj, TurnRight): self._mc.turn_right(obj.angle_degrees, rate=obj.rate) elif isinstance(obj, Up): self._mc.up(obj.distance_m, velocity=obj.velocity) except Exception as e: print('Exception in action server %s' % self._name + '/position_control') print(str(e)) print('Action aborted') self._position_control_as.set_aborted() return self._position_control_as.set_succeeded() def _takeoff(self, goal): try: self._mc.take_off(height=goal.height) time.sleep(5) except BaseException as e: self._takeoff_as.set_aborted() print(e) return self._takeoff_as.set_succeeded(TakeOffResult(True)) def _land(self, goal): try: self._mc.land(velocity=goal.velocity) except BaseException as e: self._land_as.set_aborted() print(e) return self._land_as.set_succeeded(LandResult(True)) def _move_distance(self, goal): try: x = goal.x y = goal.y z = goal.z velocity = goal.velocity dist = np.sqrt(x**2 + y**2 + z**2) vx = x / dist * velocity vy = y / dist * velocity vz = z / dist * velocity # self._velocity_setpoint_pub.publish(Vector3(vx, vy, vz)) self._mc.move_distance(x, y, z, velocity=velocity) # self._velocity_setpoint_pub.publish(Vector3(vx, vy, vz)) except BaseException as e: self._move_distance_as.set_aborted() print(e) return self._move_distance_as.set_succeeded()
class FlightCtrl: #initialize variables for attitude tracking yawInit = 0 yawCurr = 0 theta = 0 rtCoef = [0, -1] lfCoef = [0, 1] fwCoef = [1, 0] bkCoef = [-1, 0] lvSpeed = 0.3 def __init__(self, _scf): self.mc = MotionCommander(_scf, default_height=0.7) self.scf = _scf self.scf.open_link() def perform_gesture(self, g_id): global consecDoubleTaps global inMotion d = 0.3 if g_id[0] == DOUBLE_TAP: if self.mc._is_flying: print("Hovering...") #mc.stop() self.resetYawInit() else: consecDoubleTaps = 0 print("Taking off...") inMotion = True self.mc.take_off() inMotion = False self.resetYawInit() threadUpdate = Thread(target=self._updateYaw, args=(self.scf, )) threadUpdate.start() elif g_id[0] == NO_POSE and g_id[1] == yaw_id: print("Roll...") inMotion = True self.mc.move_distance(0, math.copysign(d, g_id[2]), 0) inMotion = False """if (g_id[2] > 0): #turn left print("turning left") inMotion = True self.mc.move_distance(self.lvSpeed * self.lfCoef[0], self.lvSpeed * self.lfCoef[1], 0) inMotion = False else: #turn right print("turning right") inMotion = True self.mc.move_distance(self.lvSpeed * self.rtCoef[0], self.lvSpeed * self.rtCoef[1], 0) inMotion = False """ elif g_id[0] == NO_POSE and g_id[1] == pitch_id: print("Pitch...") inMotion = True self.mc.move_distance(-math.copysign(d, g_id[2]), 0, 0) inMotion = False """if (g_id[2] < 0): #move forward print("moving forward") inMotion = True self.mc.move_distance(self.lvSpeed * self.fwCoef[0], self.lvSpeed * self.fwCoef[1], 0) inMotion = False else: #move backward print("moving backward") inMotion = True self.mc.move_distance(self.lvSpeed * self.bkCoef[0], self.lvSpeed * self.bkCoef[1], 0) inMotion = False """ elif g_id[0] == FINGERS_SPREAD and g_id[1] == pitch_id: if g_id[2] > 0: if self.mc._thread.get_height() + d < self.mc.max_height: print("Up...") inMotion = True self.mc.up(d) inMotion = False else: print("Max. height %.2fm reached: requested height: %.2f" ) % (self.mc.max_height, self.mc._thread.get_height() + d) else: if self.mc._thread.get_height() - d < self.mc.min_height: print("Down...") inMotion = True self.mc.down(d) inMotion = False else: print("Max. height %.2fm reached: requested height: %.2f" ) % (self.mc.max_height, self.mc._thread.get_height() + d) elif g_id[0] == FIST and g_id[1] == yaw_id: print('Yaw...') if g_id[2] > 0: inMotion = True self.mc.turn_left(30) inMotion = False else: inMotion = True self.mc.turn_right(30) inMotion = False elif g_id[0] == LAND: print("Landing...") inMotion = True self.mc.land() inMotion = False else: #rest behaviour if self.mc._is_flying: inMotion = True self.mc.stop() inMotion = False """Functions to update attitude by reading storage text file""" def updateCoef(self): try: self.theta = (self.yawCurr - self.yawInit) * (3.1415926 / 180) self.rtCoef = [-sin(self.theta), -cos(self.theta)] self.lfCoef = [sin(self.theta), cos(self.theta)] self.fwCoef = [cos(self.theta), -sin(self.theta)] self.bkCoef = [-cos(self.theta), sin(self.theta)] except Exception, e: print str(e) print("Update failed")
def key_ctrl(self, scf): mc = MotionCommander(scf) mc._reset_position_estimator() print 'Spacebar to start' raw_input() pygame.display.set_mode((400, 300)) print 'WASD for throttle & yaw; arrow keys for left/right/forward/backward' print 'Spacebar to land' vel_x = 0 vel_y = 0 vel_z = 0 yaw_rate = 0 try: mc.take_off() #set inital Yaw value with open('SensorMaster.txt', 'r') as stbFile: stbLines = stbFile.readlines() while len(stbLines) == 0: with open('SensorMaster.txt', 'r') as stbFile: stbLines = stbFile.readlines() currAttitude = stbLines[len(stbLines) - 1] need, to, currentYaw, test = currAttitude.split(',') self.yawCurr = float(currentYaw) Thread(target=self._updateYaw, args=(scf, )).start() while True: for event in pygame.event.get(): if event.type == pygame.KEYDOWN: if event.key == pygame.K_w: vel_z = 0.3 if event.key == pygame.K_SPACE: print 'Space pressed, landing' mc.land() time.sleep(2) print 'Closing link...' scf.close_link() print 'Link closed' pygame.quit() sys.exit(0) if event.key == pygame.K_a: yaw_rate = -45 if event.key == pygame.K_s: vel_z = -0.3 if event.key == pygame.K_d: yaw_rate = 45 if event.key == pygame.K_UP: #vel_x = 0.3 vel_x = 0.2 * self.fwCoef[0] vel_y = 0.2 * self.fwCoef[1] print('vel_x = %.2f, vel_y = %.2f' % (vel_x * self.fwCoef[0], vel_y * self.fwCoef[1])) print("the current yaw is:") print(self.yawCurr) if event.key == pygame.K_DOWN: #vel_x = -0.3 vel_x = 0.2 * self.bkCoef[0] vel_y = 0.2 * self.bkCoef[1] print('vel_x = %.2f, vel_y = %.2f' % (vel_x * self.bkCoef[0], vel_y * self.bkCoef[1])) print("the current yaw is:") print(self.yawCurr) if event.key == pygame.K_LEFT: #vel_y = 0.3 vel_x = 0.2 * self.lfCoef[0] vel_y = 0.2 * self.lfCoef[1] print('vel_x = %.2f, vel_y = %.2f' % (vel_x * self.bkCoef[0], vel_y * self.bkCoef[1])) print("the current yaw is:") print(self.yawCurr) if event.key == pygame.K_RIGHT: #vel_y = -0.3 vel_x = 0.2 * self.rtCoef[0] vel_y = 0.2 * self.rtCoef[1] print('vel_x = %.2f, vel_y = %.2f' % (vel_x * self.bkCoef[0], vel_y * self.bkCoef[1])) print("the current yaw is:") print(self.yawCurr) if event.key == pygame.K_r: #set recalibrate initial Yaw value with open('SensorMaster.txt', 'r') as stbFile: stbLines = stbFile.readlines() while len(stbLines) == 0: with open('SensorMaster.txt', 'r') as stbFile: stbLines = stbFile.readlines() print("yaw angle recalibrated :") newInitAttitude = stbLines[len(stbLines) - 1] need, to, initYaw, test = newInitAttitude.split( ',') print(initYaw) self.yawInit = float(initYaw) if event.key == pygame.K_RCTRL: mc.stop() if event.type == pygame.KEYUP: if event.key == pygame.K_w or event.key == pygame.K_s: vel_z = 0 if event.key == pygame.K_a or event.key == pygame.K_d: yaw_rate = 0 if event.key == pygame.K_UP or event.key == pygame.K_DOWN: vel_x = 0 if event.key == pygame.K_LEFT or event.key == pygame.K_RIGHT: vel_y = 0 mc._set_vel_setpoint(vel_x, vel_y, vel_z, yaw_rate) except KeyboardInterrupt: print('\nShutting down...') scf.close_link() except Exception, e: print str(e) scf.close_link()
class Aruco_tracker(): def __init__(self, distance): """ Inicialização dos drivers, parâmetros do controle PID e decolagem do drone. """ self.distance = distance self.pid_foward = PID(distance, 0.01, 0.0001, 0.01, 500, -500, 0.7, -0.7) self.pid_yaw = PID(0, 0.33, 0.0, 0.33, 500, -500, 100, -100) self.pid_angle = PID(0.0, 0.01, 0.0, 0.01, 500, -500, 0.3, -0.3) self.pid_height = PID(0.0, 0.002, 0.0002, 0.002, 500, -500, 0.3, -0.2) cflib.crtp.init_drivers(enable_debug_driver=False) self.factory = CachedCfFactory(rw_cache='./cache') self.URI4 = 'radio://0/40/2M/E7E7E7E7E4' self.cf = Crazyflie(rw_cache='./cache') self.sync = SyncCrazyflie(self.URI4, cf=self.cf) self.sync.open_link() self.mc = MotionCommander(self.sync) self.cont = 0 self.notFoundCount = 0 self.calculator = DistanceCalculator() self.cap = cv2.VideoCapture(1) self.mc.take_off() time.sleep(1) def search_marker(self): """ Interrompe o movimento se nao encontrar o marcador por tres frames consecutivos. Após 4 segundos, inicia movimento de rotação para procurar pelo marcador. """ if ((3 <= self.notFoundCount <= 20) and self.mc.isRunning): self.mc.stop() self.mc.setIsRunning(False) elif (self.notFoundCount > 20): self.mc._set_vel_setpoint(0.0, 0.0, 0.0, 80) self.mc.setIsRunning(True) return def update_motion(self): """ Atualiza as velocidades utilizando controlador PID. """ horizontal_distance = self.calculator.horizontal_distance vertical_distance = self.calculator.vertical_distance x_distance = self.calculator.distance_x alpha = self.calculator.alpha velocity_x = self.pid_foward.update(x_distance) Velocity_z = self.pid_height.update(vertical_distance) if (x_distance < (self.distance + 10)): velocity_y = self.pid_angle.update(alpha) else: velocity_y = 0 velocity_z = 0 yaw = self.pid_yaw.update(horizontal_distance) self.mc._set_vel_setpoint(velocity_x, velocity_y, 0, yaw) self.mc.setIsRunning(True) return def track_marker(self): """ Programa principal, drone segue um marcador aruco. """ while (self.cap.isOpened()): ret, frame = self.cap.read() if (frame is None): break if (self.cont % 6 == 0): thread = threading.Thread( target=self.calculator.calculateDistance, args=(frame, )) thread.setDaemon(True) thread.start() if (self.calculator.markerFound): self.notFoundCount = 0 self.update_motion() else: self.notFoundCount += 1 self.search_marker() self.calculator.writeDistance(frame) cv2.imshow('frame', frame) self.cont += 1 if cv2.waitKey(10) & 0xFF == ord('q'): self.mc.land() cv2.destroyAllWindows() break cv2.waitKey() self.sync.close_link()
class Drone: def __init__(self, URI): cflib.crtp.init_drivers(enable_debug_driver=False) self.cf = Crazyflie(ro_cache=None, rw_cache='./cache') # Connect callbacks from the Crazyflie API self.cf.connected.add_callback(self.connected) self.cf.disconnected.add_callback(self.disconnected) # Connect to the Crazyflie self.cf.open_link(URI) # Tool to process the data from drone's sensors # self.processing = Processing() self.motion_commander = MotionCommander(self.cf) time.sleep(3) self.motion_commander.take_off(0.3, 0.2) time.sleep(1) self.motion_commander.start_forward(0.15) time.sleep(0.5) self.stop_recording = [] thread.start_new_thread(self.land_command, ()) self.start_mission() def land_command(self): raw_input() self.stop_recording.append(True) print('Landing...') self.motion_commander.stop() self.motion_commander.land(0.2) time.sleep(1) def start_mission(self): rate = rospy.Rate(4000) while not self.stop_recording: print('fly') self.sendHoverCommand() rate.sleep() def sendHoverCommand(self): """ Change UAV flight direction based on obstacles detected with multiranger. """ if is_close(self.measurement['front'] ) and self.measurement['left'] > self.measurement['right']: print('FRONT RIGHT') self.motion_commander.stop() self.motion_commander.turn_left(60, 70) self.motion_commander.start_forward(0.15) if is_close(self.measurement['front'] ) and self.measurement['left'] < self.measurement['right']: print('FRONT LEFT') self.motion_commander.stop() self.motion_commander.turn_right(60, 70) self.motion_commander.start_forward(0.15) if is_close(self.measurement['left']): print('LEFT') self.motion_commander.right(0.1, 0.2) self.motion_commander.stop() self.motion_commander.turn_right(45, 70) self.motion_commander.start_forward(0.15) if is_close(self.measurement['right']): print('RIGHT') self.motion_commander.left(0.1, 0.2) self.motion_commander.stop() self.motion_commander.turn_left(45, 70) self.motion_commander.start_forward(0.15) def disconnected(self, URI): print('Disconnected') def connected(self, URI): print('We are now connected to {}'.format(URI)) # The definition of the logconfig can be made before connecting lpos = LogConfig(name='Position', period_in_ms=100) lpos.add_variable('lighthouse.x') lpos.add_variable('lighthouse.y') lpos.add_variable('lighthouse.z') lpos.add_variable('stabilizer.roll') lpos.add_variable('stabilizer.pitch') lpos.add_variable('stabilizer.yaw') try: self.cf.log.add_config(lpos) lpos.data_received_cb.add_callback(self.pos_data) lpos.start() except KeyError as e: print('Could not start log configuration,' '{} not found in TOC'.format(str(e))) except AttributeError: print('Could not add Position log config, bad configuration.') lmeas = LogConfig(name='Meas', period_in_ms=100) lmeas.add_variable('range.front') lmeas.add_variable('range.back') lmeas.add_variable('range.up') lmeas.add_variable('range.left') lmeas.add_variable('range.right') lmeas.add_variable('range.zrange') lmeas.add_variable('stabilizer.roll') lmeas.add_variable('stabilizer.pitch') lmeas.add_variable('stabilizer.yaw') try: self.cf.log.add_config(lmeas) lmeas.data_received_cb.add_callback(self.meas_data) lmeas.start() except KeyError as e: print('Could not start log configuration,' '{} not found in TOC'.format(str(e))) except AttributeError: print('Could not add Measurement log config, bad configuration.') lbat = LogConfig( name='Battery', period_in_ms=500) # read battery status with 2 Hz rate lbat.add_variable('pm.vbat', 'float') try: self.cf.log.add_config(lbat) lbat.data_received_cb.add_callback(self.battery_data) lbat.start() except KeyError as e: print('Could not start log configuration,' '{} not found in TOC'.format(str(e))) except AttributeError: print('Could not add Measurement log config, bad configuration.') def pos_data(self, timestamp, data, logconf): # Transformation according to https://wiki.bitcraze.io/doc:lighthouse:setup position = [ -data['lighthouse.z'], -data['lighthouse.x'], data['lighthouse.y'] # data['stateEstimate.x'], # data['stateEstimate.y'], # data['stateEstimate.z'] ] orientation = [ data['stabilizer.roll'], data['stabilizer.pitch'], data['stabilizer.yaw'] ] self.position = position self.orientation = orientation # self.processing.set_position(position, orientation) def meas_data(self, timestamp, data, logconf): measurement = { 'roll': data['stabilizer.roll'], 'pitch': data['stabilizer.pitch'], 'yaw': data['stabilizer.yaw'], 'front': data['range.front'], 'back': data['range.back'], 'up': data['range.up'], 'down': data['range.zrange'], 'left': data['range.left'], 'right': data['range.right'] } self.measurement = measurement # self.processing.set_measurement(measurement) def battery_data(self, timestamp, data, logconf): self.V_bat = data['pm.vbat'] # print('Battery status: %.2f [V]' %self.V_bat) if self.V_bat <= V_BATTERY_TO_GO_HOME: self.battery_state = 'needs_charging' # print('Battery is not charged: %.2f' %self.V_bat) elif self.V_bat >= V_BATTERY_CHARGED: self.battery_state = 'fully_charged'