This fails tests althought i can't get it to fail sim

rio/components/drivetrain.py

@@ -13,37 +13,42 @@
     SwerveDrive4Odometry,
 )
 from navx import AHRS
-from constants.constants import DriveConstants
+from constants.complexConstants import DriveConstants
 import swerveutils
 from .maxswervemodule import MAXSwerveModule
 
+from constants.getConstants import getConstants
+
 
 class DriveSubsystem(Subsystem):
     def __init__(self) -> None:
         super().__init__()
+        # hardware constants
+        hardwareconstants = getConstants("simple_hardware")
+        self.driveConsts = hardwareconstants["drive"]
 
         # Create MAXSwerveModules
         self.frontLeft = MAXSwerveModule(
-            DriveConstants.kFrontLeftDrivingCanId,
-            DriveConstants.kFrontLeftTurningCanId,
+            self.driveConsts["kFrontLeftDrivingCanId"],
+            self.driveConsts["kFrontLeftTurningCanId"],
             DriveConstants.kFrontLeftChassisAngularOffset,
         )
 
         self.frontRight = MAXSwerveModule(
-            DriveConstants.kFrontRightDrivingCanId,
-            DriveConstants.kFrontRightTurningCanId,
+            self.driveConsts["kFrontRightDrivingCanId"],
+            self.driveConsts["kFrontRightTurningCanId"],
             DriveConstants.kFrontRightChassisAngularOffset,
         )
 
         self.rearLeft = MAXSwerveModule(
-            DriveConstants.kRearLeftDrivingCanId,
-            DriveConstants.kRearLeftTurningCanId,
+            self.driveConsts["kRearLeftDrivingCanId"],
+            self.driveConsts["kRearLeftTurningCanId"],
             DriveConstants.kBackLeftChassisAngularOffset,
         )
 
         self.rearRight = MAXSwerveModule(
-            DriveConstants.kRearRightDrivingCanId,
-            DriveConstants.kRearRightTurningCanId,
+            self.driveConsts["kRearRightDrivingCanId"],
+            self.driveConsts["kRearRightTurningCanId"],
             DriveConstants.kBackRightChassisAngularOffset,
         )
 
@@ -55,8 +60,8 @@ def __init__(self) -> None:
         self.currentTranslationDir = 0.0
         self.currentTranslationMag = 0.0
 
-        self.magLimiter = SlewRateLimiter(DriveConstants.kMagnitudeSlewRate)
-        self.rotLimiter = SlewRateLimiter(DriveConstants.kRotationalSlewRate)
+        self.magLimiter = SlewRateLimiter(self.driveConsts["kMagnitudeSlewRate"])
+        self.rotLimiter = SlewRateLimiter(self.driveConsts["kRotationalSlewRate"])
         self.prevTime = wpilib.Timer.getFPGATimestamp()
 
         # Odometry class for tracking robot pose
@@ -136,7 +141,7 @@ def drive(
             # Calculate the direction slew rate based on an estimate of the lateral acceleration
             if self.currentTranslationMag != 0.0:
                 directionSlewRate = abs(
-                    DriveConstants.kDirectionSlewRate / self.currentTranslationMag
+                    self.driveConsts["kDirectionSlewRate"] / self.currentTranslationMag
                 )
             else:
                 directionSlewRate = 500.0
@@ -192,8 +197,8 @@ def drive(
             self.currentRotation = rot
 
         # Convert the commanded speeds into the correct units for the drivetrain
-        xSpeedDelivered = xSpeedCommanded * DriveConstants.kMaxSpeedMetersPerSecond
-        ySpeedDelivered = ySpeedCommanded * DriveConstants.kMaxSpeedMetersPerSecond
+        xSpeedDelivered = xSpeedCommanded * self.driveConsts["kMaxSpeedMetersPerSecond"]
+        ySpeedDelivered = ySpeedCommanded * self.driveConsts["kMaxSpeedMetersPerSecond"]
         rotDelivered = self.currentRotation * DriveConstants.kMaxAngularSpeed
 
         swerveModuleStates = DriveConstants.kDriveKinematics.toSwerveModuleStates(
@@ -207,7 +212,7 @@ def drive(
             else ChassisSpeeds(xSpeedDelivered, ySpeedDelivered, rotDelivered)
         )
         fl, fr, rl, rr = SwerveDrive4Kinematics.desaturateWheelSpeeds(
-            swerveModuleStates, DriveConstants.kMaxSpeedMetersPerSecond
+            swerveModuleStates, self.driveConsts["kMaxSpeedMetersPerSecond"]
         )
         self.frontLeft.setDesiredState(fl)
         self.frontRight.setDesiredState(fr)
@@ -234,7 +239,7 @@ def setModuleStates(
         :param desiredStates: The desired SwerveModule states.
         """
         fl, fr, rl, rr = SwerveDrive4Kinematics.desaturateWheelSpeeds(
-            desiredStates, DriveConstants.kMaxSpeedMetersPerSecond
+            desiredStates, self.driveConsts["kMaxSpeedMetersPerSecond"]
         )
         self.frontLeft.setDesiredState(fl)
         self.frontRight.setDesiredState(fr)
@@ -264,4 +269,6 @@ def getTurnRate(self) -> float:
 
         :returns: The turn rate of the robot, in degrees per second
         """
-        return self.gyro.getRate() * (-1.0 if DriveConstants.kGyroReversed else 1.0)
+        return self.gyro.getRate() * (
+            -1.0 if self.driveConsts["kGyroReversed"] else 1.0
+        )

rio/components/maxswervemodule.py

@@ -2,7 +2,8 @@
 from wpimath.geometry import Rotation2d
 from wpimath.kinematics import SwerveModuleState, SwerveModulePosition
 
-from constants.constants import ModuleConstants
+from constants.complexConstants import ModuleConstants
+from constants.getConstants import getConstants
 
 
 class MAXSwerveModule:
@@ -14,6 +15,13 @@ def __init__(
         MAXSwerve Module built with NEOs, SPARKS MAX, and a Through Bore
         Encoder.
         """
+        # hardware constants
+        hardwareconstants = getConstants("simple_hardware")
+        self.moduleConsts = hardwareconstants["module"]
+
+        # pid constants
+        pidconstants = getConstants("simple_pid")
+        self.pidConsts = pidconstants["PID"]
 
         self.chassisAngularOffset = 0
         self.desiredState = SwerveModuleState(0.0, Rotation2d())
@@ -62,47 +70,47 @@ def __init__(
 
         # Invert the turning encoder, since the output shaft rotates in the opposite direction of
         # the steering motor in the MAXSwerve Module.
-        self.turningEncoder.setInverted(ModuleConstants.kTurningEncoderInverted)
+        self.turningEncoder.setInverted(self.moduleConsts["kTurningEncoderInverted"])
 
         # Enable PID wrap around for the turning motor. This will allow the PID
         # controller to go through 0 to get to the setpoint i.e. going from 350 degrees
         # to 10 degrees will go through 0 rather than the other direction which is a
         # longer route.
         self.turningPIDController.setPositionPIDWrappingEnabled(True)
         self.turningPIDController.setPositionPIDWrappingMinInput(
-            ModuleConstants.kTurningEncoderPositionPIDMinInput
+            self.pidConsts["kTurningEncoderPositionPIDMinInput"]
         )
         self.turningPIDController.setPositionPIDWrappingMaxInput(
             ModuleConstants.kTurningEncoderPositionPIDMaxInput
         )
 
         # Set the PID gains for the driving motor. Note these are example gains, and you
         # may need to tune them for your own robot!
-        self.drivingPIDController.setP(ModuleConstants.kDrivingP)
-        self.drivingPIDController.setI(ModuleConstants.kDrivingI)
-        self.drivingPIDController.setD(ModuleConstants.kDrivingD)
+        self.drivingPIDController.setP(self.pidConsts["kDrivingP"])
+        self.drivingPIDController.setI(self.pidConsts["kDrivingI"])
+        self.drivingPIDController.setD(self.pidConsts["kDrivingD"])
         self.drivingPIDController.setFF(ModuleConstants.kDrivingFF)
         self.drivingPIDController.setOutputRange(
-            ModuleConstants.kDrivingMinOutput, ModuleConstants.kDrivingMaxOutput
+            self.pidConsts["kDrivingMinOutput"], self.pidConsts["kDrivingMaxOutput"]
         )
 
         # Set the PID gains for the turning motor. Note these are example gains, and you
         # may need to tune them for your own robot!
-        self.turningPIDController.setP(ModuleConstants.kTurningP)
-        self.turningPIDController.setI(ModuleConstants.kTurningI)
-        self.turningPIDController.setD(ModuleConstants.kTurningD)
-        self.turningPIDController.setFF(ModuleConstants.kTurningFF)
+        self.turningPIDController.setP(self.pidConsts["kTurningP"])
+        self.turningPIDController.setI(self.pidConsts["kTurningI"])
+        self.turningPIDController.setD(self.pidConsts["kTurningD"])
+        self.turningPIDController.setFF(self.pidConsts["kTurningFF"])
         self.turningPIDController.setOutputRange(
-            ModuleConstants.kTurningMinOutput, ModuleConstants.kTurningMaxOutput
+            self.pidConsts["kTurningMinOutput"], self.pidConsts["kTurningMaxOutput"]
         )
 
         self.drivingSparkMax.setIdleMode(ModuleConstants.kDrivingMotorIdleMode)
         self.turningSparkMax.setIdleMode(ModuleConstants.kTurningMotorIdleMode)
         self.drivingSparkMax.setSmartCurrentLimit(
-            ModuleConstants.kDrivingMotorCurrentLimit
+            self.moduleConsts["kDrivingMotorCurrentLimit"]
         )
         self.turningSparkMax.setSmartCurrentLimit(
-            ModuleConstants.kTurningMotorCurrentLimit
+            self.moduleConsts["kTurningMotorCurrentLimit"]
         )
 
         # Save the SPARK MAX configurations. If a SPARK MAX browns out during

rio/constants/complexConstants.py

@@ -0,0 +1,85 @@
+# 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 constants module is a convenience place for teams to hold robot-wide
+numerical or boolean constants. Don't use this for any other purpose!
+"""
+
+
+import math
+
+from wpimath.geometry import Translation2d
+from wpimath.kinematics import SwerveDrive4Kinematics
+from wpimath.trajectory import TrapezoidProfileRadians
+
+from rev import CANSparkMax
+
+from constants.getConstants import getConstants
+
+constants = getConstants("simple_hardware")
+neoConsts = constants["neo"]
+driveConsts = constants["drive"]
+moduleConsts = constants["module"]
+
+
+class DriveConstants:
+    # Driving Parameters - Note that these are not the maximum capable speeds of
+    kMaxAngularSpeed = math.tau  # radians per second
+
+    # Distance between front and back wheels on robot
+    kModulePositions = [
+        Translation2d(driveConsts["kWheelBase"] / 2, driveConsts["kTrackWidth"] / 2),
+        Translation2d(driveConsts["kWheelBase"] / 2, -driveConsts["kTrackWidth"] / 2),
+        Translation2d(-driveConsts["kWheelBase"] / 2, driveConsts["kTrackWidth"] / 2),
+        Translation2d(-driveConsts["kWheelBase"] / 2, -driveConsts["kTrackWidth"] / 2),
+    ]
+    kDriveKinematics = SwerveDrive4Kinematics(*kModulePositions)
+
+    # Angular offsets of the modules relative to the chassis in radians
+    kFrontLeftChassisAngularOffset = -math.pi / 2
+    kFrontRightChassisAngularOffset = 0
+    kBackLeftChassisAngularOffset = math.pi
+    kBackRightChassisAngularOffset = math.pi / 2
+
+
+class ModuleConstants:
+    # Calculations required for driving motor conversion factors and feed forward
+    kDrivingMotorFreeSpeedRps = neoConsts["kFreeSpeedRpm"] / 60
+
+    kWheelCircumferenceMeters = moduleConsts["kWheelDiameterMeters"] * math.pi
+    # 45 teeth on the wheel's bevel gear, 22 teeth on the first-stage spur gear, 15 teeth on the bevel pinion
+    kDrivingMotorReduction = (45.0 * 22) / (
+        moduleConsts["kDrivingMotorPinionTeeth"] * 15
+    )
+    kDriveWheelFreeSpeedRps = (
+        kDrivingMotorFreeSpeedRps * kWheelCircumferenceMeters
+    ) / kDrivingMotorReduction
+
+    kDrivingEncoderPositionFactor = (
+        moduleConsts["kWheelDiameterMeters"] * math.pi
+    ) / kDrivingMotorReduction  # meters
+    kDrivingEncoderVelocityFactor = (
+        (moduleConsts["kWheelDiameterMeters"] * math.pi) / kDrivingMotorReduction
+    ) / 60.0  # meters per second
+
+    kTurningEncoderPositionFactor = math.tau  # radian
+    kTurningEncoderVelocityFactor = math.tau / 60.0  # radians per second
+
+    kTurningEncoderPositionPIDMaxInput = kTurningEncoderPositionFactor  # radian
+
+    kDrivingFF = 1 / kDriveWheelFreeSpeedRps
+
+    kDrivingMotorIdleMode = CANSparkMax.IdleMode.kBrake
+    kTurningMotorIdleMode = CANSparkMax.IdleMode.kBrake
+
+
+class AutoConstants:
+    kMaxAngularSpeedRadiansPerSecond = math.pi
+    kMaxAngularSpeedRadiansPerSecondSquared = math.pi
+
+    # Constraint for the motion profiled robot angle controller
+    kThetaControllerConstraints = TrapezoidProfileRadians.Constraints(
+        kMaxAngularSpeedRadiansPerSecond, kMaxAngularSpeedRadiansPerSecondSquared
+    )