Test12354

# -*- coding: utf-8 -*-
#
#     ||          ____  _ __
#  +------+      / __ )(_) /_______________ _____  ___
#  | 0xBC |     / __  / / __/ ___/ ___/ __ `/_  / / _ \
#  +------+    / /_/ / / /_/ /__/ /  / /_/ / / /_/  __/
#   ||  ||    /_____/_/\__/\___/_/   \__,_/ /___/\___/
#
#  Copyright (C) 2016 Bitcraze AB
#
#  Crazyflie Nano Quadcopter Client
#
#  This program is free software; you can redistribute it and/or
#  modify it under the terms of the GNU General Public License
#  as published by the Free Software Foundation; either version 2
#  of the License, or (at your option) any later version.
#
#  This program is distributed in the hope that it will be useful,
#  but WITHOUT ANY WARRANTY; without even the implied warranty of
#  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
#  GNU General Public License for more details.
#  You should have received a copy of the GNU General Public License
#  along with this program; if not, write to the Free Software
#  Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston,
#  MA  02110-1301, USA.
"""
Simple example that connects to the crazyflie at `URI` and runs a figure 8
sequence. This script requires some kind of location system, it has been
tested with (and designed for) the flow deck.

Change the URI variable to your Crazyflie configuration.
"""
import logging
import time

import cflib.crtp
from cflib.crazyflie import Crazyflie
from cflib.crazyflie.syncCrazyflie import SyncCrazyflie

URI = 'radio://0/80/250K'

# Only output errors from the logging framework
logging.basicConfig(level=logging.ERROR)


if __name__ == '__main__':
    # Initialize the low-level drivers (don't list the debug drivers)
    cflib.crtp.init_drivers(enable_debug_driver=False)

    with SyncCrazyflie(URI, cf=Crazyflie(rw_cache='./cache')) as scf:
        cf = scf.cf

        cf.param.set_value('kalman.resetEstimation', '1')
        time.sleep(0.1)
        cf.param.set_value('kalman.resetEstimation', '0')
        time.sleep(2) # Maximum time to sleep is 2 secconds

        # Flight parameters
        centerHeight = 0.7
        maxHeight = 1.2
        minHeight = 0.2
        updateRate = 10
        updateSpan = 1 / updateRate
        maxFigures = 3

        # Initial variables
        upwardState = 'Reset'
        shiftSideState = 'Reset'
        figureState = 'BottomLeftTopCircle'
        height = centerHeight
        speed = 0
        fast = 0

        # Begin Flying routine
        cf.commander.send_hover_setpoint(0, 0, 0, centerHeight)
        time.sleep(0.3)
        cf.commander.send_hover_setpoint(0, 0, 0, centerHeight)
        time.sleep(0.3)
        cf.commander.send_hover_setpoint(0, 0, 0, centerHeight)
        time.sleep(0.3)

        figuresDone = 0
        while True:
            currentHeight = height

            # General Figure 8 State Machine
            if figureState == 'BottomLeftTopCircle':
                if height > (centerHeight + (maxHeight - centerHeight) / 2):
                    shiftSide = 0
                    upward = 1
                    figureState = 'TopLeftTopCircle'
                else:
                    shiftSide = -1
                    upward = 1
                    fast = 1
            elif figureState == 'TopLeftTopCircle':
                if height > maxHeight:
                    shiftSide = 0
                    upward = 0
                    figureState = 'TopRightTopCircle'
                else:
                    shiftSide = 1
                    upward = 1
                    fast = 0
            elif figureState == 'TopRightTopCircle':
                if height < (centerHeight + (maxHeight - centerHeight) / 2):
                    shiftSide = 0
                    upward = -1
                    figureState = 'BottomRightTopCircle'
                else:
                    shiftSide = 1
                    upward = -1
                    fast = 1
            elif figureState == 'BottomRightTopCircle':
                if height < centerHeight:
                    shiftSide = 0
                    upward = -1
                    figureState = 'TopLeftBottomCircle'
                else:
                    shiftSide = -1
                    upward = -1
                    fast = 0
            elif figureState == 'TopLeftBottomCircle':
                if height < (centerHeight - (centerHeight - minHeight) / 2):
                    shiftSide = 0
                    upward = -1
                    figureState = 'BottomLeftBottomCircle' # corrected from design
                else:
                    shiftSide = -1
                    upward = -1
                    fast = 1
            elif figureState == 'BottomLeftBottomCircle':
                if height < minHeight:
                    shiftSide = 0
                    upward = 0
                    figureState = 'BottomRightBottomCircle' # corrected from design
                else:
                    shiftSide = 1
                    upward = -1
                    fast = 0
            elif figureState == 'BottomRightBottomCircle':
                if height > (centerHeight - (centerHeight - minHeight) / 2):
                    shiftSide = 0
                    upward = 1
                    figureState = 'TopRightBottomCircle' # corrected from design
                else:
                    shiftSide = 1
                    upward = 1
                    fast = 1
            elif figureState == 'TopRightBottomCircle':
                if height > centerHeight:
                    shiftSide = 0
                    upward = 1
                    figureState = 'BottomLeftTopCircle'
                    figuresDone += 1
                    if figuresDone >= maxFigures:
                        break
                else:
                    shiftSide = -1
                    upward = 1
                    fast = 0

            heightSpeed = 0.2
            # Upward State Machine
            if upwardState == 'Reset':
                if upward == 1:
                    upwardState = 'Upward'
                elif upward == -1:
                    upwardState = 'Downward'
            elif upwardState == 'Upward':
                if upward == 0:
                    upwardState = 'Reset'
                else:
                    height += heightSpeed * updateSpan
            elif upwardState == 'Downward':
                if upward == 0:
                    upwardState = 'Reset'
                else:
                    height -= heightSpeed * updateSpan

            sidewardInitialFastSpeed = 0.4
            timeToCompleteQuarterCircle = ((maxHeight-centerHeight)/2)/heightSpeed
            sideSideDecrementAmount = sidewardInitialFastSpeed/timeToCompleteQuarterCircle
            # ShiftSide State Machine
            if shiftSideState == 'Reset':
                if shiftSide == -1 and fast == 1:
                    shiftSideState = 'DriftLeftFast'
                    speed = -1*sidewardInitialFastSpeed # corrected from design
                elif shiftSide == -1 and fast == 0:
                    shiftSideState = 'DriftLeftSlow'
                    speed = 0
                elif shiftSide == 1 and fast == 1:
                    shiftSideState = 'DriftRightFast'
                    speed = sidewardInitialFastSpeed
                elif shiftSide == 1 and fast == 0:
                    shiftSideState = 'DriftRightSlow'
                    speed = 0
            elif shiftSideState == 'DriftLeftFast':
                if shiftSide == 0:
                    shiftSideState = 'Reset'
                else:
                    speed += sideSideDecrementAmount * updateSpan
            elif shiftSideState == 'DriftLeftSlow':
                if shiftSide == 0:
                    shiftSideState = 'Reset'
                else:
                    speed -= sideSideDecrementAmount * updateSpan
            elif shiftSideState == 'DriftRightFast':
                if shiftSide == 0:
                    shiftSideState = 'Reset'
                else:
                    speed -= sideSideDecrementAmount * updateSpan
            elif shiftSideState == 'DriftRightSlow':
                if shiftSide == 0:
                    shiftSideState = 'Reset'
                else:
                    speed += sideSideDecrementAmount * updateSpan

            # Apply speed and height outputs
            cf.commander.send_hover_setpoint(0, speed, 0, height)
            time.sleep(updateSpan)
            #print("height " + str(height))
            #print("sideState " + shiftSideState + " figure state " + figureState)


        # End flying routine
        cf.commander.send_stop_setpoint()