SHARE
TWEET

Test12354

a guest Nov 13th, 2019 93 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) 2016 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. Simple example that connects to the crazyflie at `URI` and runs a figure 8
  28. sequence. This script requires some kind of location system, it has been
  29. tested with (and designed for) the flow deck.
  30.  
  31. Change the URI variable to your Crazyflie configuration.
  32. """
  33. import logging
  34. import time
  35.  
  36. import cflib.crtp
  37. from cflib.crazyflie import Crazyflie
  38. from cflib.crazyflie.syncCrazyflie import SyncCrazyflie
  39.  
  40. URI = 'radio://0/80/250K'
  41.  
  42. # Only output errors from the logging framework
  43. logging.basicConfig(level=logging.ERROR)
  44.  
  45.  
  46. if __name__ == '__main__':
  47.     # Initialize the low-level drivers (don't list the debug drivers)
  48.     cflib.crtp.init_drivers(enable_debug_driver=False)
  49.  
  50.     with SyncCrazyflie(URI, cf=Crazyflie(rw_cache='./cache')) as scf:
  51.         cf = scf.cf
  52.  
  53.         cf.param.set_value('kalman.resetEstimation', '1')
  54.         time.sleep(0.1)
  55.         cf.param.set_value('kalman.resetEstimation', '0')
  56.         time.sleep(2) # Maximum time to sleep is 2 secconds
  57.  
  58.         # Flight parameters
  59.         centerHeight = 0.7
  60.         maxHeight = 1.2
  61.         minHeight = 0.2
  62.         updateRate = 10
  63.         updateSpan = 1 / updateRate
  64.         maxFigures = 3
  65.  
  66.         # Initial variables
  67.         upwardState = 'Reset'
  68.         shiftSideState = 'Reset'
  69.         figureState = 'BottomLeftTopCircle'
  70.         height = centerHeight
  71.         speed = 0
  72.         fast = 0
  73.  
  74.         # Begin Flying routine
  75.         cf.commander.send_hover_setpoint(0, 0, 0, centerHeight)
  76.         time.sleep(0.3)
  77.         cf.commander.send_hover_setpoint(0, 0, 0, centerHeight)
  78.         time.sleep(0.3)
  79.         cf.commander.send_hover_setpoint(0, 0, 0, centerHeight)
  80.         time.sleep(0.3)
  81.  
  82.         figuresDone = 0
  83.         while True:
  84.             currentHeight = height
  85.  
  86.             # General Figure 8 State Machine
  87.             if figureState == 'BottomLeftTopCircle':
  88.                 if height > (centerHeight + (maxHeight - centerHeight) / 2):
  89.                     shiftSide = 0
  90.                     upward = 1
  91.                     figureState = 'TopLeftTopCircle'
  92.                 else:
  93.                     shiftSide = -1
  94.                     upward = 1
  95.                     fast = 1
  96.             elif figureState == 'TopLeftTopCircle':
  97.                 if height > maxHeight:
  98.                     shiftSide = 0
  99.                     upward = 0
  100.                     figureState = 'TopRightTopCircle'
  101.                 else:
  102.                     shiftSide = 1
  103.                     upward = 1
  104.                     fast = 0
  105.             elif figureState == 'TopRightTopCircle':
  106.                 if height < (centerHeight + (maxHeight - centerHeight) / 2):
  107.                     shiftSide = 0
  108.                     upward = -1
  109.                     figureState = 'BottomRightTopCircle'
  110.                 else:
  111.                     shiftSide = 1
  112.                     upward = -1
  113.                     fast = 1
  114.             elif figureState == 'BottomRightTopCircle':
  115.                 if height < centerHeight:
  116.                     shiftSide = 0
  117.                     upward = -1
  118.                     figureState = 'TopLeftBottomCircle'
  119.                 else:
  120.                     shiftSide = -1
  121.                     upward = -1
  122.                     fast = 0
  123.             elif figureState == 'TopLeftBottomCircle':
  124.                 if height < (centerHeight - (centerHeight - minHeight) / 2):
  125.                     shiftSide = 0
  126.                     upward = -1
  127.                     figureState = 'BottomLeftBottomCircle' # corrected from design
  128.                 else:
  129.                     shiftSide = -1
  130.                     upward = -1
  131.                     fast = 1
  132.             elif figureState == 'BottomLeftBottomCircle':
  133.                 if height < minHeight:
  134.                     shiftSide = 0
  135.                     upward = 0
  136.                     figureState = 'BottomRightBottomCircle' # corrected from design
  137.                 else:
  138.                     shiftSide = 1
  139.                     upward = -1
  140.                     fast = 0
  141.             elif figureState == 'BottomRightBottomCircle':
  142.                 if height > (centerHeight - (centerHeight - minHeight) / 2):
  143.                     shiftSide = 0
  144.                     upward = 1
  145.                     figureState = 'TopRightBottomCircle' # corrected from design
  146.                 else:
  147.                     shiftSide = 1
  148.                     upward = 1
  149.                     fast = 1
  150.             elif figureState == 'TopRightBottomCircle':
  151.                 if height > centerHeight:
  152.                     shiftSide = 0
  153.                     upward = 1
  154.                     figureState = 'BottomLeftTopCircle'
  155.                     figuresDone += 1
  156.                     if figuresDone >= maxFigures:
  157.                         break
  158.                 else:
  159.                     shiftSide = -1
  160.                     upward = 1
  161.                     fast = 0
  162.  
  163.             heightSpeed = 0.2
  164.             # Upward State Machine
  165.             if upwardState == 'Reset':
  166.                 if upward == 1:
  167.                     upwardState = 'Upward'
  168.                 elif upward == -1:
  169.                     upwardState = 'Downward'
  170.             elif upwardState == 'Upward':
  171.                 if upward == 0:
  172.                     upwardState = 'Reset'
  173.                 else:
  174.                     height += heightSpeed * updateSpan
  175.             elif upwardState == 'Downward':
  176.                 if upward == 0:
  177.                     upwardState = 'Reset'
  178.                 else:
  179.                     height -= heightSpeed * updateSpan
  180.  
  181.             sidewardInitialFastSpeed = 0.4
  182.             timeToCompleteQuarterCircle = ((maxHeight-centerHeight)/2)/heightSpeed
  183.             sideSideDecrementAmount = sidewardInitialFastSpeed/timeToCompleteQuarterCircle
  184.             # ShiftSide State Machine
  185.             if shiftSideState == 'Reset':
  186.                 if shiftSide == -1 and fast == 1:
  187.                     shiftSideState = 'DriftLeftFast'
  188.                     speed = -1*sidewardInitialFastSpeed # corrected from design
  189.                 elif shiftSide == -1 and fast == 0:
  190.                     shiftSideState = 'DriftLeftSlow'
  191.                     speed = 0
  192.                 elif shiftSide == 1 and fast == 1:
  193.                     shiftSideState = 'DriftRightFast'
  194.                     speed = sidewardInitialFastSpeed
  195.                 elif shiftSide == 1 and fast == 0:
  196.                     shiftSideState = 'DriftRightSlow'
  197.                     speed = 0
  198.             elif shiftSideState == 'DriftLeftFast':
  199.                 if shiftSide == 0:
  200.                     shiftSideState = 'Reset'
  201.                 else:
  202.                     speed += sideSideDecrementAmount * updateSpan
  203.             elif shiftSideState == 'DriftLeftSlow':
  204.                 if shiftSide == 0:
  205.                     shiftSideState = 'Reset'
  206.                 else:
  207.                     speed -= sideSideDecrementAmount * updateSpan
  208.             elif shiftSideState == 'DriftRightFast':
  209.                 if shiftSide == 0:
  210.                     shiftSideState = 'Reset'
  211.                 else:
  212.                     speed -= sideSideDecrementAmount * updateSpan
  213.             elif shiftSideState == 'DriftRightSlow':
  214.                 if shiftSide == 0:
  215.                     shiftSideState = 'Reset'
  216.                 else:
  217.                     speed += sideSideDecrementAmount * updateSpan
  218.  
  219.             # Apply speed and height outputs
  220.             cf.commander.send_hover_setpoint(0, speed, 0, height)
  221.             time.sleep(updateSpan)
  222.             #print("height " + str(height))
  223.             #print("sideState " + shiftSideState + " figure state " + figureState)
  224.  
  225.  
  226.         # End flying routine
  227.         cf.commander.send_stop_setpoint()
RAW Paste Data
We use cookies for various purposes including analytics. By continuing to use Pastebin, you agree to our use of cookies as described in the Cookies Policy. OK, I Understand
Not a member of Pastebin yet?
Sign Up, it unlocks many cool features!
 
Top