From 3cf3d6e161770b463def10694ad3ad3b4b92f2ff Mon Sep 17 00:00:00 2001 From: KenwoodFox Date: Tue, 16 Jan 2024 10:21:56 -0500 Subject: [PATCH] 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 | 29 +++++++--- .../{maxswervemodule.py => swerveModule.py} | 16 ++--- rio/constants/robotHardware.yaml | 58 +++++++++++++++++++ rio/constants/simple_hardware.yaml | 35 ----------- 4 files changed, 86 insertions(+), 52 deletions(-) rename rio/components/{maxswervemodule.py => swerveModule.py} (95%) create mode 100644 rio/constants/robotHardware.yaml delete mode 100644 rio/constants/simple_hardware.yaml diff --git a/rio/components/drivetrain.py b/rio/components/drivetrain.py index b37a29dc..73625eb8 100644 --- a/rio/components/drivetrain.py +++ b/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,7 +15,7 @@ from navx import AHRS from constants.complexConstants import DriveConstants import swerveutils -from .maxswervemodule import MAXSwerveModule +from .swerveModule import MikeSwerveModule from constants.getConstants import getConstants @@ -23,30 +23,41 @@ 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, diff --git a/rio/components/maxswervemodule.py b/rio/components/swerveModule.py similarity index 95% rename from rio/components/maxswervemodule.py rename to rio/components/swerveModule.py index 401da334..2b26b43c 100644 --- a/rio/components/maxswervemodule.py +++ b/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"] diff --git a/rio/constants/robotHardware.yaml b/rio/constants/robotHardware.yaml new file mode 100644 index 00000000..bf71d63f --- /dev/null +++ b/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 diff --git a/rio/constants/simple_hardware.yaml b/rio/constants/simple_hardware.yaml deleted file mode 100644 index 6ca7b436..00000000 --- a/rio/constants/simple_hardware.yaml +++ /dev/null @@ -1,35 +0,0 @@ -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 \ No newline at end of file