From c561bb4b9e4050a29b79048e74e3176a32e3e6ca Mon Sep 17 00:00:00 2001 From: Jack Lingle Date: Wed, 21 Aug 2024 19:39:14 -0400 Subject: [PATCH 1/5] add PhoenixDriveConstants --- .vscode/settings.json | 3 ++- gradlew | 0 src/main/java/frc/robot/Constants/PhoenixDriveConstants | 6 ++++++ 3 files changed, 8 insertions(+), 1 deletion(-) mode change 100644 => 100755 gradlew create mode 100644 src/main/java/frc/robot/Constants/PhoenixDriveConstants diff --git a/.vscode/settings.json b/.vscode/settings.json index 4ed293b..2f1b9b3 100644 --- a/.vscode/settings.json +++ b/.vscode/settings.json @@ -25,5 +25,6 @@ } }, ], - "java.test.defaultConfig": "WPIlibUnitTests" + "java.test.defaultConfig": "WPIlibUnitTests", + "java.compile.nullAnalysis.mode": "automatic" } diff --git a/gradlew b/gradlew old mode 100644 new mode 100755 diff --git a/src/main/java/frc/robot/Constants/PhoenixDriveConstants b/src/main/java/frc/robot/Constants/PhoenixDriveConstants new file mode 100644 index 0000000..010b09f --- /dev/null +++ b/src/main/java/frc/robot/Constants/PhoenixDriveConstants @@ -0,0 +1,6 @@ +package frc.robot.constants; + + +public static final class PhoenixDriveConstants { + +} \ No newline at end of file From 86db87fe74699f023515ed649f83d53f329dcc3c Mon Sep 17 00:00:00 2001 From: Jack Lingle Date: Sat, 24 Aug 2024 10:02:41 -0400 Subject: [PATCH 2/5] add drive constants file and PhoenixDrive empty class --- src/main/java/frc/robot/BuildConstants.java | 12 +- .../frc/robot/Constants/PhoenixDriveConstants | 6 - .../constants/PhoenixDriveConstants.java | 179 ++++++++++++++++++ .../frc/robot/subsystems/PhoenixDrive.java | 7 + 4 files changed, 192 insertions(+), 12 deletions(-) delete mode 100644 src/main/java/frc/robot/Constants/PhoenixDriveConstants create mode 100644 src/main/java/frc/robot/constants/PhoenixDriveConstants.java create mode 100644 src/main/java/frc/robot/subsystems/PhoenixDrive.java diff --git a/src/main/java/frc/robot/BuildConstants.java b/src/main/java/frc/robot/BuildConstants.java index 16ef967..5c33558 100644 --- a/src/main/java/frc/robot/BuildConstants.java +++ b/src/main/java/frc/robot/BuildConstants.java @@ -5,12 +5,12 @@ public final class BuildConstants { public static final String MAVEN_GROUP = ""; public static final String MAVEN_NAME = "high-key-2024"; public static final String VERSION = "unspecified"; - public static final int GIT_REVISION = 2; - public static final String GIT_SHA = "d6f3c583e188320dfc6bb16c4deda3efe3eb86ce"; - public static final String GIT_DATE = "2024-08-21 18:58:11 EDT"; - public static final String GIT_BRANCH = "add-vendordeps-plugins-ci"; - public static final String BUILD_DATE = "2024-08-21 19:32:54 EDT"; - public static final long BUILD_UNIX_TIME = 1724283174206L; + public static final int GIT_REVISION = 4; + public static final String GIT_SHA = "c561bb4b9e4050a29b79048e74e3176a32e3e6ca"; + public static final String GIT_DATE = "2024-08-21 19:40:00 EDT"; + public static final String GIT_BRANCH = "basic-swerve"; + public static final String BUILD_DATE = "2024-08-24 09:47:06 EDT"; + public static final long BUILD_UNIX_TIME = 1724507226931L; public static final int DIRTY = 1; private BuildConstants() {} diff --git a/src/main/java/frc/robot/Constants/PhoenixDriveConstants b/src/main/java/frc/robot/Constants/PhoenixDriveConstants deleted file mode 100644 index 010b09f..0000000 --- a/src/main/java/frc/robot/Constants/PhoenixDriveConstants +++ /dev/null @@ -1,6 +0,0 @@ -package frc.robot.constants; - - -public static final class PhoenixDriveConstants { - -} \ No newline at end of file diff --git a/src/main/java/frc/robot/constants/PhoenixDriveConstants.java b/src/main/java/frc/robot/constants/PhoenixDriveConstants.java new file mode 100644 index 0000000..71b29f9 --- /dev/null +++ b/src/main/java/frc/robot/constants/PhoenixDriveConstants.java @@ -0,0 +1,179 @@ +package frc.robot.constants; + +import com.ctre.phoenix6.configs.Slot0Configs; +import com.ctre.phoenix6.mechanisms.swerve.SwerveDrivetrainConstants; +import com.ctre.phoenix6.mechanisms.swerve.SwerveModule.ClosedLoopOutputType; +import com.ctre.phoenix6.mechanisms.swerve.SwerveModuleConstants; +import com.ctre.phoenix6.mechanisms.swerve.SwerveModuleConstants.SteerFeedbackType; +import com.ctre.phoenix6.mechanisms.swerve.SwerveModuleConstantsFactory; + +import edu.wpi.first.math.geometry.Translation2d; +import edu.wpi.first.math.kinematics.SwerveDriveKinematics; +import edu.wpi.first.math.util.Units; + +public final class PhoenixDriveConstants { + // Both sets of gains need to be tuned to your individual robot. + + // The steer motor uses any SwerveModule.SteerRequestType control request with the + // output type specified by SwerveModuleConstants.SteerMotorClosedLoopOutput + public static final Slot0Configs steerGains = + new Slot0Configs() + .withKP(150).withKI(50).withKD(0.2) + .withKS(0.25).withKV(1.5).withKA(0); + // When using closed-loop control, the drive motor uses the control + // output type specified by SwerveModuleConstants.DriveMotorClosedLoopOutput + public static final Slot0Configs driveGains = + new Slot0Configs() + .withKP(0).withKI(0.02).withKD(0) + .withKS(0.26).withKV(0.12).withKA(0.01); + + // The closed-loop output type to use for the steer motors; + // This affects the PID/FF gains for the steer motors + private static final ClosedLoopOutputType steerClosedLoopOutput = + ClosedLoopOutputType.Voltage; + // The closed-loop output type to use for the drive motors; + // This affects the PID/FF gains for the drive motors + private static final ClosedLoopOutputType driveClosedLoopOutput = + ClosedLoopOutputType.Voltage; + + // The stator current at which the wheels start to slip; + // This needs to be tuned to your individual robot + private static final double kSlipCurrentA = 80; + + // Theoretical free speed (m/s) at 12v applied output; + // This needs to be tuned to your individual robot + public static final double kSpeedAt12VoltsMps = 5.02; // 5.21 OR 5.02 + + // Every 1 rotation of the azimuth results in kCoupleRatio drive motor turns; + // This may need to be tuned to your individual robot + private static final double kCoupleRatio = 3.5714285714285716; + + private static final double kDriveGearRatio = 6.122448979591837; + private static final double kSteerGearRatio = 21.428571428571427; + private static final double kWheelRadiusInches = 1.965; + + private static final boolean kSteerMotorReversed = true; + private static final boolean kInvertLeftSide = false; + private static final boolean kInvertRightSide = true; + + private static final String kCANbusName = "Canivore"; + private static final int kPigeonId = 1; + + // These are only used for simulation + private static final double kSteerInertia = 0.00001; + private static final double kDriveInertia = 0.001; + // Simulated voltage necessary to overcome friction + private static final double kSteerFrictionVoltage = 0.25; + private static final double kDriveFrictionVoltage = 0.25; + + private static final SwerveDrivetrainConstants DrivetrainConstants = + new SwerveDrivetrainConstants() + .withPigeon2Id(kPigeonId) + .withCANbusName(kCANbusName); + + private static final SwerveModuleConstantsFactory ConstantCreator = + new SwerveModuleConstantsFactory() + .withDriveMotorGearRatio(kDriveGearRatio) + .withSteerMotorGearRatio(kSteerGearRatio) + .withWheelRadius(kWheelRadiusInches) + .withSlipCurrent(kSlipCurrentA) + .withSteerMotorGains(steerGains) + .withDriveMotorGains(driveGains) + .withSteerMotorClosedLoopOutput(steerClosedLoopOutput) + .withDriveMotorClosedLoopOutput(driveClosedLoopOutput) + .withSpeedAt12VoltsMps(kSpeedAt12VoltsMps) + .withSteerInertia(kSteerInertia) + .withDriveInertia(kDriveInertia) + .withSteerFrictionVoltage(kSteerFrictionVoltage) + .withDriveFrictionVoltage(kDriveFrictionVoltage) + .withFeedbackSource(SteerFeedbackType.FusedCANcoder) + .withCouplingGearRatio(kCoupleRatio) + .withSteerMotorInverted(kSteerMotorReversed); + + // Front Left + private static final int kBackRightDriveMotorId = 2; + private static final int kBackRightSteerMotorId = 1; + private static final int kBackRightEncoderId = 1; + private static final double kBackRightEncoderOffset = 0.3486328125; + + private static final double kBackRightXPosInches = 10.375; + private static final double kBackRightYPosInches = 10.375; + + // Front Right + private static final int kBackLeftDriveMotorId = 4; + private static final int kBackLeftSteerMotorId = 3; + private static final int kBackLeftEncoderId = 2; + private static final double kBackLeftEncoderOffset = 0.096435546875; + + private static final double kBackLeftXPosInches = 10.375; + private static final double kBackLeftYPosInches = -10.375; + + // Back Left + private static final int kFrontRightDriveMotorId = 8; + private static final int kFrontRightSteerMotorId = 7; + private static final int kFrontRightEncoderId = 4; + private static final double kFrontRightEncoderOffset = 0.130859375; + + private static final double kFrontRightXPosInches = -10.375; + private static final double kFrontRightYPosInches = 10.375; + + // Back Right + private static final int kFrontLeftDriveMotorId = 6; + private static final int kFrontLeftSteerMotorId = 5; + private static final int kFrontLeftEncoderId = 3; + private static final double kFrontLeftEncoderOffset = -0.372802734375; + + private static final double kFrontLeftXPosInches = -10.375; + private static final double kFrontLeftYPosInches = -10.375; + + public static final double kModuleRadiusMeters = + Units.inchesToMeters(Math.hypot(kFrontLeftXPosInches, kFrontLeftYPosInches)); + + private static final SwerveModuleConstants FrontLeft = + ConstantCreator.createModuleConstants( + kFrontLeftSteerMotorId, + kFrontLeftDriveMotorId, + kFrontLeftEncoderId, + kFrontLeftEncoderOffset, + Units.inchesToMeters(kFrontLeftXPosInches), + Units.inchesToMeters(kFrontLeftYPosInches), + kInvertLeftSide); + private static final SwerveModuleConstants FrontRight = + ConstantCreator.createModuleConstants( + kFrontRightSteerMotorId, + kFrontRightDriveMotorId, + kFrontRightEncoderId, + kFrontRightEncoderOffset, + Units.inchesToMeters(kFrontRightXPosInches), + Units.inchesToMeters(kFrontRightYPosInches), + kInvertRightSide); + private static final SwerveModuleConstants BackLeft = + ConstantCreator.createModuleConstants( + kBackLeftSteerMotorId, + kBackLeftDriveMotorId, + kBackLeftEncoderId, + kBackLeftEncoderOffset, + Units.inchesToMeters(kBackLeftXPosInches), + Units.inchesToMeters(kBackLeftYPosInches), + kInvertLeftSide); + private static final SwerveModuleConstants BackRight = + ConstantCreator.createModuleConstants( + kBackRightSteerMotorId, + kBackRightDriveMotorId, + kBackRightEncoderId, + kBackRightEncoderOffset, + Units.inchesToMeters(kBackRightXPosInches), + Units.inchesToMeters(kBackRightYPosInches), + kInvertRightSide); + + public static final SwerveDriveKinematics kinematics = + new SwerveDriveKinematics( + new Translation2d(FrontLeft.LocationX, FrontLeft.LocationY), + new Translation2d(FrontLeft.LocationX, FrontRight.LocationY), + new Translation2d(BackLeft.LocationX, BackLeft.LocationY), + new Translation2d(BackRight.LocationX, BackRight.LocationY)); + + public static final PhoenixDrive DriveTrain = + new PhoenixDrive( + DrivetrainConstants, FrontLeft, FrontRight, BackLeft, BackRight); +} \ No newline at end of file diff --git a/src/main/java/frc/robot/subsystems/PhoenixDrive.java b/src/main/java/frc/robot/subsystems/PhoenixDrive.java new file mode 100644 index 0000000..e2374cc --- /dev/null +++ b/src/main/java/frc/robot/subsystems/PhoenixDrive.java @@ -0,0 +1,7 @@ +package frc.robot.subsystems; + +import com.ctre.phoenix6.mechanisms.swerve.SwerveDrivetrain; + +public class PhoenixDrive extends SwerveDrivetrain implements Subsystem { + +} From 659c29ccbb5c0478c38182ec7a1970c4204886a8 Mon Sep 17 00:00:00 2001 From: Jack Lingle Date: Mon, 26 Aug 2024 17:30:11 -0400 Subject: [PATCH 3/5] add very basic PhoenixDrive example; --- .vscode/settings.json | 5 +- src/main/java/frc/robot/BuildConstants.java | 10 +-- .../constants/PhoenixDriveConstants.java | 43 +++++------ .../frc/robot/subsystems/PhoenixDrive.java | 72 ++++++++++++++++++- 4 files changed, 103 insertions(+), 27 deletions(-) diff --git a/.vscode/settings.json b/.vscode/settings.json index 2f1b9b3..b331de9 100644 --- a/.vscode/settings.json +++ b/.vscode/settings.json @@ -26,5 +26,8 @@ }, ], "java.test.defaultConfig": "WPIlibUnitTests", - "java.compile.nullAnalysis.mode": "automatic" + "java.compile.nullAnalysis.mode": "automatic", + "cSpell.words": [ + "odometry" + ] } diff --git a/src/main/java/frc/robot/BuildConstants.java b/src/main/java/frc/robot/BuildConstants.java index 5c33558..4c06bf0 100644 --- a/src/main/java/frc/robot/BuildConstants.java +++ b/src/main/java/frc/robot/BuildConstants.java @@ -5,12 +5,12 @@ public final class BuildConstants { public static final String MAVEN_GROUP = ""; public static final String MAVEN_NAME = "high-key-2024"; public static final String VERSION = "unspecified"; - public static final int GIT_REVISION = 4; - public static final String GIT_SHA = "c561bb4b9e4050a29b79048e74e3176a32e3e6ca"; - public static final String GIT_DATE = "2024-08-21 19:40:00 EDT"; + public static final int GIT_REVISION = 5; + public static final String GIT_SHA = "86db87fe74699f023515ed649f83d53f329dcc3c"; + public static final String GIT_DATE = "2024-08-24 10:02:41 EDT"; public static final String GIT_BRANCH = "basic-swerve"; - public static final String BUILD_DATE = "2024-08-24 09:47:06 EDT"; - public static final long BUILD_UNIX_TIME = 1724507226931L; + public static final String BUILD_DATE = "2024-08-26 17:29:01 EDT"; + public static final long BUILD_UNIX_TIME = 1724707741400L; public static final int DIRTY = 1; private BuildConstants() {} diff --git a/src/main/java/frc/robot/constants/PhoenixDriveConstants.java b/src/main/java/frc/robot/constants/PhoenixDriveConstants.java index 71b29f9..20468ba 100644 --- a/src/main/java/frc/robot/constants/PhoenixDriveConstants.java +++ b/src/main/java/frc/robot/constants/PhoenixDriveConstants.java @@ -6,35 +6,41 @@ import com.ctre.phoenix6.mechanisms.swerve.SwerveModuleConstants; import com.ctre.phoenix6.mechanisms.swerve.SwerveModuleConstants.SteerFeedbackType; import com.ctre.phoenix6.mechanisms.swerve.SwerveModuleConstantsFactory; - import edu.wpi.first.math.geometry.Translation2d; import edu.wpi.first.math.kinematics.SwerveDriveKinematics; import edu.wpi.first.math.util.Units; +import frc.robot.subsystems.PhoenixDrive; public final class PhoenixDriveConstants { - // Both sets of gains need to be tuned to your individual robot. + // Both sets of gains need to be tuned to your individual robot. - // The steer motor uses any SwerveModule.SteerRequestType control request with the - // output type specified by SwerveModuleConstants.SteerMotorClosedLoopOutput + // The steer motor uses any SwerveModule.SteerRequestType control request with the + // output type specified by SwerveModuleConstants.SteerMotorClosedLoopOutput public static final Slot0Configs steerGains = new Slot0Configs() - .withKP(150).withKI(50).withKD(0.2) - .withKS(0.25).withKV(1.5).withKA(0); - // When using closed-loop control, the drive motor uses the control - // output type specified by SwerveModuleConstants.DriveMotorClosedLoopOutput + .withKP(150) + .withKI(50) + .withKD(0.2) + .withKS(0.25) + .withKV(1.5) + .withKA(0); + // When using closed-loop control, the drive motor uses the control + // output type specified by SwerveModuleConstants.DriveMotorClosedLoopOutput public static final Slot0Configs driveGains = new Slot0Configs() - .withKP(0).withKI(0.02).withKD(0) - .withKS(0.26).withKV(0.12).withKA(0.01); + .withKP(0) + .withKI(0.02) + .withKD(0) + .withKS(0.26) + .withKV(0.12) + .withKA(0.01); // The closed-loop output type to use for the steer motors; // This affects the PID/FF gains for the steer motors - private static final ClosedLoopOutputType steerClosedLoopOutput = - ClosedLoopOutputType.Voltage; + private static final ClosedLoopOutputType steerClosedLoopOutput = ClosedLoopOutputType.Voltage; // The closed-loop output type to use for the drive motors; // This affects the PID/FF gains for the drive motors - private static final ClosedLoopOutputType driveClosedLoopOutput = - ClosedLoopOutputType.Voltage; + private static final ClosedLoopOutputType driveClosedLoopOutput = ClosedLoopOutputType.Voltage; // The stator current at which the wheels start to slip; // This needs to be tuned to your individual robot @@ -67,9 +73,7 @@ public final class PhoenixDriveConstants { private static final double kDriveFrictionVoltage = 0.25; private static final SwerveDrivetrainConstants DrivetrainConstants = - new SwerveDrivetrainConstants() - .withPigeon2Id(kPigeonId) - .withCANbusName(kCANbusName); + new SwerveDrivetrainConstants().withPigeon2Id(kPigeonId).withCANbusName(kCANbusName); private static final SwerveModuleConstantsFactory ConstantCreator = new SwerveModuleConstantsFactory() @@ -174,6 +178,5 @@ public final class PhoenixDriveConstants { new Translation2d(BackRight.LocationX, BackRight.LocationY)); public static final PhoenixDrive DriveTrain = - new PhoenixDrive( - DrivetrainConstants, FrontLeft, FrontRight, BackLeft, BackRight); -} \ No newline at end of file + new PhoenixDrive(DrivetrainConstants, FrontLeft, FrontRight, BackLeft, BackRight); +} diff --git a/src/main/java/frc/robot/subsystems/PhoenixDrive.java b/src/main/java/frc/robot/subsystems/PhoenixDrive.java index e2374cc..97ea573 100644 --- a/src/main/java/frc/robot/subsystems/PhoenixDrive.java +++ b/src/main/java/frc/robot/subsystems/PhoenixDrive.java @@ -1,7 +1,77 @@ package frc.robot.subsystems; import com.ctre.phoenix6.mechanisms.swerve.SwerveDrivetrain; +import com.ctre.phoenix6.mechanisms.swerve.SwerveDrivetrainConstants; +import com.ctre.phoenix6.mechanisms.swerve.SwerveModuleConstants; +import com.ctre.phoenix6.mechanisms.swerve.SwerveRequest; +import edu.wpi.first.math.geometry.Rotation2d; +import edu.wpi.first.math.kinematics.ChassisSpeeds; +import edu.wpi.first.wpilibj.DriverStation; +import edu.wpi.first.wpilibj.DriverStation.Alliance; +import edu.wpi.first.wpilibj.Notifier; +import edu.wpi.first.wpilibj2.command.Command; +import edu.wpi.first.wpilibj2.command.CommandScheduler; +import edu.wpi.first.wpilibj2.command.Subsystem; +import java.util.function.Supplier; public class PhoenixDrive extends SwerveDrivetrain implements Subsystem { - + private static final double kSimLoopPeriod = 0.005; // 5 ms + private Notifier m_simNotifier = null; + private double m_lastSimTime; + + /* Blue alliance sees forward as 0 degrees (toward red alliance wall) */ + private final Rotation2d BlueAlliancePerspectiveRotation = Rotation2d.fromDegrees(0); + /* Red alliance sees forward as 180 degrees (toward blue alliance wall) */ + private final Rotation2d RedAlliancePerspectiveRotation = Rotation2d.fromDegrees(180); + /* Keep track if we've ever applied the operator perspective before or not */ + private boolean hasAppliedOperatorPerspective = false; + + private final SwerveRequest.ApplyChassisSpeeds AutoRequest = + new SwerveRequest.ApplyChassisSpeeds(); + + private final SwerveRequest.SysIdSwerveTranslation TranslationCharacterization = + new SwerveRequest.SysIdSwerveTranslation(); + private final SwerveRequest.SysIdSwerveRotation RotationCharacterization = + new SwerveRequest.SysIdSwerveRotation(); + private final SwerveRequest.SysIdSwerveSteerGains SteerCharacterization = + new SwerveRequest.SysIdSwerveSteerGains(); + + public PhoenixDrive( + SwerveDrivetrainConstants driveConstants, + double odometryUpdateFrequency, + SwerveModuleConstants... modules) { + super(driveConstants, odometryUpdateFrequency, modules); + + CommandScheduler.getInstance().registerSubsystem(this); + } + + public PhoenixDrive( + SwerveDrivetrainConstants driveConstants, SwerveModuleConstants... modules) { + super(driveConstants, modules); + + CommandScheduler.getInstance().registerSubsystem(this); + } + + public Command applyDriveRequest(Supplier requestSupplier) { + return run(() -> this.setControl(requestSupplier.get())); + } + + public ChassisSpeeds getCurrentSpeeds() { + return m_kinematics.toChassisSpeeds(getState().ModuleStates); + } + + @Override + public void periodic() { + if (!hasAppliedOperatorPerspective || DriverStation.isDisabled()) { + DriverStation.getAlliance() + .ifPresent( + (color) -> { + this.setOperatorPerspectiveForward( + color == Alliance.Red + ? RedAlliancePerspectiveRotation + : BlueAlliancePerspectiveRotation); + }); + hasAppliedOperatorPerspective = true; + } + } } From 061772401d43564c1cb7a725c67bc24c0642336f Mon Sep 17 00:00:00 2001 From: Jack Lingle Date: Mon, 26 Aug 2024 18:09:50 -0400 Subject: [PATCH 4/5] fix format --- src/main/java/frc/robot/BuildConstants.java | 12 +- .../constants/PhoenixDriveConstants.java | 322 +++++++++--------- 2 files changed, 167 insertions(+), 167 deletions(-) diff --git a/src/main/java/frc/robot/BuildConstants.java b/src/main/java/frc/robot/BuildConstants.java index 4c06bf0..66ef7b3 100644 --- a/src/main/java/frc/robot/BuildConstants.java +++ b/src/main/java/frc/robot/BuildConstants.java @@ -5,13 +5,13 @@ public final class BuildConstants { public static final String MAVEN_GROUP = ""; public static final String MAVEN_NAME = "high-key-2024"; public static final String VERSION = "unspecified"; - public static final int GIT_REVISION = 5; - public static final String GIT_SHA = "86db87fe74699f023515ed649f83d53f329dcc3c"; - public static final String GIT_DATE = "2024-08-24 10:02:41 EDT"; + public static final int GIT_REVISION = 6; + public static final String GIT_SHA = "659c29ccbb5c0478c38182ec7a1970c4204886a8"; + public static final String GIT_DATE = "2024-08-26 17:30:11 EDT"; public static final String GIT_BRANCH = "basic-swerve"; - public static final String BUILD_DATE = "2024-08-26 17:29:01 EDT"; - public static final long BUILD_UNIX_TIME = 1724707741400L; - public static final int DIRTY = 1; + public static final String BUILD_DATE = "2024-08-26 18:09:35 EDT"; + public static final long BUILD_UNIX_TIME = 1724710175643L; + public static final int DIRTY = 0; private BuildConstants() {} } diff --git a/src/main/java/frc/robot/constants/PhoenixDriveConstants.java b/src/main/java/frc/robot/constants/PhoenixDriveConstants.java index 20468ba..e58eff8 100644 --- a/src/main/java/frc/robot/constants/PhoenixDriveConstants.java +++ b/src/main/java/frc/robot/constants/PhoenixDriveConstants.java @@ -16,167 +16,167 @@ public final class PhoenixDriveConstants { // The steer motor uses any SwerveModule.SteerRequestType control request with the // output type specified by SwerveModuleConstants.SteerMotorClosedLoopOutput - public static final Slot0Configs steerGains = - new Slot0Configs() - .withKP(150) - .withKI(50) - .withKD(0.2) - .withKS(0.25) - .withKV(1.5) - .withKA(0); + public static final Slot0Configs steerGains = + new Slot0Configs() + .withKP(150) + .withKI(50) + .withKD(0.2) + .withKS(0.25) + .withKV(1.5) + .withKA(0); // When using closed-loop control, the drive motor uses the control // output type specified by SwerveModuleConstants.DriveMotorClosedLoopOutput - public static final Slot0Configs driveGains = - new Slot0Configs() - .withKP(0) - .withKI(0.02) - .withKD(0) - .withKS(0.26) - .withKV(0.12) - .withKA(0.01); - - // The closed-loop output type to use for the steer motors; - // This affects the PID/FF gains for the steer motors - private static final ClosedLoopOutputType steerClosedLoopOutput = ClosedLoopOutputType.Voltage; - // The closed-loop output type to use for the drive motors; - // This affects the PID/FF gains for the drive motors - private static final ClosedLoopOutputType driveClosedLoopOutput = ClosedLoopOutputType.Voltage; - - // The stator current at which the wheels start to slip; - // This needs to be tuned to your individual robot - private static final double kSlipCurrentA = 80; - - // Theoretical free speed (m/s) at 12v applied output; - // This needs to be tuned to your individual robot - public static final double kSpeedAt12VoltsMps = 5.02; // 5.21 OR 5.02 - - // Every 1 rotation of the azimuth results in kCoupleRatio drive motor turns; - // This may need to be tuned to your individual robot - private static final double kCoupleRatio = 3.5714285714285716; - - private static final double kDriveGearRatio = 6.122448979591837; - private static final double kSteerGearRatio = 21.428571428571427; - private static final double kWheelRadiusInches = 1.965; - - private static final boolean kSteerMotorReversed = true; - private static final boolean kInvertLeftSide = false; - private static final boolean kInvertRightSide = true; - - private static final String kCANbusName = "Canivore"; - private static final int kPigeonId = 1; - - // These are only used for simulation - private static final double kSteerInertia = 0.00001; - private static final double kDriveInertia = 0.001; - // Simulated voltage necessary to overcome friction - private static final double kSteerFrictionVoltage = 0.25; - private static final double kDriveFrictionVoltage = 0.25; - - private static final SwerveDrivetrainConstants DrivetrainConstants = - new SwerveDrivetrainConstants().withPigeon2Id(kPigeonId).withCANbusName(kCANbusName); - - private static final SwerveModuleConstantsFactory ConstantCreator = - new SwerveModuleConstantsFactory() - .withDriveMotorGearRatio(kDriveGearRatio) - .withSteerMotorGearRatio(kSteerGearRatio) - .withWheelRadius(kWheelRadiusInches) - .withSlipCurrent(kSlipCurrentA) - .withSteerMotorGains(steerGains) - .withDriveMotorGains(driveGains) - .withSteerMotorClosedLoopOutput(steerClosedLoopOutput) - .withDriveMotorClosedLoopOutput(driveClosedLoopOutput) - .withSpeedAt12VoltsMps(kSpeedAt12VoltsMps) - .withSteerInertia(kSteerInertia) - .withDriveInertia(kDriveInertia) - .withSteerFrictionVoltage(kSteerFrictionVoltage) - .withDriveFrictionVoltage(kDriveFrictionVoltage) - .withFeedbackSource(SteerFeedbackType.FusedCANcoder) - .withCouplingGearRatio(kCoupleRatio) - .withSteerMotorInverted(kSteerMotorReversed); - - // Front Left - private static final int kBackRightDriveMotorId = 2; - private static final int kBackRightSteerMotorId = 1; - private static final int kBackRightEncoderId = 1; - private static final double kBackRightEncoderOffset = 0.3486328125; - - private static final double kBackRightXPosInches = 10.375; - private static final double kBackRightYPosInches = 10.375; - - // Front Right - private static final int kBackLeftDriveMotorId = 4; - private static final int kBackLeftSteerMotorId = 3; - private static final int kBackLeftEncoderId = 2; - private static final double kBackLeftEncoderOffset = 0.096435546875; - - private static final double kBackLeftXPosInches = 10.375; - private static final double kBackLeftYPosInches = -10.375; - - // Back Left - private static final int kFrontRightDriveMotorId = 8; - private static final int kFrontRightSteerMotorId = 7; - private static final int kFrontRightEncoderId = 4; - private static final double kFrontRightEncoderOffset = 0.130859375; - - private static final double kFrontRightXPosInches = -10.375; - private static final double kFrontRightYPosInches = 10.375; - - // Back Right - private static final int kFrontLeftDriveMotorId = 6; - private static final int kFrontLeftSteerMotorId = 5; - private static final int kFrontLeftEncoderId = 3; - private static final double kFrontLeftEncoderOffset = -0.372802734375; - - private static final double kFrontLeftXPosInches = -10.375; - private static final double kFrontLeftYPosInches = -10.375; - - public static final double kModuleRadiusMeters = - Units.inchesToMeters(Math.hypot(kFrontLeftXPosInches, kFrontLeftYPosInches)); - - private static final SwerveModuleConstants FrontLeft = - ConstantCreator.createModuleConstants( - kFrontLeftSteerMotorId, - kFrontLeftDriveMotorId, - kFrontLeftEncoderId, - kFrontLeftEncoderOffset, - Units.inchesToMeters(kFrontLeftXPosInches), - Units.inchesToMeters(kFrontLeftYPosInches), - kInvertLeftSide); - private static final SwerveModuleConstants FrontRight = - ConstantCreator.createModuleConstants( - kFrontRightSteerMotorId, - kFrontRightDriveMotorId, - kFrontRightEncoderId, - kFrontRightEncoderOffset, - Units.inchesToMeters(kFrontRightXPosInches), - Units.inchesToMeters(kFrontRightYPosInches), - kInvertRightSide); - private static final SwerveModuleConstants BackLeft = - ConstantCreator.createModuleConstants( - kBackLeftSteerMotorId, - kBackLeftDriveMotorId, - kBackLeftEncoderId, - kBackLeftEncoderOffset, - Units.inchesToMeters(kBackLeftXPosInches), - Units.inchesToMeters(kBackLeftYPosInches), - kInvertLeftSide); - private static final SwerveModuleConstants BackRight = - ConstantCreator.createModuleConstants( - kBackRightSteerMotorId, - kBackRightDriveMotorId, - kBackRightEncoderId, - kBackRightEncoderOffset, - Units.inchesToMeters(kBackRightXPosInches), - Units.inchesToMeters(kBackRightYPosInches), - kInvertRightSide); - - public static final SwerveDriveKinematics kinematics = - new SwerveDriveKinematics( - new Translation2d(FrontLeft.LocationX, FrontLeft.LocationY), - new Translation2d(FrontLeft.LocationX, FrontRight.LocationY), - new Translation2d(BackLeft.LocationX, BackLeft.LocationY), - new Translation2d(BackRight.LocationX, BackRight.LocationY)); - - public static final PhoenixDrive DriveTrain = - new PhoenixDrive(DrivetrainConstants, FrontLeft, FrontRight, BackLeft, BackRight); + public static final Slot0Configs driveGains = + new Slot0Configs() + .withKP(0) + .withKI(0.02) + .withKD(0) + .withKS(0.26) + .withKV(0.12) + .withKA(0.01); + + // The closed-loop output type to use for the steer motors; + // This affects the PID/FF gains for the steer motors + private static final ClosedLoopOutputType steerClosedLoopOutput = ClosedLoopOutputType.Voltage; + // The closed-loop output type to use for the drive motors; + // This affects the PID/FF gains for the drive motors + private static final ClosedLoopOutputType driveClosedLoopOutput = ClosedLoopOutputType.Voltage; + + // The stator current at which the wheels start to slip; + // This needs to be tuned to your individual robot + private static final double kSlipCurrentA = 80; + + // Theoretical free speed (m/s) at 12v applied output; + // This needs to be tuned to your individual robot + public static final double kSpeedAt12VoltsMps = 5.02; // 5.21 OR 5.02 + + // Every 1 rotation of the azimuth results in kCoupleRatio drive motor turns; + // This may need to be tuned to your individual robot + private static final double kCoupleRatio = 3.5714285714285716; + + private static final double kDriveGearRatio = 6.122448979591837; + private static final double kSteerGearRatio = 21.428571428571427; + private static final double kWheelRadiusInches = 1.965; + + private static final boolean kSteerMotorReversed = true; + private static final boolean kInvertLeftSide = false; + private static final boolean kInvertRightSide = true; + + private static final String kCANbusName = "Canivore"; + private static final int kPigeonId = 1; + + // These are only used for simulation + private static final double kSteerInertia = 0.00001; + private static final double kDriveInertia = 0.001; + // Simulated voltage necessary to overcome friction + private static final double kSteerFrictionVoltage = 0.25; + private static final double kDriveFrictionVoltage = 0.25; + + private static final SwerveDrivetrainConstants DrivetrainConstants = + new SwerveDrivetrainConstants().withPigeon2Id(kPigeonId).withCANbusName(kCANbusName); + + private static final SwerveModuleConstantsFactory ConstantCreator = + new SwerveModuleConstantsFactory() + .withDriveMotorGearRatio(kDriveGearRatio) + .withSteerMotorGearRatio(kSteerGearRatio) + .withWheelRadius(kWheelRadiusInches) + .withSlipCurrent(kSlipCurrentA) + .withSteerMotorGains(steerGains) + .withDriveMotorGains(driveGains) + .withSteerMotorClosedLoopOutput(steerClosedLoopOutput) + .withDriveMotorClosedLoopOutput(driveClosedLoopOutput) + .withSpeedAt12VoltsMps(kSpeedAt12VoltsMps) + .withSteerInertia(kSteerInertia) + .withDriveInertia(kDriveInertia) + .withSteerFrictionVoltage(kSteerFrictionVoltage) + .withDriveFrictionVoltage(kDriveFrictionVoltage) + .withFeedbackSource(SteerFeedbackType.FusedCANcoder) + .withCouplingGearRatio(kCoupleRatio) + .withSteerMotorInverted(kSteerMotorReversed); + + // Front Left + private static final int kBackRightDriveMotorId = 2; + private static final int kBackRightSteerMotorId = 1; + private static final int kBackRightEncoderId = 1; + private static final double kBackRightEncoderOffset = 0.3486328125; + + private static final double kBackRightXPosInches = 10.375; + private static final double kBackRightYPosInches = 10.375; + + // Front Right + private static final int kBackLeftDriveMotorId = 4; + private static final int kBackLeftSteerMotorId = 3; + private static final int kBackLeftEncoderId = 2; + private static final double kBackLeftEncoderOffset = 0.096435546875; + + private static final double kBackLeftXPosInches = 10.375; + private static final double kBackLeftYPosInches = -10.375; + + // Back Left + private static final int kFrontRightDriveMotorId = 8; + private static final int kFrontRightSteerMotorId = 7; + private static final int kFrontRightEncoderId = 4; + private static final double kFrontRightEncoderOffset = 0.130859375; + + private static final double kFrontRightXPosInches = -10.375; + private static final double kFrontRightYPosInches = 10.375; + + // Back Right + private static final int kFrontLeftDriveMotorId = 6; + private static final int kFrontLeftSteerMotorId = 5; + private static final int kFrontLeftEncoderId = 3; + private static final double kFrontLeftEncoderOffset = -0.372802734375; + + private static final double kFrontLeftXPosInches = -10.375; + private static final double kFrontLeftYPosInches = -10.375; + + public static final double kModuleRadiusMeters = + Units.inchesToMeters(Math.hypot(kFrontLeftXPosInches, kFrontLeftYPosInches)); + + private static final SwerveModuleConstants FrontLeft = + ConstantCreator.createModuleConstants( + kFrontLeftSteerMotorId, + kFrontLeftDriveMotorId, + kFrontLeftEncoderId, + kFrontLeftEncoderOffset, + Units.inchesToMeters(kFrontLeftXPosInches), + Units.inchesToMeters(kFrontLeftYPosInches), + kInvertLeftSide); + private static final SwerveModuleConstants FrontRight = + ConstantCreator.createModuleConstants( + kFrontRightSteerMotorId, + kFrontRightDriveMotorId, + kFrontRightEncoderId, + kFrontRightEncoderOffset, + Units.inchesToMeters(kFrontRightXPosInches), + Units.inchesToMeters(kFrontRightYPosInches), + kInvertRightSide); + private static final SwerveModuleConstants BackLeft = + ConstantCreator.createModuleConstants( + kBackLeftSteerMotorId, + kBackLeftDriveMotorId, + kBackLeftEncoderId, + kBackLeftEncoderOffset, + Units.inchesToMeters(kBackLeftXPosInches), + Units.inchesToMeters(kBackLeftYPosInches), + kInvertLeftSide); + private static final SwerveModuleConstants BackRight = + ConstantCreator.createModuleConstants( + kBackRightSteerMotorId, + kBackRightDriveMotorId, + kBackRightEncoderId, + kBackRightEncoderOffset, + Units.inchesToMeters(kBackRightXPosInches), + Units.inchesToMeters(kBackRightYPosInches), + kInvertRightSide); + + public static final SwerveDriveKinematics kinematics = + new SwerveDriveKinematics( + new Translation2d(FrontLeft.LocationX, FrontLeft.LocationY), + new Translation2d(FrontLeft.LocationX, FrontRight.LocationY), + new Translation2d(BackLeft.LocationX, BackLeft.LocationY), + new Translation2d(BackRight.LocationX, BackRight.LocationY)); + + public static final PhoenixDrive DriveTrain = + new PhoenixDrive(DrivetrainConstants, FrontLeft, FrontRight, BackLeft, BackRight); } From 7ba14701fc4522162b88d931bc78e45d6611029f Mon Sep 17 00:00:00 2001 From: Jack Lingle Date: Tue, 27 Aug 2024 15:52:01 -0400 Subject: [PATCH 5/5] git ignore build constants --- .gitignore | 3 +++ src/main/java/frc/robot/BuildConstants.java | 17 ----------------- 2 files changed, 3 insertions(+), 17 deletions(-) delete mode 100644 src/main/java/frc/robot/BuildConstants.java diff --git a/.gitignore b/.gitignore index 5528d4f..a5fef6d 100644 --- a/.gitignore +++ b/.gitignore @@ -176,3 +176,6 @@ logs/ # Folder that has CTRE Phoenix Sim device config storage ctre_sim/ + + +src/main/java/frc/robot/BuildConstants.java \ No newline at end of file diff --git a/src/main/java/frc/robot/BuildConstants.java b/src/main/java/frc/robot/BuildConstants.java deleted file mode 100644 index 66ef7b3..0000000 --- a/src/main/java/frc/robot/BuildConstants.java +++ /dev/null @@ -1,17 +0,0 @@ -package frc.robot; - -/** Automatically generated file containing build version information. */ -public final class BuildConstants { - public static final String MAVEN_GROUP = ""; - public static final String MAVEN_NAME = "high-key-2024"; - public static final String VERSION = "unspecified"; - public static final int GIT_REVISION = 6; - public static final String GIT_SHA = "659c29ccbb5c0478c38182ec7a1970c4204886a8"; - public static final String GIT_DATE = "2024-08-26 17:30:11 EDT"; - public static final String GIT_BRANCH = "basic-swerve"; - public static final String BUILD_DATE = "2024-08-26 18:09:35 EDT"; - public static final long BUILD_UNIX_TIME = 1724710175643L; - public static final int DIRTY = 0; - - private BuildConstants() {} -}