crazyflie_controller_win_project_py.py

# Crazyflie autonomous control, [email protected]

from controller import Receiver, Emitter
from controller import Robot
from crazyflie_flyingcontroller import CrazyflieFlyingController
from controller import Keyboard

import math
import numpy as np


class Crazyflie:

    def __init__(self):
        self.robot = Robot()
        self.timestep = int(self.robot.getBasicTimeStep())
        self.crazyflie_controller = CrazyflieFlyingController(
            self.robot, self.timestep)
        self.pitch_vel = 0
        self.roll_vel = 0
        self.yaw_vel = 0
        self.altitude = 0.25

        self.keyboard = Keyboard()
        self.keyboard.enable(self.timestep)
        self.homepos_x = float('nan')
        self.location_history = []

    def save_location(self):
        self.location_history.append({
            "position": self.crazyflie_controller.getPosition(),
            "time": self.robot.getTime()
        })
        # print("Location saved: ", self.location_history)

    def manual_fly(self):
        self.past_time = self.robot.getTime()

        print("\n")
        print("====== Controls =======\n\n")
        print(" The Crazyflie can be controlled from your keyboard lllllll!\n")
        print(" All controllable movement is in body coordinates\n")
        print("- Use the up, back, right and left button to move in the horizontal plane\n")
        print("- Use Q and E to rotate around yaw ")
        print("- Use W and S to go up and down\n ")
        print("- Use P to set the desired position (same as home position with target altitude) ")
        print("- Use H to return to home\n ")
        print("- Use D to set a separate desired position (replace with your logic) ")
        print("- Use A to fly autonomously to the desired position\n ")

        while self.robot.step(self.timestep) != -1:
            dt = self.robot.getTime() - self.past_time
            self.pitch_vel = 0
            self.roll_vel = 0
            self.yaw_vel = 0
            self.altitude = 0.25
            key = self.keyboard.getKey()
            while key > 0:
                if key == Keyboard.UP:
                    self.pitch_vel = 0.2
                elif key == Keyboard.DOWN:
                    self.pitch_vel = -0.2
                elif key == Keyboard.RIGHT:
                    self.roll_vel = -0.2
                elif key == Keyboard.LEFT:
                    self.roll_vel = 0.2
                elif key == ord('Q'):
                    self.yaw_vel = + 1
                elif key == ord('E'):
                    self.yaw_vel = - 1
                elif key == ord('W'):
                    self.altitude += 0.1
                elif key == ord('S'):
                    self.altitude -= 0.1
                elif key == ord('H'):
                    if not math.isnan(self.homepos_x):
                        self.auto_fly()
                    else:
                        print("you need to set a home position first")
                elif key == ord('P'):
                    self.homepos_x, self.homepos_y, self.homepos_z = self.crazyflie_controller.getPosition()
                    # Set desired position to home position with target altitude of 2.5 meters
                    self.desired_position = np.array(
                        [self.homepos_x, self.homepos_y, 2.5]
                    )
                    print(
                        "Desired position set to home position with target altitude: ", self.desired_position
                    )
                elif key == ord('D'):
                    # Implement logic to get a separate desired position (not covered here)
                    pass
                elif key == ord('A'):
                    if self.desired_position is not None:
                        self.fly_to_desired_position()
                    else:
                        print("No desired position set!")
                self.save_location()
            # Check for failsafe condition (replace with your specific logic)
            self.crazyflie_controller.move_vel(
                dt, self.pitch_vel, self.roll_vel, self.yaw_vel, self.altitude
            )
            self.past_time = self.robot.getTime()

    def failsafe_handler(self):
        print("Failsafe triggered!")

        # Reduce velocity to minimum for a short duration (replace 0.1 with desired time)
        # Loop for a specific duration based on timestep
        for _ in range(int(0.1 * self.timestep)):
            self.crazyflie_controller.move_vel(
                self.timestep, 0.01, 0.01, 0.0, self.altitude)  # Set minimal velocities

        # Descend to a safe altitude (e.g., 0.5 meters)
        while self.altitude > 0.5:
            self.altitude -= 0.05  # Adjust descent speed as needed
        self.crazyflie_controller.move_vel(
            self.timestep, 0, 0, 0, self.altitude)

        # Disarm motors
        self.crazyflie_controller.disarm()
        print("Landed safely due to failsafe!")

    def fly_to_desired_position(self):
        if self.desired_position is None:
            print("No desired position set!")
            return

        # Get current position
        current_position = self.crazyflie_controller.getPosition()

        # Calculate distance vector to desired position
        distance_vector = self.desired_position - current_position

        def scaled_velocity(distance):
            # Implement a smoother scaling function here (e.g., proportional control)
            if distance < 0.2:  # Within a small tolerance of the target
                return 0.1  # Reduce velocity significantly near the target
            else:
                return 0.3  # Maintain a moderate velocity for most of the flight

        # Move towards the desired position with scaled velocity
        # Stop when within 0.1 meters of target
        while np.linalg.norm(distance_vector) > 0.1:
            velocity = scaled_velocity(np.linalg.norm(distance_vector))
            self.crazyflie_controller.move_xyz(
                self.timestep, distance_vector / np.linalg.norm(distance_vector) * velocity)
            distance_vector = self.desired_position - \
                self.crazyflie_controller.getPosition()  # Update distance vector
        print("Reached desired position.")

    def auto_fly(self):
        self.past_time = self.robot.getTime()

        while self.robot.step(self.timestep) != -1:
            dt = self.robot.getTime() - self.past_time

            desiredwaypoint = np.array([self.homepos_x, self.homepos_y, 0.5])
            # desiredwaypoint = np.array([0.0, 0.1, 0.5])
            self.crazyflie_controller.move_xyz(dt, desiredwaypoint)

            self.past_time = self.robot.getTime()

            current_position = self.crazyflie_controller.getPosition()
            distance_vector = self.desired_position - current_position

            # Scale velocity based on distance (replace with your logic)
            # This is a simple example, you can implement a smoother scaling function
            # Within a small tolerance of the target
            if np.linalg.norm(distance_vector) < 0.1:
                scaled_velocity = 0.1  # Reduce velocity significantly near the target
            else:
                scaled_velocity = 0.3  # Maintain a moderate velocity for most of the flight

            # Move towards the desired position with scaled velocity
            self.crazyflie_controller.move_xyz(
                self.timestep, distance_vector / np.linalg.norm(distance_vector) * scaled_velocity)

            print("Moving towards desired position...")


if __name__ == "__main__":
    crazyflie = Crazyflie()
    crazyflie.manual_fly()