Move hardware options, tidy up drivetrain constants generation

Goals before merging with feat/magicSwerve, get simulator printing turning angles, make stable (ish), comment and clean more

rio/components/drivetrain.py

@@ -5,7 +5,7 @@
 
 from commands2 import Subsystem
 from wpimath.filter import SlewRateLimiter
-from wpimath.geometry import Pose2d, Rotation2d
+from wpimath.geometry import Pose2d, Rotation2d, Translation2d
 from wpimath.kinematics import (
     ChassisSpeeds,
     SwerveModuleState,
@@ -15,38 +15,49 @@
 from navx import AHRS
 from constants.complexConstants import DriveConstants
 import swerveutils
-from .maxswervemodule import MAXSwerveModule
+from .swerveModule import MikeSwerveModule
 
 from constants.getConstants import getConstants
 
 
 class DriveSubsystem(Subsystem):
     def __init__(self) -> None:
         super().__init__()
-        # hardware constants
-        hardwareconstants = getConstants("simple_hardware")
-        self.driveConsts = hardwareconstants["drive"]
+
+        # Get Hardware constants
+        hardwareConsts = getConstants("robotHardware")
+        # Get Swerve Modules
+        moduleConstants = hardwareConsts["modules"]
+
+        # Build constants (these are copied from old complexConstants.py)
+        modulePositions = []  # All the module positions
+        for module in moduleConstants:  # For every module in the moduleConstants list
+            # Create a translation2d that represents its pose
+            modulePositions.append(Translation2d(module["pose"][0], module["pose"][0]))
+
+        # Build the kinematics
+        kDriveKinematics = SwerveDrive4Kinematics(*modulePositions)
 
         # Create MAXSwerveModules
-        self.frontLeft = MAXSwerveModule(
+        self.frontLeft = MikeSwerveModule(
             self.driveConsts["kFrontLeftDrivingCanId"],
             self.driveConsts["kFrontLeftTurningCanId"],
             DriveConstants.kFrontLeftChassisAngularOffset,
         )
 
-        self.frontRight = MAXSwerveModule(
+        self.frontRight = MikeSwerveModule(
             self.driveConsts["kFrontRightDrivingCanId"],
             self.driveConsts["kFrontRightTurningCanId"],
             DriveConstants.kFrontRightChassisAngularOffset,
         )
 
-        self.rearLeft = MAXSwerveModule(
+        self.rearLeft = MikeSwerveModule(
             self.driveConsts["kRearLeftDrivingCanId"],
             self.driveConsts["kRearLeftTurningCanId"],
             DriveConstants.kBackLeftChassisAngularOffset,
         )
 
-        self.rearRight = MAXSwerveModule(
+        self.rearRight = MikeSwerveModule(
             self.driveConsts["kRearRightDrivingCanId"],
             self.driveConsts["kRearRightTurningCanId"],
             DriveConstants.kBackRightChassisAngularOffset,

rio/components/maxswervemodule.py → rio/components/swerveModule.py

@@ -6,20 +6,20 @@
 from constants.getConstants import getConstants
 
 
-class MAXSwerveModule:
+class MikeSwerveModule:
     def __init__(
         self, drivingCANId: int, turningCANId: int, chassisAngularOffset: float
     ) -> None:
-        """Constructs a MAXSwerveModule and configures the driving and turning motor,
-        encoder, and PID controller. This configuration is specific to the REV
-        MAXSwerve Module built with NEOs, SPARKS MAX, and a Through Bore
-        Encoder.
         """
-        # hardware constants
-        hardwareconstants = getConstants("simple_hardware")
+        Construct a custom swerve module, this model is similar to a MAXSwerveModule but
+        using our custom construction.
+        """
+
+        # Hardware constants
+        hardwareconstants = getConstants("robotHardware")
         self.moduleConsts = hardwareconstants["module"]
 
-        # pid constants
+        # PID constants
         pidconstants = getConstants("simple_pid")
         self.pidConsts = pidconstants["PID"]
 

rio/constants/robotHardware.yaml

@@ -0,0 +1,58 @@
+# There are four swerve modules that make up a drivetrain
+modules:
+  frontRight:
+    CANSteerID: 0 # Configured Never
+    CANDriveID: 1 # Configured Never
+    Pose: [1, 1] # Configured Never
+  frontLeft:
+    CANSteerID: 2 # Configured Never
+    CANDriveID: 3 # Configured Never
+    Pose: [-1, 1] # Configured Never
+  rearRight:
+    CANSteerID: 4 # Configured Never
+    CANDriveID: 5 # Configured Never
+    Pose: [1, -1] # Configured Never
+  rearLeft:
+    CANSteerID: 6 # Configured Never
+    CANDriveID: 7 # Configured Never
+    Pose: [-1, -1] # Configured Never
+
+# ============================
+# Old stuff joe isn't touching
+# ============================
+
+neo:
+  kFreeSpeedRpm: 5676
+
+drive:
+  kMaxSpeedMetersPerSecond: 4.8
+
+  kDirectionSlewRate: 1.2
+  kMagnitudeSlewRate: 1.8
+  kRotationalSlewRate: 2.0
+
+  kTrackWidth: 0.6731
+  kWheelBase: 0.6731
+
+  # SPARK MAX CAN IDs
+  kFrontLeftDrivingCanId: 11
+  kRearLeftDrivingCanId: 13
+  kFrontRightDrivingCanId: 15
+  kRearRightDrivingCanId: 17
+
+  kFrontLeftTurningCanId: 10
+  kRearLeftTurningCanId: 12
+  kFrontRightTurningCanId: 14
+  kRearRightTurningCanId: 16
+
+  kGyroReversed: False
+
+module:
+  kDrivingMotorPinionTeeth: 14
+
+  kTurningEncoderInverted: True
+
+  kWheelDiameterMeters: 0.0762
+
+  kDrivingMotorCurrentLimit: 50
+  kTurningMotorCurrentLimit: 20