Advertisement
Guest User

Untitled

a guest
Nov 26th, 2021
117
Never
Not a member of Pastebin yet? Sign Up, it unlocks many cool features!
  1. # -*- coding: utf-8 -*-
  2. #
  3. # || ____ _ __
  4. # +------+ / __ )(_) /_______________ _____ ___
  5. # | 0xBC | / __ / / __/ ___/ ___/ __ `/_ / / _ \
  6. # +------+ / /_/ / / /_/ /__/ / / /_/ / / /_/ __/
  7. # || || /_____/_/\__/\___/_/ \__,_/ /___/\___/
  8. #
  9. # Copyright (C) 2017 Bitcraze AB
  10. #
  11. # Crazyflie Nano Quadcopter Client
  12. #
  13. # This program is free software; you can redistribute it and/or
  14. # modify it under the terms of the GNU General Public License
  15. # as published by the Free Software Foundation; either version 2
  16. # of the License, or (at your option) any later version.
  17. #
  18. # This program is distributed in the hope that it will be useful,
  19. # but WITHOUT ANY WARRANTY; without even the implied warranty of
  20. # MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
  21. # GNU General Public License for more details.
  22. # You should have received a copy of the GNU General Public License
  23. # along with this program; if not, write to the Free Software
  24. # Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston,
  25. # MA 02110-1301, USA.
  26. """
  27. A script to fly 5 Crazyflies in formation. One stays in the center and the
  28. other four fly around it in a circle. Mainly intended to be used with the
  29. Flow deck.
  30. The starting positions are vital and should be oriented like this
  31.  
  32. >
  33.  
  34. ^ + v
  35.  
  36. <
  37.  
  38. The distance from the center to the perimeter of the circle is around 0.5 m
  39.  
  40. """
  41. import math
  42. import time
  43.  
  44.  
  45. import cflib.crtp
  46. from basiclog import LoggingExample
  47. from cflib.crazyflie import Crazyflie
  48. from cflib.crazyflie.log import LogConfig
  49. from cflib.crazyflie.syncCrazyflie import SyncCrazyflie
  50. from cflib.crazyflie.syncLogger import SyncLogger
  51. from cflib.utils import uri_helper
  52. from cflib.crazyflie.swarm import CachedCfFactory
  53. from cflib.crazyflie.swarm import Swarm
  54. from cflib.positioning.motion_commander import MotionCommander
  55. from cflib.utils.multiranger import Multiranger
  56. from datetime import datetime
  57. from threading import Timer
  58.  
  59. # Change uris according to your setup
  60. URI0 = 'radio://0/48/2M/E7E7E7E701'#1
  61. URI1 = 'radio://0/25/2M/E7E7E7E703' #3
  62. URI2 = 'radio://0/50/2M/E7E7E7E702'#2
  63.  
  64. # d: diameter of circle
  65. # z: altitude
  66. params0 = {'d': 1.0, 'z': 0.3}
  67. params1 = {'d': 1.0, 'z': 0.3}
  68. params2 = {'d': 1.0, 'z': 0.3}
  69. #params3 = {'d': 0.0, 'z': 0.5}
  70. #params4 = {'d': 1.0, 'z': 0.3}
  71.  
  72. #addresses for each crazyflie
  73. uris = {
  74. URI0,
  75. URI1,
  76. URI2,
  77. # URI3,
  78. # URI4,
  79. }
  80.  
  81. #parameters for each respective crazyflie
  82. params = {
  83. URI0: [params0],
  84. URI1: [params1],
  85. URI2: [params2],
  86. # URI3: [params3],
  87. # URI4: [params4],
  88. }
  89. def is_close(range):
  90. MIN_DISTANCE = 0.3 # m
  91.  
  92. if range is None:
  93. return False
  94. else:
  95. return range < MIN_DISTANCE
  96.  
  97. #method to run the drone and fly
  98. def run_sequence(scf, params):
  99. with MotionCommander(scf) as motion_commander:
  100. with Multiranger(scf) as multiranger:
  101. keep_flying = True
  102.  
  103. while keep_flying:
  104. motion_commander.start_linear_motion(0.2, 0, 0)
  105. #detects object and will know to turn and continue path
  106. if is_close(multiranger.front) and is_close(multiranger.right) and is_close(multiranger.left):
  107. motion_commander.turn_right(180, 90)
  108. motion_commander.start_forward(0.1)
  109.  
  110. if is_close(multiranger.front) and multiranger.right < multiranger.left:
  111. motion_commander.turn_left(90, 90)
  112.  
  113. if is_close(multiranger.front) and multiranger.right > multiranger.left:
  114. motion_commander.turn_right(90, 90)
  115.  
  116. if is_close(multiranger.front) and is_close(multiranger.right):
  117. motion_commander.turn_left(90, 90)
  118. motion_commander.start_forward(0.1)
  119.  
  120. if is_close(multiranger.front) and is_close(multiranger.left):
  121. motion_commander.turn_right(90, 90)
  122. motion_commander.start_forward(0.1)
  123.  
  124. if is_close(multiranger.back):
  125. motion_commander.start_forward(0.2)
  126.  
  127. if is_close(multiranger.left):
  128. motion_commander.turn_right(45)
  129. motion_commander.start_forward(0.1)
  130.  
  131. if is_close(multiranger.right):
  132. motion_commander.turn_left(45)
  133. motion_commander.start_left(0.1)
  134.  
  135. if is_close(multiranger.up):
  136. motion_commander.land(0.1)
  137. keep_flying = False
  138.  
  139. time.sleep(0.1)
  140.  
  141. if __name__ == '__main__':
  142. cflib.crtp.init_drivers()
  143. factory = CachedCfFactory(rw_cache='./cache')
  144. count = 0
  145. with Swarm(uris, factory=factory) as swarm:
  146. # swarm.reset_estimators()
  147. print("Initiating flight...")
  148. swarm.parallel(run_sequence, args_dict=params)
  149. print("End of Program, Goodbye!\n")
Advertisement
RAW Paste Data Copied
Advertisement