From f14be0fad764643f33554cc1cd3858ad639013e3 Mon Sep 17 00:00:00 2001 From: Jack Lingle Date: Tue, 27 Aug 2024 16:35:00 -0400 Subject: [PATCH] Basic swerve (#8) * add PhoenixDriveConstants * add drive constants file and PhoenixDrive empty class * add very basic PhoenixDrive example; * fix format * git ignore build constants --- .gitignore | 3 + .vscode/settings.json | 6 +- gradlew | 0 src/main/java/frc/robot/BuildConstants.java | 17 -- .../constants/PhoenixDriveConstants.java | 182 ++++++++++++++++++ .../frc/robot/subsystems/PhoenixDrive.java | 77 ++++++++ 6 files changed, 267 insertions(+), 18 deletions(-) mode change 100644 => 100755 gradlew delete mode 100644 src/main/java/frc/robot/BuildConstants.java 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/.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/.vscode/settings.json b/.vscode/settings.json index 4ed293b..b331de9 100644 --- a/.vscode/settings.json +++ b/.vscode/settings.json @@ -25,5 +25,9 @@ } }, ], - "java.test.defaultConfig": "WPIlibUnitTests" + "java.test.defaultConfig": "WPIlibUnitTests", + "java.compile.nullAnalysis.mode": "automatic", + "cSpell.words": [ + "odometry" + ] } diff --git a/gradlew b/gradlew old mode 100644 new mode 100755 diff --git a/src/main/java/frc/robot/BuildConstants.java b/src/main/java/frc/robot/BuildConstants.java deleted file mode 100644 index 16ef967..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 = 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 DIRTY = 1; - - private BuildConstants() {} -} 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..e58eff8 --- /dev/null +++ b/src/main/java/frc/robot/constants/PhoenixDriveConstants.java @@ -0,0 +1,182 @@ +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; +import frc.robot.subsystems.PhoenixDrive; + +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); +} 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..97ea573 --- /dev/null +++ b/src/main/java/frc/robot/subsystems/PhoenixDrive.java @@ -0,0 +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; + } + } +}