From e07bb2fa7b33a13828af06f678bb7bb26a25a31f Mon Sep 17 00:00:00 2001 From: C4llSiqn <83995467+C4llSqin@users.noreply.github.com> Date: Mon, 6 Jan 2025 18:18:43 -0700 Subject: [PATCH] A lot of constants changes and copy paste example from the docs --- src/main/java/frc4388/robot/Constants.java | 230 ++++++++++++++++++--- 1 file changed, 205 insertions(+), 25 deletions(-) diff --git a/src/main/java/frc4388/robot/Constants.java b/src/main/java/frc4388/robot/Constants.java index 3c6f252..35dc115 100644 --- a/src/main/java/frc4388/robot/Constants.java +++ b/src/main/java/frc4388/robot/Constants.java @@ -7,15 +7,25 @@ package frc4388.robot; -import java.util.ArrayList; -import java.util.List; +import static edu.wpi.first.units.Units.Inches; +import static edu.wpi.first.units.Units.Rotations; import com.ctre.phoenix6.configs.CANcoderConfiguration; -import com.ctre.phoenix6.configs.ParentConfiguration; +import com.ctre.phoenix6.configs.CurrentLimitsConfigs; +import com.ctre.phoenix6.configs.Slot0Configs; import com.ctre.phoenix6.configs.TalonFXConfiguration; +import com.ctre.phoenix6.signals.StaticFeedforwardSignValue; +import com.ctre.phoenix6.swerve.SwerveDrivetrainConstants; import com.ctre.phoenix6.swerve.SwerveModuleConstants; +import com.ctre.phoenix6.swerve.SwerveModuleConstants.ClosedLoopOutputType; +import com.ctre.phoenix6.swerve.SwerveModuleConstants.DriveMotorArrangement; +import com.ctre.phoenix6.swerve.SwerveModuleConstants.SteerFeedbackType; +import com.ctre.phoenix6.swerve.SwerveModuleConstants.SteerMotorArrangement; +import com.ctre.phoenix6.swerve.SwerveModuleConstantsFactory; import edu.wpi.first.math.trajectory.TrapezoidProfile; +import edu.wpi.first.units.measure.Angle; +import edu.wpi.first.units.measure.Distance; import frc4388.utility.CanDevice; import frc4388.utility.Gains; import frc4388.utility.LEDPatterns; @@ -49,13 +59,48 @@ public final class Constants { public static final double FAST_SPEED = 0.5; public static final double TURBO_SPEED = 1.0; - // public static List CAN_DEVICES = new ArrayList<>(); + // dimensions + public static final double WIDTH = 18.5; + public static final double HEIGHT = 18.5; + public static final double HALF_WIDTH = WIDTH / 2.d; + public static final double HALF_HEIGHT = HEIGHT / 2.d; - public static final class DefaultSwerveRotOffsets { - public static final double FRONT_LEFT_ROT_OFFSET = 0.0; //TODO: per robot swerve module offsets. - public static final double FRONT_RIGHT_ROT_OFFSET = 0.0; //TODO: per robot swerve module offsets. - public static final double BACK_LEFT_ROT_OFFSET = 0.0; //TODO: per robot swerve module offsets. - public static final double BACK_RIGHT_ROT_OFFSET = 0.0; //TODO: per robot swerve module offsets. + private static final class ModuleSpecificConstants { + //Front Left + private static final Angle FRONT_LEFT_ENCODER_OFFSET = Rotations.of(0.0); + private static final boolean FRONT_LEFT_DRIVE_MOTOR_INVERTED = false; + private static final boolean FRONT_LEFT_STEER_MOTOR_INVERTED = false; + private static final boolean FRONT_LEFT_ENCODER_INVERTED = false; + private static final Distance FRONT_LEFT_XPOS = Inches.of(HALF_WIDTH); + private static final Distance FRONT_LEFT_YPOS = Inches.of(HALF_HEIGHT); + + //Front Right + private static final Angle FRONT_RIGHT_ENCODER_OFFSET = Rotations.of(0.0); + private static final boolean FRONT_RIGHT_DRIVE_MOTOR_INVERTED = false; + private static final boolean FRONT_RIGHT_STEER_MOTOR_INVERTED = false; + private static final boolean FRONT_RIGHT_ENCODER_INVERTED = false; + private static final Distance FRONT_RIGHT_XPOS = Inches.of(HALF_WIDTH); + private static final Distance FRONT_RIGHT_YPOS = Inches.of(HALF_HEIGHT); + + //Back Left + private static final Angle BACK_LEFT_ENCODER_OFFSET = Rotations.of(0.0); + private static final boolean BACK_LEFT_DRIVE_MOTOR_INVERTED = false; + private static final boolean BACK_LEFT_STEER_MOTOR_INVERTED = false; + private static final boolean BACK_LEFT_ENCODER_INVERTED = false; + private static final Distance BACK_LEFT_XPOS = Inches.of(HALF_WIDTH); + private static final Distance BACK_LEFT_YPOS = Inches.of(HALF_HEIGHT); + + //Back Right + private static final Angle BACK_RIGHT_ENCODER_OFFSET = Rotations.of(0.0); + private static final boolean BACK_RIGHT_DRIVE_MOTOR_INVERTED = false; + private static final boolean BACK_RIGHT_STEER_MOTOR_INVERTED = false; + private static final boolean BACK_RIGHT_ENCODER_INVERTED = false; + private static final Distance BACK_RIGHT_XPOS = Inches.of(HALF_WIDTH); + private static final Distance BACK_RIGHT_YPOS = Inches.of(HALF_HEIGHT); + } + + private static final class DrivetrainSpecificConstants { + } public static final class IDs { @@ -75,7 +120,7 @@ public final class Constants { public static final CanDevice RIGHT_BACK_STEER = new CanDevice("RIGHT_BACK_STEER", 9); public static final CanDevice RIGHT_BACK_ENCODER = new CanDevice("RIGHT_BACK_ENCODER", 13); - public static final CanDevice DRIVE_PIGEON = new CanDevice("DRIVE_PIGEON", 4); + public static final CanDevice DRIVE_PIGEON = new CanDevice("DRIVE_PIGEON", 14); public static final CanDevice e = new CanDevice("NONEXISTANT_CAN", 50); } @@ -127,23 +172,158 @@ public final class Constants { public static final double MAX_SPEED_FEET_PER_SECOND = 20.4; public static final double MAX_ANGULAR_SPEED_FEET_PER_SECOND = 2 * 2 * Math.PI; - // dimensions - public static final double WIDTH = 18.5; - public static final double HEIGHT = 18.5; - public static final double HALF_WIDTH = WIDTH / 2.d; - public static final double HALF_HEIGHT = HEIGHT / 2.d; + - public static final SwerveModuleConstants FRONT_LEFT = new SwerveModuleConstants<>() - .withLocationX(HALF_WIDTH) - .withLocationX(HALF_HEIGHT) - .withDriveMotorId(2) - .withDriveMotorInverted(false) - .withSteerMotorId(3) - .withSteerMotorInverted(false) - .withEncoderId(10) - .withEncoderInverted(false) - .withEncoderOffset(0.0); + // No mans land + // Beware, there be dragons. + // The steer motor uses any SwerveModule.SteerRequestType control request with the + // output type specified by SwerveModuleConstants.SteerMotorClosedLoopOutput + private static final Slot0Configs steerGains = new Slot0Configs() + .withKP(100).withKI(0).withKD(0.5) + .withKS(0.1).withKV(1.91).withKA(0) + .withStaticFeedforwardSign(StaticFeedforwardSignValue.UseClosedLoopSign); + // When using closed-loop control, the drive motor uses the control + // output type specified by SwerveModuleConstants.DriveMotorClosedLoopOutput + private static final Slot0Configs driveGains = new Slot0Configs() + .withKP(0.1).withKI(0).withKD(0) + .withKS(0).withKV(0.124); + + // 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 kSteerClosedLoopOutput = 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 kDriveClosedLoopOutput = ClosedLoopOutputType.Voltage; + + // The type of motor used for the drive motor + private static final DriveMotorArrangement kDriveMotorType = DriveMotorArrangement.TalonFX_Integrated; + // The type of motor used for the drive motor + private static final SteerMotorArrangement kSteerMotorType = SteerMotorArrangement.TalonFX_Integrated; + + // The remote sensor feedback type to use for the steer motors; + // When not Pro-licensed, FusedCANcoder/SyncCANcoder automatically fall back to RemoteCANcoder + private static final SteerFeedbackType kSteerFeedbackType = SteerFeedbackType.RemoteCANcoder; + + // The stator current at which the wheels start to slip; + // This needs to be tuned to your individual robot + private static final double kSlipCurrent = 120; // TODO: tune??? + + // Initial configs for the drive and steer motors and the azimuth encoder; these cannot be null. + // Some configs will be overwritten; check the `with*InitialConfigs()` API documentation. + private static final TalonFXConfiguration driveInitialConfigs = new TalonFXConfiguration() + .withCurrentLimits( + new CurrentLimitsConfigs() + // Swerve azimuth does not require much torque output, so we can set a relatively low + // stator current limit to help avoid brownouts without impacting performance. + .withStatorCurrentLimit(100) // TODO: tune??? + .withStatorCurrentLimitEnable(true) + ); + private static final TalonFXConfiguration steerInitialConfigs = new TalonFXConfiguration() + .withCurrentLimits( + new CurrentLimitsConfigs() + // Swerve azimuth does not require much torque output, so we can set a relatively low + // stator current limit to help avoid brownouts without impacting performance. + .withStatorCurrentLimit(40) // TODO: tune??? + .withStatorCurrentLimitEnable(true) + ); + private static final CANcoderConfiguration encoderInitialConfigs = new CANcoderConfiguration(); + + // Theoretical free speed (m/s) at 12 V applied output; + // This needs to be tuned to your individual robot + public static final double kSpeedAt12Volts = 4.69;// todo: tune??? + + // 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.8181818181818183; //todo: find + + private static final double kDriveGearRatio = 6.12; + private static final double kSteerGearRatio = (150/7); + private static final Distance kWheelRadius = Inches.of(2); // 2 inches in meters + + public static final SwerveDrivetrainConstants DrivetrainConstants = new SwerveDrivetrainConstants() + .withPigeon2Id(IDs.DRIVE_PIGEON.id); + + private static final SwerveModuleConstantsFactory ConstantCreator = + new SwerveModuleConstantsFactory() // holy verbosity batman. + .withDriveMotorGearRatio(kDriveGearRatio) + .withSteerMotorGearRatio(kSteerGearRatio) + .withCouplingGearRatio(kCoupleRatio) + .withWheelRadius(kWheelRadius) + .withSteerMotorGains(steerGains) + .withDriveMotorGains(driveGains) + .withSteerMotorClosedLoopOutput(kSteerClosedLoopOutput) + .withDriveMotorClosedLoopOutput(kDriveClosedLoopOutput) + .withSlipCurrent(kSlipCurrent) + .withSpeedAt12Volts(kSpeedAt12Volts) + .withDriveMotorType(kDriveMotorType) + .withSteerMotorType(kSteerMotorType) + .withFeedbackSource(kSteerFeedbackType) + .withDriveMotorInitialConfigs(driveInitialConfigs) + .withSteerMotorInitialConfigs(steerInitialConfigs) + .withEncoderInitialConfigs(encoderInitialConfigs); + + // Front Left + private static final int kFrontLeftDriveMotorId = 3; + private static final int kFrontLeftSteerMotorId = 2; + private static final int kFrontLeftEncoderId = 10; + private static final Angle kFrontLeftEncoderOffset = Rotations.of(0.15234375); + private static final boolean kFrontLeftSteerMotorInverted = true; + private static final boolean kFrontLeftEncoderInverted = false; + + private static final Distance kFrontLeftXPos = Inches.of(10); + private static final Distance kFrontLeftYPos = Inches.of(10); + + // Front Right + private static final int kFrontRightDriveMotorId = 1; + private static final int kFrontRightSteerMotorId = 0; + private static final int kFrontRightEncoderId = 0; + private static final Angle kFrontRightEncoderOffset = Rotations.of(-0.4873046875); + private static final boolean kFrontRightSteerMotorInverted = true; + private static final boolean kFrontRightEncoderInverted = false; + + private static final Distance kFrontRightXPos = Inches.of(10); + private static final Distance kFrontRightYPos = Inches.of(-10); + + // Back Left + private static final int kBackLeftDriveMotorId = 7; + private static final int kBackLeftSteerMotorId = 6; + private static final int kBackLeftEncoderId = 3; + private static final Angle kBackLeftEncoderOffset = Rotations.of(-0.219482421875); + private static final boolean kBackLeftSteerMotorInverted = true; + private static final boolean kBackLeftEncoderInverted = false; + + private static final Distance kBackLeftXPos = Inches.of(-10); + private static final Distance kBackLeftYPos = Inches.of(10); + + // Back Right + private static final int kBackRightDriveMotorId = 5; + private static final int kBackRightSteerMotorId = 4; + private static final int kBackRightEncoderId = 2; + private static final Angle kBackRightEncoderOffset = Rotations.of(0.17236328125); + private static final boolean kBackRightSteerMotorInverted = true; + private static final boolean kBackRightEncoderInverted = false; + + private static final Distance kBackRightXPos = Inches.of(-10); + private static final Distance kBackRightYPos = Inches.of(-10); + + + public static final SwerveModuleConstants FrontLeft = + ConstantCreator.createModuleConstants( + kFrontLeftSteerMotorId, kFrontLeftDriveMotorId, kFrontLeftEncoderId, Rotations.of(SwerveRotOffsets.FRONT_LEFT_ROT_OFFSET), + Inches.of(HALF_WIDTH), Inches.of(HALF_HEIGHT), false, false, false + ); + public static final SwerveModuleConstants FrontRight = + ConstantCreator.createModuleConstants( + kFrontRightSteerMotorId, kFrontRightDriveMotorId, kFrontRightEncoderId, kFrontRightEncoderOffset, + kFrontRightXPos, kFrontRightYPos, false, false, false + ); + public static final SwerveModuleConstants BackLeft = + ConstantCreator.createModuleConstants( + kBackLeftSteerMotorId, kBackLeftDriveMotorId, kBackLeftEncoderId, kBackLeftEncoderOffset, + kBackLeftXPos, kBackLeftYPos, false, false, false + ); + public static final SwerveModuleConstants BackRight = // misc public static final int TIMEOUT_MS = 30; public static final int SMARTDASHBOARD_UPDATE_FRAME = 2;