# Copyright (c) FIRST and other WPILib contributors.
# Open Source Software; you can modify and/or share it under the terms of
# the WPILib BSD license file in the root directory of this project.

import math

import commands2
import wpilib
import wpilib.drive

from sensors.romigyro import RomiGyro


class Drivetrain(commands2.SubsystemBase):

    kCountsPerRevolution = 1440.0
    kWheelDiameterInch = 2.75591

    def __init__(self) -> None:
        super().__init__()

        # The Romi has the left and right motors set to
        # PWM channels 0 and 1 respectively
        self.leftMotor = wpilib.Spark(0)
        self.rightMotor = wpilib.Spark(1)
        # The new controller is connected to PWM channel
        self.rearMotor = wpilib.Spark(2)

        # The Romi has onboard encoders that are hardcoded
        # to use DIO pins 4/5 and 6/7 for the left and right
        self.leftEncoder = wpilib.Encoder(4, 5)
        self.rightEncoder = wpilib.Encoder(6, 7)
        # Encoders from the third motor are connected to EXT 3 & 4
        self.rearEncoder = wpilib.Encoder(10,11)

        # Set up the Killough drive controller
        self.drive = wpilib.drive.KilloughDrive(self.leftMotor, self.rightMotor, self.rearMotor)

        # Set up the RomiGyro
        self.gyro = RomiGyro()

        # Set up the BuiltInAccelerometer
        self.accelerometer = wpilib.BuiltInAccelerometer()

        # Use inches as unit for encoder distances
        self.leftEncoder.setDistancePerPulse(
            (math.pi * self.kWheelDiameterInch) / self.kCountsPerRevolution
        )
        self.rightEncoder.setDistancePerPulse(
            (math.pi * self.kWheelDiameterInch) / self.kCountsPerRevolution
        )
        self.rearEncoder.setDistancePerPulse(
            (math.pi * self.kWheelDiameterInch) / self.kCountsPerRevolution
        )
        self.resetEncoders()

    def driveCartesian(self, fwd: float, sdw: float, rot: float) -> None:
        """
        Drives the robot using arcade style controls.

        :param fwd: the commanded forward movement
        :param sdw: the commanded sideways movement
        :param rot: the commanded rotation
        """
        self.drive.driveCartesian(fwd, sdw, rot)

    def resetEncoders(self) -> None:
        """Resets the drive encoders to currently read a position of 0."""
        self.leftEncoder.reset()
        self.rightEncoder.reset()
        self.rearEncoder.reset()

    def getLeftEncoderCount(self) -> int:
        return self.leftEncoder.get()

    def getRightEncoderCount(self) -> int:
        return self.rightEncoder.get()

    def getRearEncoderCount(self) -> int:
        return self.rearEncoder.get()

    def getLeftDistanceInch(self) -> float:
        return self.leftEncoder.getDistance()

    def getRightDistanceInch(self) -> float:
        return self.rightEncoder.getDistance()

    def getRearDistanceInch(self) -> float:
        return self.rearEncoder.getDistance()

    def getAverageDistanceInch(self) -> float:
        """Gets the average distance of the TWO encoders."""
        return (self.getLeftDistanceInch() + self.getRightDistanceInch() + self.getRearDistanceInch()) / 3.0

    def getAccelX(self) -> float:
        """The acceleration in the X-axis.

        :returns: The acceleration of the Romi along the X-axis in Gs
        """
        return self.accelerometer.getX()

    def getAccelY(self) -> float:
        """The acceleration in the Y-axis.

        :returns: The acceleration of the Romi along the Y-axis in Gs
        """
        return self.accelerometer.getY()

    def getAccelZ(self) -> float:
        """The acceleration in the Z-axis.

        :returns: The acceleration of the Romi along the Z-axis in Gs
        """
        return self.accelerometer.getZ()

    def getGyroAngleX(self) -> float:
        """Current angle of the Romi around the X-axis.

        :returns: The current angle of the Romi in degrees
        """
        return self.gyro.getAngleX()

    def getGyroAngleY(self) -> float:
        """Current angle of the Romi around the Y-axis.

        :returns: The current angle of the Romi in degrees
        """
        return self.gyro.getAngleY()

    def getGyroAngleZ(self) -> float:
        """Current angle of the Romi around the Z-axis.

        :returns: The current angle of the Romi in degrees
        """
        return self.gyro.getAngleZ()

    def resetGyro(self) -> None:
        """Reset the gyro"""
        self.gyro.reset()

# Copyright (c) FIRST and other WPILib contributors.
# Open Source Software; you can modify and/or share it under the terms of
# the WPILib BSD license file in the root directory of this project.

import math

import commands2
import wpilib
import wpilib.drive

from sensors.romigyro import RomiGyro


class Drivetrain(commands2.SubsystemBase):

    kCountsPerRevolution = 1440.0
    kWheelDiameterInch = 2.75591

    def __init__(self) -> None:
        super().__init__()

        # The Romi has the left and right motors set to
        # PWM channels 0 and 1 respectively
        self.leftMotor = wpilib.Spark(0)
        self.rightMotor = wpilib.Spark(1)
        # The new controller is connected to PWM channel
        self.rearMotor = wpilib.Spark(2)

        # The Romi has onboard encoders that are hardcoded
        # to use DIO pins 4/5 and 6/7 for the left and right
        self.leftEncoder = wpilib.Encoder(4, 5)
        self.rightEncoder = wpilib.Encoder(6, 7)
        # Encoders from the third motor are connected to EXT 3 & 4
        self.rearEncoder = wpilib.Encoder(10,11)

        # Set up the Killough drive controller
        self.drive = wpilib.drive.KilloughDrive(self.leftMotor, self.rightMotor, self.rearMotor)

        # Set up the RomiGyro
        self.gyro = RomiGyro()

        # Set up the BuiltInAccelerometer
        self.accelerometer = wpilib.BuiltInAccelerometer()

        # Use inches as unit for encoder distances
        self.leftEncoder.setDistancePerPulse(
            (math.pi * self.kWheelDiameterInch) / self.kCountsPerRevolution
        )
        self.rightEncoder.setDistancePerPulse(
            (math.pi * self.kWheelDiameterInch) / self.kCountsPerRevolution
        )
        self.rearEncoder.setDistancePerPulse(
            (math.pi * self.kWheelDiameterInch) / self.kCountsPerRevolution
        )
        self.resetEncoders()

    def driveCartesian(self, fwd: float, sdw: float, rot: float) -> None:
        """
        Drives the robot using arcade style controls.

        :param fwd: the commanded forward movement
        :param sdw: the commanded sideways movement
        :param rot: the commanded rotation
        """
        self.drive.driveCartesian(fwd, sdw, rot)

    def resetEncoders(self) -> None:
        """Resets the drive encoders to currently read a position of 0."""
        self.leftEncoder.reset()
        self.rightEncoder.reset()
        self.rearEncoder.reset()

    def getLeftEncoderCount(self) -> int:
        return self.leftEncoder.get()

    def getRightEncoderCount(self) -> int:
        return self.rightEncoder.get()

    def getRearEncoderCount(self) -> int:
        return self.rearEncoder.get()

    def getLeftDistanceInch(self) -> float:
        return self.leftEncoder.getDistance()

    def getRightDistanceInch(self) -> float:
        return self.rightEncoder.getDistance()

    def getRearDistanceInch(self) -> float:
        return self.rearEncoder.getDistance()

    def getAverageDistanceInch(self) -> float:
        """Gets the average distance of the TWO encoders."""
        return (self.getLeftDistanceInch() + self.getRightDistanceInch() + self.getRearDistanceInch()) / 3.0

    def getAccelX(self) -> float:
        """The acceleration in the X-axis.

        :returns: The acceleration of the Romi along the X-axis in Gs
        """
        return self.accelerometer.getX()

    def getAccelY(self) -> float:
        """The acceleration in the Y-axis.

        :returns: The acceleration of the Romi along the Y-axis in Gs
        """
        return self.accelerometer.getY()

    def getAccelZ(self) -> float:
        """The acceleration in the Z-axis.

        :returns: The acceleration of the Romi along the Z-axis in Gs
        """
        return self.accelerometer.getZ()

    def getGyroAngleX(self) -> float:
        """Current angle of the Romi around the X-axis.

        :returns: The current angle of the Romi in degrees
        """
        return self.gyro.getAngleX()

    def getGyroAngleY(self) -> float:
        """Current angle of the Romi around the Y-axis.

        :returns: The current angle of the Romi in degrees
        """
        return self.gyro.getAngleY()

    def getGyroAngleZ(self) -> float:
        """Current angle of the Romi around the Z-axis.

        :returns: The current angle of the Romi in degrees
        """
        return self.gyro.getAngleZ()

    def resetGyro(self) -> None:
        """Reset the gyro"""
        self.gyro.reset()

# Copyright (c) FIRST and other WPILib contributors. # Open Source Software; you can modify and/or share it under the terms of # the WPILib BSD license file in the root directory of this project. # The Romi has the left and right motors set to # PWM channels 0 and 1 respectively # The new controller is connected to PWM channel # The Romi has onboard encoders that are hardcoded # to use DIO pins 4/5 and 6/7 for the left and right # Encoders from the third motor are connected to EXT 3 & 4 # Set up the Killough drive controller # Set up the RomiGyro # Set up the BuiltInAccelerometer # Use inches as unit for encoder distances Drives the robot using arcade style controls.          :param fwd: the commanded forward movement         :param sdw: the commanded sideways movement         :param rot: the commanded rotation Resets the drive encoders to currently read a position of 0. Gets the average distance of the TWO encoders. The acceleration in the X-axis.          :returns: The acceleration of the Romi along the X-axis in Gs The acceleration in the Y-axis.          :returns: The acceleration of the Romi along the Y-axis in Gs The acceleration in the Z-axis.          :returns: The acceleration of the Romi along the Z-axis in Gs Current angle of the Romi around the X-axis.          :returns: The current angle of the Romi in degrees Current angle of the Romi around the Y-axis.          :returns: The current angle of the Romi in degrees Current angle of the Romi around the Z-axis.          :returns: The current angle of the Romi in degrees Reset the gyro