movment mathods

#!/usr/bin/env python3
'''The module that includes all the movements we need for the main algorithm'''

# Import necessary libraries for controlling motors and sensors
from ev3dev2.motor import MediumMotor, LargeMotor, OUTPUT_A, OUTPUT_B, OUTPUT_D, OUTPUT_C, SpeedPercent, MoveTank, MoveSteering
import ev3dev2.motor as motor
from ev3dev2.sensor.lego import GyroSensor, UltrasonicSensor, TouchSensor, ColorSensor
from ev3dev2.sensor import INPUT_1, INPUT_4, INPUT_2, INPUT_3
from time import sleep
import logging
import random
import time

class Movement:
    """This class contains all the general movements needed to perform the search algorithm."""

    def __init__(self):
        # Initialize motors for different parts of the robot
        self.left_Motor = MediumMotor(OUTPUT_B)
        self.right_Motor = MediumMotor(OUTPUT_C)
        self.hiter_Motor = LargeMotor(OUTPUT_A)
        self.catcher_Motor = MediumMotor(OUTPUT_D)
        
        # Initialize drive and steering bases
        self.DriveBase = MoveTank(OUTPUT_B, OUTPUT_C, motor_class=MediumMotor)
        self.SteerBase = MoveSteering(OUTPUT_B, OUTPUT_C, motor_class=MediumMotor)
        self.ReversedSteerBase = MoveSteering(OUTPUT_C, OUTPUT_B, motor_class=MediumMotor)
        
        # Set motor polarity for right motor
        self.right_Motor.polarity = MediumMotor.POLARITY_INVERSED
        
        # Initialize gyro sensor and calibrate it
        self.GyroSensor = GyroSensor(INPUT_4)
        self.GyroSensor.reset()
        self.GyroSensor.calibrate()

    # Method to spin the robot exactly 90 degrees to the right using the gyro sensor
    def Spin_90_Degrees_Right(self):
        """Spins 90 Degrees To The Right Using Gyro"""
        gyro_angle = self.GyroSensor.angle
        while (self.GyroSensor.angle - gyro_angle) < 85:
            self.DriveBase.on(20, -20)
        else:
            self.DriveBase.stop(brake=True)

    # Method to spin the robot exactly 90 degrees to the left using the gyro sensor
    def Spin_90_Degrees_Left(self):
        """Spins 90 Degrees To The Left Using Gyro Sensor"""
        gyro_angle = self.GyroSensor.angle
        while (self.GyroSensor.angle - gyro_angle) > -85:
            self.DriveBase.on(-20, 20)
        else:
            self.DriveBase.stop(brake=True)

    # Method to spin the robot 180 degrees to the right
    def Spin_180_Degrees_Right(self):
        """Spins 180 Degrees To The Right Using Gyro Sensor"""
        gyro_angle = self.GyroSensor.angle
        while (self.GyroSensor.angle - gyro_angle) < 185:
            self.DriveBase.on(10, -10)
        else:
            self.DriveBase.stop(brake=True)

    # Method to move the robot straight for a specified number of degrees
    def Drive_Straight_Degs(self, speed: int, degrees: int):
        """Drives The Robot Straight Forward Or Backward For A Specified Number Of Degrees"""
        self.DriveBase.on_for_degrees(speed, speed, degrees, True, True)

    # Method to move the robot straight until the motors stall
    def Drive_Untill_Stalled(self, speed: int):
        """Drives The Robot Until The Motor Is Stalled (Stuck)"""
        start_time = time.time()
        self.GyroSensor.reset()
        while not self.DriveBase.is_stalled:
            self.DriveBase.on(speed, speed)
            if time.time() - start_time >= 4:
                break
        self.DriveBase.stop(brake=True)

    # Method to continuously move the robot straight until stopped manually
    def Drive_Straight(self, speed: int):
        """Drives The Robot Straight Until Stopped Manually"""
        self.DriveBase.on(speed, speed)

    # Method to turn the robot to a specific angle using the gyro sensor
    def Turn(self, speed: int, angle: int):
        """Turns The Robot To A Specific Angle Using Gyro Sensor"""
        if angle >= 0:
            gyro_angle = self.GyroSensor.angle
            while (self.GyroSensor.angle - gyro_angle) < angle:
                self.DriveBase.on(speed, -speed)
            else:
                self.DriveBase.stop(brake=True)
        elif angle < 0:
            gyro_angle = self.GyroSensor.angle
            while (self.GyroSensor.angle - gyro_angle) > angle:
                self.DriveBase.on(-speed, speed)
            else:
                self.DriveBase.stop(brake=True)

    # Method to perform a long turn by rotating one wheel at a time
    def Long_Turn(self, speed: int, angle: int):
        """Performs A Long Turn By Rotating One Wheel At A Time"""
        if angle > 0:
            if speed > 0:
                gyro_angle = self.GyroSensor.angle
                while (self.GyroSensor.angle - gyro_angle) < angle:
                    self.DriveBase.on(speed, 0)
                else:
                    self.DriveBase.stop(brake=True)
            elif speed < 0:
                gyro_angle = self.GyroSensor.angle
                while (self.GyroSensor.angle - gyro_angle) < angle:
                    self.DriveBase.on(0, speed)
                else:
                    self.DriveBase.stop(brake=True)
        if angle < 0:
            if speed > 0:
                gyro_angle = self.GyroSensor.angle
                while (self.GyroSensor.angle - gyro_angle) > angle:
                    self.DriveBase.on(0, speed)
                else:
                    self.DriveBase.stop(brake=True)
            elif speed < 0:
                gyro_angle = self.GyroSensor.angle
                while (self.GyroSensor.angle - gyro_angle) > angle:
                    self.DriveBase.on(speed, 0)
                else:
                    self.DriveBase.stop(brake=True)

    # Method to drive straight using a PD controller with the gyro sensor
    def Gyro_Straight_Move(self, speed: int, continue_: int):
        """Drives The Robot Very Straight Using A PD Controller With Gyro Sensor"""
        kp = 1.2
        ki = 0
        kd = 0.5
        target_angle = 0
        integral = 0
        last_error = 0
        max_speed = speed
        self.left_Motor.reset()
        self.GyroSensor.reset()
        while continue_ == 1:
            error = target_angle - self.GyroSensor.angle
            proportional = kp * error
            integral += error
            integral_term = ki * integral
            derivative = error - last_error
            derivative_term = kd * derivative
            turn_rate = proportional + integral_term + derivative_term
            turn_rate = max(-max_speed, min(max_speed, turn_rate))
            left_motor_speed = SpeedPercent(max_speed + turn_rate)
            right_motor_speed = SpeedPercent(max_speed - turn_rate)
            self.left_Motor.on(left_motor_speed)
            self.right_Motor.on(right_motor_speed)
            last_error = error
            sleep(0.01)
        self.DriveBase.stop(brake=False)

    # Method to drive straight for a specified number of degrees using a PD controller with the gyro sensor
    def Gyro_Straight_Move_Degs(self, speed: int, degrees: int):
        """Drives The Robot Very Straight Using A PD Controller With Gyro Sensor For A Specified Number Of Degrees"""
        kp = 0.3
        ki = 0.01
        kd = 0.9
        target_angle = 0
        integral = 0
        last_error = 0
        max_speed = speed
        self.left_Motor.reset()
        self.GyroSensor.reset()
        while self.left_Motor.degrees < degrees:
            error = target_angle - self.GyroSensor.angle
            proportional = kp * error
            integral += error
            integral_term = ki * integral
            derivative = error - last_error
            derivative_term = kd * derivative
            turn_rate = proportional + integral_term + derivative_term
            turn_rate = max(-max_speed, min(max_speed, turn_rate))
            left_motor_speed = SpeedPercent(max_speed + turn_rate)
            right_motor_speed = SpeedPercent(max_speed - turn_rate)
            self.left_Motor.on(left_motor_speed)
            self.right_Motor.on(right_motor_speed)
            last_error = error
            sleep(0.01)
        self.right_Motor.stop()
        self.left_Motor.stop()

    # Method to drive straight until the robot is stalled using a PD controller with the gyro sensor
    def Gyro_Straight_Move_Until_Stalled(self, speed: int):
        """Drives The Robot Very Straight Using A PD Controller With Gyro Sensor Until The Robot Is Stalled"""
        kp = 0.3
        ki = 0.01
        kd = 0.9
        target_angle = 0
        integral = 0
        last_error = 0
        max_speed = speed
        self.left_Motor.reset()
        self.GyroSensor.reset()
        while