drivetrain day 1

.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

config.py

@@ -0,0 +1,6 @@
+CAN_IDS_DRIVETRAIN = {
+    'left_1': 0,
+    'left_2': 1,
+    'right_1': 2,
+    'right_2': 3
+}

constants.py

@@ -1 +0,0 @@
-

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)

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)