diff --git a/.gitignore b/.gitignore index 2dc53ca..195ec50 100644 --- a/.gitignore +++ b/.gitignore @@ -158,3 +158,4 @@ cython_debug/ # and can be added to the global gitignore or merged into this file. For a more nuclear # option (not recommended) you can uncomment the following to ignore the entire idea folder. .idea/ +.vscode \ No newline at end of file diff --git a/command/drivetrain.py b/command/drivetrain.py new file mode 100644 index 0000000..e69de29 diff --git a/config.py b/config.py index e69de29..9bcf9f2 100644 --- a/config.py +++ b/config.py @@ -0,0 +1,6 @@ +CAN_IDS_DRIVETRAIN = { + 'left_1': 0, + 'left_2': 1, + 'right_1': 2, + 'right_2': 3 +} diff --git a/constants.py b/constants.py index 8b13789..e69de29 100644 --- a/constants.py +++ b/constants.py @@ -1 +0,0 @@ - diff --git a/subsystem/config/subsystem_base.py b/subsystem/config/subsystem_base.py new file mode 100644 index 0000000..4362ae7 --- /dev/null +++ b/subsystem/config/subsystem_base.py @@ -0,0 +1,28 @@ +from typing import Generic, TypeVar +import commands2 + +class Subsystem(commands2.SubsystemBase): + """ + Extendable subsystem + """ + ... + +T = TypeVar("T", bound=Subsystem) + + +class BasicCommand(commands2.CommandBase): + """ + Extendable basic command + """ + ... + + +class SubsystemCommand(BasicCommand, Generic[T]): + """ + Extendable subsystem command + """ + + def __init__(self, subsystem: T): + super().__init__() + self.subsystem = subsystem + self.addRequirements(subsystem) diff --git a/subsystem/drivetrain.py b/subsystem/drivetrain.py new file mode 100644 index 0000000..1471d5a --- /dev/null +++ b/subsystem/drivetrain.py @@ -0,0 +1,46 @@ +import config + +from ctre import TalonFX, ControlMode +from subsystem.config.subsystem_base import Subsystem + +class Drivetrain(Subsystem): + + left_motor_1 = TalonFX(config.CAN_IDS_DRIVETRAIN['left_1']) + right_motor_1 = TalonFX(config.CAN_IDS_DRIVETRAIN['right_1']) + left_motor_2 = TalonFX(config.CAN_IDS_DRIVETRAIN['left_2']) + right_motor_2 = TalonFX(config.CAN_IDS_DRIVETRAIN['right_2']) + + def set_raw_output(self, speed: float, is_left: bool) -> None: + if is_left: + self.left_motor_1.set(ControlMode.PercentOutput, speed) + self.left_motor_2.set(ControlMode.PercentOutput, speed) + else: + self.right_motor_1.set(ControlMode.PercentOutput, speed) + self.right_motor_2.set(ControlMode.PercentOutput, speed) + + def set_velocity(self, velocity: float, is_left: bool) -> None: + if is_left: + self.left_motor_1.set(ControlMode.Velocity, velocity) + self.left_motor_2.set(ControlMode.Velocity, velocity) + else: + self.right_motor_1.set(ControlMode.Velocity, velocity) + self.right_motor_2.set(ControlMode.Velocity, velocity) + + + def get_raw_output(self, is_left: bool) -> float: + if is_left: + return self.left_motor_1.getMotorOutputPercent() + else: + return self.right_motor_1.getMotorOutputPercent() + + def get_velocity(self, is_left: bool) -> float: + if is_left: + return self.left_motor_1.getSelectedSensorVelocity() + else: + return self.right_motor_1.getSelectedSensorVelocity() + + def stop(self) -> None: + self.right_motor_1.set(0) + self.right_motor_2.set(0) + self.left_motor_1.set(0) + self.left_motor_2.set(0) \ No newline at end of file