From 2931467e10c03bf090b86583d12bbba3c02f24eb Mon Sep 17 00:00:00 2001 From: Jack Lingle Date: Wed, 20 Nov 2024 19:30:49 -0500 Subject: [PATCH 1/5] json class works, begin transition to schemas --- DriverConstants.json | 3 + src/main/java/frc/robot/Robot.java | 3 + src/main/java/frc/robot/RobotContainer.java | 42 +++--- .../robot/commands/DriveWithJoysticks.java | 6 +- .../frc/robot/constants/ConstantsLoader.java | 48 +++++++ .../robot/constants/ConversionConstants.java | 14 +- .../frc/robot/constants/DriverConstants.java | 6 - .../constants/DriverConstantsSchema.java | 5 + .../frc/robot/constants/FeatureFlags.java | 13 -- .../robot/constants/FeatureFlagsSchema.java | 13 ++ .../frc/robot/constants/FieldConstants.java | 72 ---------- .../robot/constants/FieldConstantsSchema.java | 72 ++++++++++ .../frc/robot/constants/IntakeConstants.java | 8 +- .../frc/robot/constants/LEDConstants.java | 4 +- .../frc/robot/constants/ModeConstants.java | 8 +- .../constants/PhoenixDriveConstants.java | 132 +++++++++--------- .../frc/robot/constants/ScoringConstants.java | 106 +++++++------- .../frc/robot/constants/SensorConstants.java | 6 +- .../frc/robot/constants/SimConstants.java | 4 +- .../frc/robot/constants/VisionConstants.java | 30 ++-- .../robot/subsystems/drive/PhoenixDrive.java | 33 ++--- .../java/frc/robot/utils/AllianceUtil.java | 68 ++++----- 22 files changed, 376 insertions(+), 320 deletions(-) create mode 100644 DriverConstants.json create mode 100644 src/main/java/frc/robot/constants/ConstantsLoader.java delete mode 100644 src/main/java/frc/robot/constants/DriverConstants.java create mode 100644 src/main/java/frc/robot/constants/DriverConstantsSchema.java delete mode 100644 src/main/java/frc/robot/constants/FeatureFlags.java create mode 100644 src/main/java/frc/robot/constants/FeatureFlagsSchema.java delete mode 100644 src/main/java/frc/robot/constants/FieldConstants.java create mode 100644 src/main/java/frc/robot/constants/FieldConstantsSchema.java diff --git a/DriverConstants.json b/DriverConstants.json new file mode 100644 index 0000000..6a1bafa --- /dev/null +++ b/DriverConstants.json @@ -0,0 +1,3 @@ +{ + "joystickDeadband": 0.16 +} \ No newline at end of file diff --git a/src/main/java/frc/robot/Robot.java b/src/main/java/frc/robot/Robot.java index 3654448..12721f5 100644 --- a/src/main/java/frc/robot/Robot.java +++ b/src/main/java/frc/robot/Robot.java @@ -8,6 +8,7 @@ import edu.wpi.first.wpilibj.PowerDistribution.ModuleType; import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.CommandScheduler; +import frc.robot.constants.ConstantsLoader; import frc.robot.constants.ModeConstants; import frc.robot.constants.ModeConstants.Mode; import org.littletonrobotics.junction.LoggedRobot; @@ -49,6 +50,8 @@ public void robotInit() { // new // log }*/ + + ConstantsLoader.loadDriverConstants(); Logger.start(); } diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 7a9a33f..be9e3a4 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -15,7 +15,7 @@ import edu.wpi.first.wpilibj2.command.sysid.SysIdRoutine.Direction; import frc.robot.commands.DriveWithJoysticks; import frc.robot.commands.ShootWithGamepad; -import frc.robot.constants.FeatureFlags; +import frc.robot.constants.ConstantsLoader; import frc.robot.constants.ModeConstants; import frc.robot.constants.PhoenixDriveConstants; import frc.robot.constants.PhoenixDriveConstants.AlignTarget; @@ -108,22 +108,24 @@ private void configureNamedCommands() { } private void configureSubsystems() { - if (FeatureFlags.runDrive) { + if (ConstantsLoader.FeatureFlags.runDrive) { initDrive(); } - if (FeatureFlags.runVision) { + if (ConstantsLoader.FeatureFlags.runVision) { initVision(); } - if (FeatureFlags.runScoring) { + if (ConstantsLoader.FeatureFlags.runScoring) { initScoring(); } - if (FeatureFlags.runIntake) { + if (ConstantsLoader.FeatureFlags.runIntake) { initIntake(); } - if (FeatureFlags.runScoring && FeatureFlags.runIntake && FeatureFlags.runLEDS) { + if (ConstantsLoader.FeatureFlags.runScoring + && ConstantsLoader.FeatureFlags.runIntake + && ConstantsLoader.FeatureFlags.runLEDS) { initLEDs(); } - if (FeatureFlags.runOrchestra) { + if (ConstantsLoader.FeatureFlags.runOrchestra) { initOrchestra(); } } @@ -171,7 +173,7 @@ private void initVision() { tagVision = new VisionLocalizer(new CameraContainerReal(VisionConstants.cameras)); break; case SIM: - if (FeatureFlags.runDrive) { + if (ConstantsLoader.FeatureFlags.runDrive) { tagVision = new VisionLocalizer( new CameraContainerSim( @@ -227,14 +229,14 @@ private void initOrchestra() { orchestraSubsystem = new OrchestraSubsystem( "music/mii_channel.chrp"); // TODO: Add music files to deploy! - if (FeatureFlags.runScoring) { + if (ConstantsLoader.FeatureFlags.runScoring) { orchestraSubsystem.addInstruments( scoringSubsystem.getAimerIO().getOrchestraMotors()); orchestraSubsystem.addInstruments( scoringSubsystem.getShooterIO().getOrchestraMotors()); } - if (FeatureFlags.runDrive) { + if (ConstantsLoader.FeatureFlags.runDrive) { orchestraSubsystem.addInstrument(drive.getModule(0).getDriveMotor()); orchestraSubsystem.addInstrument(drive.getModule(0).getSteerMotor()); orchestraSubsystem.addInstrument(drive.getModule(1).getDriveMotor()); @@ -250,7 +252,7 @@ private void initOrchestra() { } private void configureSuppliers() { - if (FeatureFlags.runScoring) { + if (ConstantsLoader.FeatureFlags.runScoring) { Supplier poseSupplier; if (drive != null) { poseSupplier = () -> drive.getState().Pose; @@ -262,7 +264,7 @@ private void configureSuppliers() { scoringSubsystem.setDriveAlignedSupplier(() -> drive.isDriveAligned()); } - if (FeatureFlags.runIntake) { + if (ConstantsLoader.FeatureFlags.runIntake) { BooleanSupplier shooterHasNote; BooleanSupplier shooterInIntakePosition; if (scoringSubsystem != null) { @@ -282,7 +284,7 @@ private void configureSuppliers() { intakeSubsystem.setShooterAtIntakePosition(shooterInIntakePosition); } - if (FeatureFlags.runVision) { + if (ConstantsLoader.FeatureFlags.runVision) { if (drive != null) { tagVision.setCameraConsumer( (m) -> drive.addVisionMeasurement(m.pose(), m.timestamp(), m.variance())); @@ -291,8 +293,8 @@ private void configureSuppliers() { } } - if (FeatureFlags.runLEDS) { - if (FeatureFlags.runVision) { + if (ConstantsLoader.FeatureFlags.runLEDS) { + if (ConstantsLoader.FeatureFlags.runVision) { // leds.setVisionWorkingSupplier(() -> tagVision.coprocessorConnected()); } else { leds.setVisionWorkingSupplier(() -> false); @@ -349,7 +351,7 @@ private void configureBindings() { } } - if (FeatureFlags.runIntake) { + if (ConstantsLoader.FeatureFlags.runIntake) { masher.b() .onTrue(new InstantCommand(() -> intakeSubsystem.run(IntakeAction.INTAKE))) .onFalse(new InstantCommand(() -> intakeSubsystem.run(IntakeAction.NONE))); @@ -374,7 +376,7 @@ private void configureBindings() { .onFalse(new InstantCommand(() -> intakeSubsystem.run(IntakeAction.NONE))); } - if (FeatureFlags.runScoring) { + if (ConstantsLoader.FeatureFlags.runScoring) { scoringSubsystem.setDefaultCommand( new ShootWithGamepad( () -> rightJoystick.getHID().getRawButton(4), @@ -385,7 +387,7 @@ private void configureBindings() { masher.getHID()::getBButton, scoringSubsystem, () -> drive.getAlignTarget())); - // FeatureFlags.runDrive ? drivetrain::getAlignTarget : () -> AlignTarget.NONE)); + // gs.runDrive ? drivetrain::getAlignTarget : () -> AlignTarget.NONE)); rightJoystick .button(11) @@ -416,7 +418,7 @@ private void configureBindings() { masher.povUp(); } - if (FeatureFlags.runDrive) { + if (ConstantsLoader.FeatureFlags.runDrive) { masher.povUp() .onTrue(new InstantCommand(() -> drive.setAlignTarget(AlignTarget.SPEAKER))); @@ -760,7 +762,7 @@ public void testInit() { } private void setUpDriveWithJoysticks() { - if (FeatureFlags.runDrive) { + if (ConstantsLoader.FeatureFlags.runDrive) { drive.setDefaultCommand(new DriveWithJoysticks(drive, leftJoystick, rightJoystick)); } } diff --git a/src/main/java/frc/robot/commands/DriveWithJoysticks.java b/src/main/java/frc/robot/commands/DriveWithJoysticks.java index 07fbc18..30fe952 100644 --- a/src/main/java/frc/robot/commands/DriveWithJoysticks.java +++ b/src/main/java/frc/robot/commands/DriveWithJoysticks.java @@ -7,7 +7,7 @@ import edu.wpi.first.networktables.NetworkTableInstance; import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.button.CommandJoystick; -import frc.robot.constants.DriverConstants; +import frc.robot.constants.ConstantsLoader; import frc.robot.constants.PhoenixDriveConstants; import frc.robot.subsystems.drive.PhoenixDrive; @@ -51,13 +51,13 @@ public void execute() { Deadband.twoAxisDeadband( leftJoystick.getX(), leftJoystick.getY(), - DriverConstants.leftJoystickDeadband); + ConstantsLoader.DriverConstants.joystickDeadband); double filteredVY = -leftJoystickDeadbanded[1] * PhoenixDriveConstants.maxSpeedMetPerSec; double filteredVX = -leftJoystickDeadbanded[0] * PhoenixDriveConstants.maxSpeedMetPerSec; double rightJoystickDeadbanded = Deadband.oneAxisDeadband( - rightJoystick.getX(), DriverConstants.rightJoystickDeadband); + rightJoystick.getX(), ConstantsLoader.DriverConstants.joystickDeadband); double filteredOmega = -rightJoystickDeadbanded * PhoenixDriveConstants.MaxAngularRateRadPerSec; diff --git a/src/main/java/frc/robot/constants/ConstantsLoader.java b/src/main/java/frc/robot/constants/ConstantsLoader.java new file mode 100644 index 0000000..1159605 --- /dev/null +++ b/src/main/java/frc/robot/constants/ConstantsLoader.java @@ -0,0 +1,48 @@ +package frc.robot.constants; + +import coppercore.parameter_tools.JSONSync; + +public class ConstantsLoader { + /* + * NOTE: Each json constants file needs a schema matching its structure + * + * -> that schema is then put into a new JSON Sync class, + * class' load method is called, + * and then object is retrieved for holding in corresponding static variable + * + * IMPORTANT: file paths are from base folder (High-Key-2024) + */ + public static DriverConstantsSchema DriverConstants; + public static FeatureFlagsSchema FeatureFlags; + public static FieldConstantsSchema FieldConstants; + + public static void loadDriverConstants() { + JSONSync synced = + new JSONSync( + new DriverConstantsSchema(), + "./DriverConstants.json", + new JSONSync.JSONSyncConfigBuilder().build()); + synced.loadData(); + DriverConstants = synced.getObject(); + } + + public static void loadFeatureFlags() { + JSONSync synced = + new JSONSync( + new FeatureFlagsSchema(), + "./DriverConstants.json", + new JSONSync.JSONSyncConfigBuilder().build()); + synced.loadData(); + FeatureFlags = synced.getObject(); + } + + public static void loadFieldConstants() { + JSONSync synced = + new JSONSync( + new FieldConstantsSchema(), + "./DriverConstants.json", + new JSONSync.JSONSyncConfigBuilder().build()); + synced.loadData(); + FieldConstants = synced.getObject(); + } +} diff --git a/src/main/java/frc/robot/constants/ConversionConstants.java b/src/main/java/frc/robot/constants/ConversionConstants.java index 410eadd..3acb6e4 100644 --- a/src/main/java/frc/robot/constants/ConversionConstants.java +++ b/src/main/java/frc/robot/constants/ConversionConstants.java @@ -1,12 +1,12 @@ package frc.robot.constants; -public final class ConversionConstants { - public static final double kRadiansPerSecondToRPM = 60.0 / (2.0 * Math.PI); - public static final double kRPMToRadiansPerSecond = 1.0 / kRadiansPerSecondToRPM; +public class ConversionConstants { + public double kRadiansPerSecondToRPM = 60.0 / (2.0 * Math.PI); + public double kRPMToRadiansPerSecond = 1.0 / kRadiansPerSecondToRPM; - public static final double kSecondsToMinutes = 1.0 / 60.0; - public static final double kMinutesToSeconds = 60.0; + public double kSecondsToMinutes = 1.0 / 60.0; + public double kMinutesToSeconds = 60.0; - public static final double kDegreesToRadians = Math.PI / 180.0; - public static final double kRadiansToDegrees = 180.0 / Math.PI; + public double kDegreesToRadians = Math.PI / 180.0; + public double kRadiansToDegrees = 180.0 / Math.PI; } diff --git a/src/main/java/frc/robot/constants/DriverConstants.java b/src/main/java/frc/robot/constants/DriverConstants.java deleted file mode 100644 index 89f8bcf..0000000 --- a/src/main/java/frc/robot/constants/DriverConstants.java +++ /dev/null @@ -1,6 +0,0 @@ -package frc.robot.constants; - -public final class DriverConstants { - public static final double leftJoystickDeadband = 0.16; - public static final double rightJoystickDeadband = 0.16; -} diff --git a/src/main/java/frc/robot/constants/DriverConstantsSchema.java b/src/main/java/frc/robot/constants/DriverConstantsSchema.java new file mode 100644 index 0000000..660ad91 --- /dev/null +++ b/src/main/java/frc/robot/constants/DriverConstantsSchema.java @@ -0,0 +1,5 @@ +package frc.robot.constants; + +public class DriverConstantsSchema { + public double joystickDeadband = 0.16; +} diff --git a/src/main/java/frc/robot/constants/FeatureFlags.java b/src/main/java/frc/robot/constants/FeatureFlags.java deleted file mode 100644 index 57fcc6c..0000000 --- a/src/main/java/frc/robot/constants/FeatureFlags.java +++ /dev/null @@ -1,13 +0,0 @@ -package frc.robot.constants; - -public final class FeatureFlags { - public static final boolean runDrive = true; - public static final boolean runVision = false; - // NOTE: A featureflag "runLocalizer" was removed from here recently. - // TODO: Figure out if we need this and add it back if necessary. - public static final boolean runLocalizer = false; - public static final boolean runIntake = true; - public static final boolean runScoring = true; - public static final boolean runLEDS = true; - public static final boolean runOrchestra = true; -} diff --git a/src/main/java/frc/robot/constants/FeatureFlagsSchema.java b/src/main/java/frc/robot/constants/FeatureFlagsSchema.java new file mode 100644 index 0000000..9a6455a --- /dev/null +++ b/src/main/java/frc/robot/constants/FeatureFlagsSchema.java @@ -0,0 +1,13 @@ +package frc.robot.constants; + +public class FeatureFlagsSchema { + public boolean runDrive = true; + public boolean runVision = false; + // NOTE: A featureflag "runLocalizer" was removed from here recently. + // TODO: Figure out if we need this and add it back if necessary. + public boolean runLocalizer = false; + public boolean runIntake = true; + public boolean runScoring = true; + public boolean runLEDS = true; + public boolean runOrchestra = true; +} diff --git a/src/main/java/frc/robot/constants/FieldConstants.java b/src/main/java/frc/robot/constants/FieldConstants.java deleted file mode 100644 index cc25c6d..0000000 --- a/src/main/java/frc/robot/constants/FieldConstants.java +++ /dev/null @@ -1,72 +0,0 @@ -package frc.robot.constants; - -import edu.wpi.first.math.geometry.Pose2d; -import edu.wpi.first.math.geometry.Rotation2d; -import edu.wpi.first.math.geometry.Translation2d; -import edu.wpi.first.math.util.Units; - -public final class FieldConstants { - public static final double lengthM = 16.451; - public static final double widthM = 8.211; - - public static final double midfieldLowThresholdM = 5.87; - public static final double midfieldHighThresholdM = 10.72; - - public static final Rotation2d ampHeading = new Rotation2d(Math.PI / 2); - - public static final Rotation2d blueUpHeading = Rotation2d.fromRadians(0.0); - public static final Rotation2d blueDownHeading = Rotation2d.fromRadians(Math.PI); - public static final Rotation2d blueLeftHeading = Rotation2d.fromRadians(Math.PI / 2.0); - public static final Rotation2d blueRightHeading = Rotation2d.fromRadians(-Math.PI / 2.0); - - public static final Rotation2d redUpHeading = Rotation2d.fromRadians(Math.PI); - public static final Rotation2d redDownHeading = Rotation2d.fromRadians(0.0); - public static final Rotation2d redLeftHeading = Rotation2d.fromRadians(-Math.PI / 2.0); - public static final Rotation2d redRightHeading = Rotation2d.fromRadians(Math.PI / 2.0); - - public static final Rotation2d redSourceHeading = new Rotation2d(Math.PI * 4 / 3); // 60 degrees - public static final Rotation2d blueSourceHeading = - new Rotation2d(Math.PI * 5 / 3); // 120 degrees - - public static final Translation2d fieldToRedSpeaker = - new Translation2d(Units.inchesToMeters(652.73), Units.inchesToMeters(218.42)); - - public static final Translation2d fieldToBlueSpeaker = - new Translation2d(Units.inchesToMeters(-1.5), Units.inchesToMeters(218.42)); - - public static final Translation2d fieldToBluePass = - new Translation2d(Units.inchesToMeters(25), Units.inchesToMeters(230)); - - public static final Translation2d fieldToRedPass = - new Translation2d(Units.inchesToMeters(625), Units.inchesToMeters(230)); - - public static final Pose2d robotAgainstBlueSpeaker = - new Pose2d(1.39, 5.56, Rotation2d.fromDegrees(180)); - - public static final Pose2d robotAgainstRedSpeaker = - new Pose2d(15.19, 5.56, Rotation2d.fromDegrees(0)); - - public static final Pose2d robotAgainstBlueSpeakerRight = - new Pose2d(0.7, 4.38, Rotation2d.fromDegrees(120)); - - public static final Pose2d robotAgainstRedSpeakerRight = - new Pose2d(15.83, 6.73, Rotation2d.fromDegrees(-60)); - - public static final Pose2d robotAgainstBlueSpeakerLeft = - new Pose2d(0.7, 6.73, Rotation2d.fromDegrees(-120)); - - public static final Pose2d robotAgainstRedSpeakerLeft = - new Pose2d(15.83, 4.38, Rotation2d.fromDegrees(60)); - - public static final Pose2d robotAgainstBluePodium = - new Pose2d(2.57, 4.09, Rotation2d.fromDegrees(180)); - - public static final Pose2d robotAgainstRedPodium = - new Pose2d(13.93, 4.09, Rotation2d.fromDegrees(0)); - - public static final Pose2d robotAgainstBlueAmpZone = - new Pose2d(2.85, 7.68, Rotation2d.fromDegrees(-90)); - - public static final Pose2d robotAgainstRedAmpZone = - new Pose2d(13.74, 7.68, Rotation2d.fromDegrees(-90)); -} diff --git a/src/main/java/frc/robot/constants/FieldConstantsSchema.java b/src/main/java/frc/robot/constants/FieldConstantsSchema.java new file mode 100644 index 0000000..1956a39 --- /dev/null +++ b/src/main/java/frc/robot/constants/FieldConstantsSchema.java @@ -0,0 +1,72 @@ +package frc.robot.constants; + +import edu.wpi.first.math.geometry.Pose2d; +import edu.wpi.first.math.geometry.Rotation2d; +import edu.wpi.first.math.geometry.Translation2d; +import edu.wpi.first.math.util.Units; + +public class FieldConstantsSchema { + public double lengthM = 16.451; + public double widthM = 8.211; + + public double midfieldLowThresholdM = 5.87; + public double midfieldHighThresholdM = 10.72; + + public Rotation2d ampHeading = new Rotation2d(Math.PI / 2); + + public Rotation2d blueUpHeading = Rotation2d.fromRadians(0.0); + public Rotation2d blueDownHeading = Rotation2d.fromRadians(Math.PI); + public Rotation2d blueLeftHeading = Rotation2d.fromRadians(Math.PI / 2.0); + public Rotation2d blueRightHeading = Rotation2d.fromRadians(-Math.PI / 2.0); + + public Rotation2d redUpHeading = Rotation2d.fromRadians(Math.PI); + public Rotation2d redDownHeading = Rotation2d.fromRadians(0.0); + public Rotation2d redLeftHeading = Rotation2d.fromRadians(-Math.PI / 2.0); + public Rotation2d redRightHeading = Rotation2d.fromRadians(Math.PI / 2.0); + + public Rotation2d redSourceHeading = new Rotation2d(Math.PI * 4 / 3); // 60 degrees + public Rotation2d blueSourceHeading = + new Rotation2d(Math.PI * 5 / 3); // 120 degrees + + public Translation2d fieldToRedSpeaker = + new Translation2d(Units.inchesToMeters(652.73), Units.inchesToMeters(218.42)); + + public Translation2d fieldToBlueSpeaker = + new Translation2d(Units.inchesToMeters(-1.5), Units.inchesToMeters(218.42)); + + public Translation2d fieldToBluePass = + new Translation2d(Units.inchesToMeters(25), Units.inchesToMeters(230)); + + public Translation2d fieldToRedPass = + new Translation2d(Units.inchesToMeters(625), Units.inchesToMeters(230)); + + public Pose2d robotAgainstBlueSpeaker = + new Pose2d(1.39, 5.56, Rotation2d.fromDegrees(180)); + + public Pose2d robotAgainstRedSpeaker = + new Pose2d(15.19, 5.56, Rotation2d.fromDegrees(0)); + + public Pose2d robotAgainstBlueSpeakerRight = + new Pose2d(0.7, 4.38, Rotation2d.fromDegrees(120)); + + public Pose2d robotAgainstRedSpeakerRight = + new Pose2d(15.83, 6.73, Rotation2d.fromDegrees(-60)); + + public Pose2d robotAgainstBlueSpeakerLeft = + new Pose2d(0.7, 6.73, Rotation2d.fromDegrees(-120)); + + public Pose2d robotAgainstRedSpeakerLeft = + new Pose2d(15.83, 4.38, Rotation2d.fromDegrees(60)); + + public Pose2d robotAgainstBluePodium = + new Pose2d(2.57, 4.09, Rotation2d.fromDegrees(180)); + + public Pose2d robotAgainstRedPodium = + new Pose2d(13.93, 4.09, Rotation2d.fromDegrees(0)); + + public Pose2d robotAgainstBlueAmpZone = + new Pose2d(2.85, 7.68, Rotation2d.fromDegrees(-90)); + + public Pose2d robotAgainstRedAmpZone = + new Pose2d(13.74, 7.68, Rotation2d.fromDegrees(-90)); +} diff --git a/src/main/java/frc/robot/constants/IntakeConstants.java b/src/main/java/frc/robot/constants/IntakeConstants.java index 933f8df..31ac5e7 100644 --- a/src/main/java/frc/robot/constants/IntakeConstants.java +++ b/src/main/java/frc/robot/constants/IntakeConstants.java @@ -1,9 +1,9 @@ package frc.robot.constants; public class IntakeConstants { - public static final int intakeMotorID = 6; - public static final int centeringMotorID = 4; + public int intakeMotorID = 6; + public int centeringMotorID = 4; - public static final double intakePower = 12.0; - public static final double beltPower = 12.0; + public double intakePower = 12.0; + public double beltPower = 12.0; } diff --git a/src/main/java/frc/robot/constants/LEDConstants.java b/src/main/java/frc/robot/constants/LEDConstants.java index 309b8af..2ccd44b 100644 --- a/src/main/java/frc/robot/constants/LEDConstants.java +++ b/src/main/java/frc/robot/constants/LEDConstants.java @@ -2,7 +2,7 @@ public class LEDConstants { - public static final int ledPort = 0; + public int ledPort = 0; - public static final int ledLength = 30; + public int ledLength = 30; } diff --git a/src/main/java/frc/robot/constants/ModeConstants.java b/src/main/java/frc/robot/constants/ModeConstants.java index e99b7b7..94c6b36 100644 --- a/src/main/java/frc/robot/constants/ModeConstants.java +++ b/src/main/java/frc/robot/constants/ModeConstants.java @@ -2,8 +2,8 @@ import frc.robot.Robot; -public final class ModeConstants { - public static enum Mode { +public class ModeConstants { + public enum Mode { REAL, SIM, REPLAY @@ -11,7 +11,7 @@ public static enum Mode { // Whether sim should be treated as sim or replay mode. // Will automatically be overridden by Mode.REAL if running on real hardware. - public static final Mode simMode = Mode.SIM; // Mode.SIM or Mode.REPLAY + public Mode simMode = Mode.SIM; // Mode.SIM or Mode.REPLAY - public static final Mode currentMode = Robot.isReal() ? Mode.REAL : simMode; + public Mode currentMode = Robot.isReal() ? Mode.REAL : simMode; } diff --git a/src/main/java/frc/robot/constants/PhoenixDriveConstants.java b/src/main/java/frc/robot/constants/PhoenixDriveConstants.java index 7d4bee9..8e64331 100644 --- a/src/main/java/frc/robot/constants/PhoenixDriveConstants.java +++ b/src/main/java/frc/robot/constants/PhoenixDriveConstants.java @@ -10,7 +10,7 @@ import edu.wpi.first.math.kinematics.SwerveDriveKinematics; import edu.wpi.first.math.util.Units; -public final class PhoenixDriveConstants { +public class PhoenixDriveConstants { public enum AlignTarget { NONE, AMP, @@ -28,7 +28,7 @@ public enum AlignTarget { // The steer motor uses any SwerveModule.SteerRequestType control request with the // output type specified by SwerveModuleConstants.SteerMotorClosedLoopOutput - public static final Slot0Configs steerGains = + public Slot0Configs steerGains = new Slot0Configs() .withKP(150) .withKI(50) @@ -38,7 +38,7 @@ public enum AlignTarget { .withKA(0); // When using closed-loop control, the drive motor uses the control // output type specified by SwerveModuleConstants.DriveMotorClosedLoopOutput - public static final Slot0Configs driveGains = + public Slot0Configs driveGains = new Slot0Configs() .withKP(0.1) .withKI(12.0) @@ -47,53 +47,53 @@ public enum AlignTarget { .withKV(0.12) .withKA(0.01); - public static final double alignmentkP = 4.0; - public static final double alignmentkI = 0.0; - public static final double alignmentkD = 0.0; - public static final double alignToleranceRadians = 0.1; + public double alignmentkP = 4.0; + public double alignmentkI = 0.0; + public double alignmentkD = 0.0; + public double alignToleranceRadians = 0.1; // 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 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 ClosedLoopOutputType driveClosedLoopOutput = ClosedLoopOutputType.Voltage; - private static final double kSlipCurrentA = 80; + private double kSlipCurrentA = 80; - public static final double kSpeedAt12VoltsMps = 5.02; // 5.21 OR 5.02 - public static final double maxSpeedMetPerSec = 6; - public static final double MaxAngularRateRadPerSec = Math.PI * 2; + public double kSpeedAt12VoltsMps = 5.02; // 5.21 OR 5.02 + public double maxSpeedMetPerSec = 6; + public double MaxAngularRateRadPerSec = Math.PI * 2; - public static final double autoAlignmentkP = 5.0; - public static final double autoAlignmentkI = 5.5; - public static final double autoAlignmentkD = 0.0; + public double autoAlignmentkP = 5.0; + public double autoAlignmentkI = 5.5; + public double autoAlignmentkD = 0.0; // 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 double kCoupleRatio = 3.5714285714285716; - private static final double kDriveGearRatio = 5.142857; - private static final double kSteerGearRatio = 25; - private static final double kWheelRadiusInches = 2; + private double kDriveGearRatio = 5.142857; + private double kSteerGearRatio = 25; + private double kWheelRadiusInches = 2; - private static final boolean kInvertLeftSide = false; - private static final boolean kInvertRightSide = true; + private boolean kInvertLeftSide = false; + private boolean kInvertRightSide = true; - private static final String kCANbusName = "Canivore"; - private static final int kPigeonId = 20; + private String kCANbusName = "Canivore"; + private int kPigeonId = 20; // These are only used for simulation - private static final double kSteerInertia = 0.00001; - private static final double kDriveInertia = 0.001; + private double kSteerInertia = 0.00001; + private 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 double kSteerFrictionVoltage = 0.25; + private double kDriveFrictionVoltage = 0.25; - public static final SwerveDrivetrainConstants DrivetrainConstants = + public SwerveDrivetrainConstants DrivetrainConstants = new SwerveDrivetrainConstants().withPigeon2Id(kPigeonId).withCANbusName(kCANbusName); - private static final SwerveModuleConstantsFactory ConstantCreator = + private SwerveModuleConstantsFactory ConstantCreator = new SwerveModuleConstantsFactory() .withDriveMotorGearRatio(kDriveGearRatio) .withSteerMotorGearRatio(kSteerGearRatio) @@ -112,53 +112,53 @@ public enum AlignTarget { .withCouplingGearRatio(kCoupleRatio); // sim constants - public static final double kSimLoopPeriod = 0.005; + public double kSimLoopPeriod = 0.005; // Front Left - private static final int kFrontLeftDriveMotorId = 7; - private static final int kFrontLeftSteerMotorId = 8; - private static final int kFrontLeftEncoderId = 9; - private static final double kFrontLeftEncoderOffset = -0.267822265625; - private static final boolean kFrontLeftSteerInvert = false; + private int kFrontLeftDriveMotorId = 7; + private int kFrontLeftSteerMotorId = 8; + private int kFrontLeftEncoderId = 9; + private double kFrontLeftEncoderOffset = -0.267822265625; + private boolean kFrontLeftSteerInvert = false; - private static final double kFrontLeftXPosInches = 10.75; - private static final double kFrontLeftYPosInches = 11.5; + private double kFrontLeftXPosInches = 10.75; + private double kFrontLeftYPosInches = 11.5; // Front Right - private static final int kFrontRightDriveMotorId = 1; - private static final int kFrontRightSteerMotorId = 2; - private static final int kFrontRightEncoderId = 12; - private static final double kFrontRightEncoderOffset = -0.228271484375; - private static final boolean kFrontRightSteerInvert = false; + private int kFrontRightDriveMotorId = 1; + private int kFrontRightSteerMotorId = 2; + private int kFrontRightEncoderId = 12; + private double kFrontRightEncoderOffset = -0.228271484375; + private boolean kFrontRightSteerInvert = false; - private static final double kFrontRightXPosInches = 10.75; - private static final double kFrontRightYPosInches = -11.5; + private double kFrontRightXPosInches = 10.75; + private double kFrontRightYPosInches = -11.5; // Back Left - private static final int kBackLeftDriveMotorId = 5; - private static final int kBackLeftSteerMotorId = 6; - private static final int kBackLeftEncoderId = 10; - private static final double kBackLeftEncoderOffset = .084716796875; - private static final boolean kBackLeftSteerInvert = false; - private static final boolean kBackLeftDriveInvert = true; + private int kBackLeftDriveMotorId = 5; + private int kBackLeftSteerMotorId = 6; + private int kBackLeftEncoderId = 10; + private double kBackLeftEncoderOffset = .084716796875; + private boolean kBackLeftSteerInvert = false; + private boolean kBackLeftDriveInvert = true; - private static final double kBackLeftXPosInches = -10.75; - private static final double kBackLeftYPosInches = 11.5; + private double kBackLeftXPosInches = -10.75; + private double kBackLeftYPosInches = 11.5; // Back Right - private static final int kBackRightDriveMotorId = 3; - private static final int kBackRightSteerMotorId = 4; - private static final int kBackRightEncoderId = 11; - private static final double kBackRightEncoderOffset = 0.39990234375; - private static final boolean kBackRightSteerInvert = true; + private int kBackRightDriveMotorId = 3; + private int kBackRightSteerMotorId = 4; + private int kBackRightEncoderId = 11; + private double kBackRightEncoderOffset = 0.39990234375; + private boolean kBackRightSteerInvert = true; - private static final double kBackRightXPosInches = -10.75; - private static final double kBackRightYPosInches = -11.5; + private double kBackRightXPosInches = -10.75; + private double kBackRightYPosInches = -11.5; - public static final double kModuleRadiusMeters = + public double kModuleRadiusMeters = Units.inchesToMeters(Math.hypot(kFrontLeftXPosInches, kFrontLeftYPosInches)); - public static final SwerveModuleConstants FrontLeft = + public SwerveModuleConstants FrontLeft = ConstantCreator.createModuleConstants( kFrontLeftSteerMotorId, kFrontLeftDriveMotorId, @@ -168,7 +168,7 @@ public enum AlignTarget { Units.inchesToMeters(kFrontLeftYPosInches), kInvertLeftSide) .withSteerMotorInverted(kFrontLeftSteerInvert); - public static final SwerveModuleConstants FrontRight = + public SwerveModuleConstants FrontRight = ConstantCreator.createModuleConstants( kFrontRightSteerMotorId, kFrontRightDriveMotorId, @@ -178,7 +178,7 @@ public enum AlignTarget { Units.inchesToMeters(kFrontRightYPosInches), kInvertRightSide) .withSteerMotorInverted(kFrontRightSteerInvert); - public static final SwerveModuleConstants BackLeft = + public SwerveModuleConstants BackLeft = ConstantCreator.createModuleConstants( kBackLeftSteerMotorId, kBackLeftDriveMotorId, @@ -189,7 +189,7 @@ public enum AlignTarget { kInvertLeftSide) .withSteerMotorInverted(kBackLeftSteerInvert) .withDriveMotorInverted(kBackLeftDriveInvert); - public static final SwerveModuleConstants BackRight = + public SwerveModuleConstants BackRight = ConstantCreator.createModuleConstants( kBackRightSteerMotorId, kBackRightDriveMotorId, @@ -200,7 +200,7 @@ public enum AlignTarget { kInvertRightSide) .withSteerMotorInverted(kBackRightSteerInvert); - public static final SwerveDriveKinematics kinematics = + public SwerveDriveKinematics kinematics = new SwerveDriveKinematics( new Translation2d(FrontLeft.LocationX, FrontLeft.LocationY), new Translation2d(FrontLeft.LocationX, FrontRight.LocationY), diff --git a/src/main/java/frc/robot/constants/ScoringConstants.java b/src/main/java/frc/robot/constants/ScoringConstants.java index 1cfd1e4..fc7e86b 100644 --- a/src/main/java/frc/robot/constants/ScoringConstants.java +++ b/src/main/java/frc/robot/constants/ScoringConstants.java @@ -4,80 +4,80 @@ import java.util.HashMap; public class ScoringConstants { - public static final double aimerkP = 100.0; - public static final double aimerkI = 2.0; // 5.0 - public static final double aimerkD = 0.0; + public final double aimerkP = 100.0; + public final double aimerkI = 2.0; // 5.0 + public final double aimerkD = 0.0; - public static final double aimerkS = 0.3; // 0.25; - public static final double aimerkG = 0.955; // 0.2; - public static final double aimerkV = 0.0; - public static final double aimerkA = 0.0; + public final double aimerkS = 0.3; // 0.25; + public final double aimerkG = 0.955; // 0.2; + public final double aimerkV = 0.0; + public final double aimerkA = 0.0; - public static final double shooterkP = 0.05; - public static final double shooterkI = 1; - public static final double shooterkD = 0.0; + public final double shooterkP = 0.05; + public final double shooterkI = 1; + public final double shooterkD = 0.0; - public static final double shooterkS = 0.0; // TODO: Find Imperically - public static final double shooterkV = 0.005; - public static final double shooterkA = 0.0; + public final double shooterkS = 0.0; // TODO: Find Imperically + public final double shooterkV = 0.005; + public final double shooterkA = 0.0; - public static final int aimerMotorId = 9; + public final int aimerMotorId = 9; - public static final int kickerMotorId = 5; // 1 + public final int kickerMotorId = 5; // 1 // TODO: REPLACE THIS WHEN THE ACTUAL SHOOTER IO IS MERGED - public static final int shooterLeftMotorId = 10; - public static final int shooterRightMotorId = 11; + public final int shooterLeftMotorId = 10; + public final int shooterRightMotorId = 11; - public static final double shooterCurrentLimit = 120; - public static final double kickerCurrentLimit = 120; - public static final double aimerCurrentLimit = 60; + public final double shooterCurrentLimit = 120; + public final double kickerCurrentLimit = 120; + public final double aimerCurrentLimit = 60; - public static final int aimerEncoderId = 13; + public final int aimerEncoderId = 13; - public static final double aimerEncoderOffset = -0.087402; // Armencoder is zeroed + public final double aimerEncoderOffset = -0.087402; // Armencoder is zeroed - public static final double aimerEncoderToMechanismRatio = 1.0; - public static final double aimerRotorToSensorRatio = 90.0; + public final double aimerEncoderToMechanismRatio = 1.0; + public final double aimerRotorToSensorRatio = 90.0; - public static final double kickerIntakeVolts = 2.0; + public final double kickerIntakeVolts = 2.0; - // public static final double aimPositionTolerance = 0.003; + // public final double aimPositionTolerance = 0.003; - public static final double aimerAcceleration = 3.5; - public static final double aimerCruiseVelocity = 0.8; + public final double aimerAcceleration = 3.5; + public final double aimerCruiseVelocity = 0.8; - public static final double shooterLowerVelocityMarginRPM = 50; - public static final double shooterUpperVelocityMarginRPM = 150; - public static final double aimAngleMarginRotations = Units.degreesToRotations(1); - public static final double aimAngleVelocityMargin = 2.0; + public final double shooterLowerVelocityMarginRPM = 50; + public final double shooterUpperVelocityMarginRPM = 150; + public final double aimAngleMarginRotations = Units.degreesToRotations(1); + public final double aimAngleVelocityMargin = 2.0; - public static final double minDistanceAlignmentNeeded = 1.3; // TODO: Tune this value + public final double minDistanceAlignmentNeeded = 1.3; // TODO: Tune this value - public static final double intakeAngleToleranceRotations = 0.05; // Todo: tune this value + public final double intakeAngleToleranceRotations = 0.05; // Todo: tune this value - public static final double aimerAmpPositionRot = 0.145; + public final double aimerAmpPositionRot = 0.145; - public static final double aimMaxAngleRotations = 0.361328 - 0.184570; - public static final double aimMinAngleRotations = -0.212285; - public static final double aimLockVoltage = -0.5; + public final double aimMaxAngleRotations = 0.361328 - 0.184570; + public final double aimMinAngleRotations = -0.212285; + public final double aimLockVoltage = -0.5; - public static final double aimAngleTolerance = 0.015; + public final double aimAngleTolerance = 0.015; - public static final double ampAimerAngleRotations = 0.14; + public final double ampAimerAngleRotations = 0.14; - public static final double maxAimIntake = 0.0; - public static final double minAimIntake = 0.0; + public final double maxAimIntake = 0.0; + public final double minAimIntake = 0.0; - public static final double shooterOffsetAdjustment = 0.6; + public final double shooterOffsetAdjustment = 0.6; - public static final double maxElevatorPosition = 0.45; - public static final double maxAimAngleElevatorLimit = Math.PI / 2; + public final double maxElevatorPosition = 0.45; + public final double maxAimAngleElevatorLimit = Math.PI / 2; // NOTE - This should be monotonically increasing // Key - Distance in meters // Value - Aimer angle in rotations - public static HashMap getAimerMap() { + public HashMap getAimerMap() { HashMap map = new HashMap(); // map.put(1.284, -0.1); map.put(1.428, -0.1); @@ -90,16 +90,16 @@ public static HashMap getAimerMap() { return map; } - public static final double passLocationRot = -0.14; - public static final double passAngleToleranceRot = 0.005; - public static final double passShooterRpm = 2800.0; + public final double passLocationRot = -0.14; + public final double passAngleToleranceRot = 0.005; + public final double passShooterRpm = 2800.0; - public static final double aimerStaticOffset = 0.0; + public final double aimerOffset = 0.0; // NOTE - This should be monotonically increasing // Key - Distance in meters // Value - Shooter RPM - public static HashMap getShooterMap() { + public HashMap getShooterMap() { HashMap map = new HashMap(); map.put(1.284, 3500.0); map.put(1.7, 4000.0); @@ -110,7 +110,7 @@ public static HashMap getShooterMap() { // NOTE - This should be monotonically increasing // Key - Distance in meters // Value - Time in seconds - public static HashMap timeToGoalMap() { + public HashMap timeToGoalMap() { HashMap map = new HashMap(); map.put(0.0, 0.15); map.put(1.3, 0.15); @@ -123,7 +123,7 @@ public static HashMap timeToGoalMap() { // NOTE - This should be monotonically increasing // Key - Distance to goal in meters // Value - Aimer angle tolerance in rotations - public static HashMap aimerToleranceTable() { + public HashMap aimerToleranceTable() { HashMap map = new HashMap(); map.put(0.0, 0.003); // Todo: tune this value after conversion from radians to rotations diff --git a/src/main/java/frc/robot/constants/SensorConstants.java b/src/main/java/frc/robot/constants/SensorConstants.java index 4721d71..ee67b2c 100644 --- a/src/main/java/frc/robot/constants/SensorConstants.java +++ b/src/main/java/frc/robot/constants/SensorConstants.java @@ -1,7 +1,7 @@ package frc.robot.constants; -public final class SensorConstants { +public class SensorConstants { // TODO: Find real values - public static final int indexerSensorPort = 0; - public static final int uptakeSensorPort = 1; + public int indexerSensorPort = 0; + public int uptakeSensorPort = 1; } diff --git a/src/main/java/frc/robot/constants/SimConstants.java b/src/main/java/frc/robot/constants/SimConstants.java index bedfc9c..bf2d2c6 100644 --- a/src/main/java/frc/robot/constants/SimConstants.java +++ b/src/main/java/frc/robot/constants/SimConstants.java @@ -1,5 +1,5 @@ package frc.robot.constants; -public final class SimConstants { - public static final double loopTime = 0.02; +public class SimConstants { + public double loopTime = 0.02; } diff --git a/src/main/java/frc/robot/constants/VisionConstants.java b/src/main/java/frc/robot/constants/VisionConstants.java index 56bac6f..0761a5f 100644 --- a/src/main/java/frc/robot/constants/VisionConstants.java +++ b/src/main/java/frc/robot/constants/VisionConstants.java @@ -16,27 +16,27 @@ import java.util.Collections; import java.util.List; -public final class VisionConstants { - public static final String tagLayoutName = "Pairs-Only"; - public static final AprilTagFieldLayout fieldLayout = initLayout(tagLayoutName); +public class VisionConstants { + public String tagLayoutName = "Pairs-Only"; + public AprilTagFieldLayout fieldLayout = initLayout(tagLayoutName); - public static final double lowUncertaintyCutoffDistance = 6.5; + public double lowUncertaintyCutoffDistance = 6.5; - public static final double skewCutoffDistance = 5.8; - public static final double skewCutoffRotation = Units.degreesToRadians(50); + public double skewCutoffDistance = 5.8; + public double skewCutoffRotation = Units.degreesToRadians(50); // Maximum average tag distance before a measurement is fully ignored - public static final double maxTagDistance = 8.0; + public double maxTagDistance = 8.0; - public static final Matrix teleopCameraUncertainty = VecBuilder.fill(0.35, 0.35, 3.5); + public Matrix teleopCameraUncertainty = VecBuilder.fill(0.35, 0.35, 3.5); - public static final Matrix lowCameraUncertainty = VecBuilder.fill(0.6, 1.0, 4); + public Matrix lowCameraUncertainty = VecBuilder.fill(0.6, 1.0, 4); - public static final Matrix highCameraUncertainty = VecBuilder.fill(12.0, 16.0, 40); + public Matrix highCameraUncertainty = VecBuilder.fill(12.0, 16.0, 40); - public static final Matrix driveUncertainty = VecBuilder.fill(0.1, 0.1, 0.1); + public Matrix driveUncertainty = VecBuilder.fill(0.1, 0.1, 0.1); - public static final List cameras = List.of(); + public List cameras = List.of(); // new CameraParams( // "Front-Left", // Front Right @@ -92,7 +92,7 @@ public final class VisionConstants { // new Rotation3d(0.0, -0.349, 0.524)), // CameraTrustZone.MIDDLE)); - public static record CameraParams( + public record CameraParams( String name, int xResolution, int yResolution, @@ -101,10 +101,10 @@ public static record CameraParams( Transform3d robotToCamera, CameraTrustZone zone) {} - private static AprilTagFieldLayout initLayout(String name) { + private AprilTagFieldLayout initLayout(String name) { AprilTagFieldLayout layout; // AprilTagFieldLayout's constructor throws an IOException, so we must catch it - // in order to initialize our layout as a static constant + // in order to initialize our layout as a constant try { layout = new AprilTagFieldLayout( diff --git a/src/main/java/frc/robot/subsystems/drive/PhoenixDrive.java b/src/main/java/frc/robot/subsystems/drive/PhoenixDrive.java index bb7125e..62ec5c0 100644 --- a/src/main/java/frc/robot/subsystems/drive/PhoenixDrive.java +++ b/src/main/java/frc/robot/subsystems/drive/PhoenixDrive.java @@ -31,7 +31,8 @@ import edu.wpi.first.wpilibj2.command.Commands; import edu.wpi.first.wpilibj2.command.Subsystem; import edu.wpi.first.wpilibj2.command.sysid.SysIdRoutine; -import frc.robot.constants.FieldConstants; +import frc.robot.constants.ConstantsLoader.FieldConstants; +import frc.robot.constants.ConstantsLoader; import frc.robot.constants.PhoenixDriveConstants; import frc.robot.constants.PhoenixDriveConstants.AlignTarget; import java.util.Optional; @@ -337,57 +338,57 @@ public Optional getAlignment() { && DriverStation.getAlliance().get() == Alliance.Blue) { return Optional.of( getTargetHeading( - new Pose2d(FieldConstants.fieldToBlueSpeaker, new Rotation2d()), + new Pose2d(ConstantsLoader.FieldConstants.fieldToBlueSpeaker, new Rotation2d()), false)); } else { return Optional.of( getTargetHeading( - new Pose2d(FieldConstants.fieldToRedSpeaker, new Rotation2d()), + new Pose2d(ConstantsLoader.FieldConstants.fieldToRedSpeaker, new Rotation2d()), true)); } case AMP: - return Optional.of(FieldConstants.ampHeading); + return Optional.of(ConstantsLoader.FieldConstants.ampHeading); case SOURCE: if (DriverStation.getAlliance().get() == Alliance.Blue) { - return Optional.of(FieldConstants.blueSourceHeading); + return Optional.of(ConstantsLoader.FieldConstants.blueSourceHeading); } else { - return Optional.of(FieldConstants.redSourceHeading); + return Optional.of(ConstantsLoader.FieldConstants.redSourceHeading); } case UP: if (DriverStation.getAlliance().get() == Alliance.Blue) { - return Optional.of(FieldConstants.blueUpHeading); + return Optional.of(ConstantsLoader.FieldConstants.blueUpHeading); } else { - return Optional.of(FieldConstants.redUpHeading); + return Optional.of(ConstantsLoader.FieldConstants.redUpHeading); } case DOWN: if (DriverStation.getAlliance().get() == Alliance.Blue) { - return Optional.of(FieldConstants.blueDownHeading); + return Optional.of(ConstantsLoader.FieldConstants.blueDownHeading); } else { - return Optional.of(FieldConstants.redDownHeading); + return Optional.of(ConstantsLoader.FieldConstants.redDownHeading); } case LEFT: if (DriverStation.getAlliance().get() == Alliance.Blue) { - return Optional.of(FieldConstants.blueLeftHeading); + return Optional.of(ConstantsLoader.FieldConstants.blueLeftHeading); } else { - return Optional.of(FieldConstants.redLeftHeading); + return Optional.of(ConstantsLoader.FieldConstants.redLeftHeading); } case RIGHT: if (DriverStation.getAlliance().get() == Alliance.Blue) { - return Optional.of(FieldConstants.blueRightHeading); + return Optional.of(ConstantsLoader.FieldConstants.blueRightHeading); } else { - return Optional.of(FieldConstants.redRightHeading); + return Optional.of(ConstantsLoader.FieldConstants.redRightHeading); } case PASS: if (!DriverStation.getAlliance().isEmpty() && DriverStation.getAlliance().get() == Alliance.Blue) { return Optional.of( getTargetHeading( - new Pose2d(FieldConstants.fieldToBluePass, new Rotation2d()), + new Pose2d(ConstantsLoader.FieldConstants.fieldToBluePass, new Rotation2d()), false)); } else { return Optional.of( getTargetHeading( - new Pose2d(FieldConstants.fieldToRedPass, new Rotation2d()), + new Pose2d(ConstantsLoader.FieldConstants.fieldToRedPass, new Rotation2d()), true)); } default: diff --git a/src/main/java/frc/robot/utils/AllianceUtil.java b/src/main/java/frc/robot/utils/AllianceUtil.java index c862caf..8656dc5 100644 --- a/src/main/java/frc/robot/utils/AllianceUtil.java +++ b/src/main/java/frc/robot/utils/AllianceUtil.java @@ -4,7 +4,7 @@ import edu.wpi.first.math.geometry.Rotation2d; import edu.wpi.first.math.geometry.Translation2d; import edu.wpi.first.wpilibj.DriverStation; -import frc.robot.constants.FieldConstants; +import frc.robot.constants.ConstantsLoader.FieldConstants; import org.littletonrobotics.junction.Logger; public class AllianceUtil { @@ -13,93 +13,93 @@ public static Translation2d getFieldToSpeaker() { if (!DriverStation.getAlliance().isEmpty()) { switch (DriverStation.getAlliance().get()) { case Blue: - Logger.recordOutput("Field/speaker", FieldConstants.fieldToBlueSpeaker); - return FieldConstants.fieldToBlueSpeaker; + Logger.recordOutput("Field/speaker", ConstantsLoader.FieldConstants.fieldToBlueSpeaker); + return ConstantsLoader.FieldConstants.fieldToBlueSpeaker; case Red: - Logger.recordOutput("Field/speaker", FieldConstants.fieldToRedSpeaker); - return FieldConstants.fieldToRedSpeaker; + Logger.recordOutput("Field/speaker", ConstantsLoader.FieldConstants.fieldToRedSpeaker); + return ConstantsLoader.FieldConstants.fieldToRedSpeaker; } } - return FieldConstants.fieldToRedSpeaker; + return ConstantsLoader.FieldConstants.fieldToRedSpeaker; } public static Rotation2d getAmpHeading() { - Logger.recordOutput("Field/amp", FieldConstants.ampHeading); - return FieldConstants.ampHeading; + Logger.recordOutput("Field/amp", ConstantsLoader.FieldConstants.ampHeading); + return ConstantsLoader.FieldConstants.ampHeading; } public static Pose2d getPoseAgainstSpeaker() { if (!DriverStation.getAlliance().isEmpty()) { switch (DriverStation.getAlliance().get()) { case Blue: - return FieldConstants.robotAgainstBlueSpeaker; + return ConstantsLoader.FieldConstants.robotAgainstBlueSpeaker; case Red: - return FieldConstants.robotAgainstRedSpeaker; + return ConstantsLoader.FieldConstants.robotAgainstRedSpeaker; } } - return FieldConstants.robotAgainstRedSpeaker; + return ConstantsLoader.FieldConstants.robotAgainstRedSpeaker; } public static Pose2d getPoseAgainstSpeakerLeft() { if (!DriverStation.getAlliance().isEmpty()) { switch (DriverStation.getAlliance().get()) { case Blue: - return FieldConstants.robotAgainstBlueSpeakerLeft; + return ConstantsLoader.FieldConstants.robotAgainstBlueSpeakerLeft; case Red: - return FieldConstants.robotAgainstRedSpeakerLeft; + return ConstantsLoader.FieldConstants.robotAgainstRedSpeakerLeft; } } - return FieldConstants.robotAgainstRedSpeakerLeft; + return ConstantsLoader.FieldConstants.robotAgainstRedSpeakerLeft; } public static Pose2d getPoseAgainstSpeakerRight() { if (!DriverStation.getAlliance().isEmpty()) { switch (DriverStation.getAlliance().get()) { case Blue: - return FieldConstants.robotAgainstBlueSpeakerRight; + return ConstantsLoader.FieldConstants.robotAgainstBlueSpeakerRight; case Red: - return FieldConstants.robotAgainstRedSpeakerRight; + return ConstantsLoader.FieldConstants.robotAgainstRedSpeakerRight; } } - return FieldConstants.robotAgainstRedSpeakerRight; + return ConstantsLoader.FieldConstants.robotAgainstRedSpeakerRight; } public static Pose2d getPoseAgainstPodium() { if (!DriverStation.getAlliance().isEmpty()) { switch (DriverStation.getAlliance().get()) { case Blue: - return FieldConstants.robotAgainstBluePodium; + return ConstantsLoader.FieldConstants.robotAgainstBluePodium; case Red: - return FieldConstants.robotAgainstRedPodium; + return ConstantsLoader.FieldConstants.robotAgainstRedPodium; } } - return FieldConstants.robotAgainstRedPodium; + return ConstantsLoader.FieldConstants.robotAgainstRedPodium; } public static Pose2d getPoseAgainstAmpZone() { if (!DriverStation.getAlliance().isEmpty()) { switch (DriverStation.getAlliance().get()) { case Blue: - return FieldConstants.robotAgainstRedAmpZone; + return ConstantsLoader.FieldConstants.robotAgainstRedAmpZone; case Red: - return FieldConstants.robotAgainstBlueAmpZone; + return ConstantsLoader.FieldConstants.robotAgainstBlueAmpZone; } } - return FieldConstants.robotAgainstRedAmpZone; + return ConstantsLoader.FieldConstants.robotAgainstRedAmpZone; } public static Rotation2d getSourceHeading() { if (!DriverStation.getAlliance().isEmpty()) { switch (DriverStation.getAlliance().get()) { case Blue: - Logger.recordOutput("Field/source", FieldConstants.blueSourceHeading); - return FieldConstants.blueSourceHeading; + Logger.recordOutput("Field/source", ConstantsLoader.FieldConstants.blueSourceHeading); + return ConstantsLoader.FieldConstants.blueSourceHeading; case Red: - Logger.recordOutput("Field/source", FieldConstants.redSourceHeading); - return FieldConstants.redSourceHeading; + Logger.recordOutput("Field/source", ConstantsLoader.FieldConstants.redSourceHeading); + return ConstantsLoader.FieldConstants.redSourceHeading; } } - return FieldConstants.redSourceHeading; + return ConstantsLoader.FieldConstants.redSourceHeading; } /** Returns whether the speaker is significantly to the robot's left */ @@ -107,12 +107,12 @@ public static boolean isLeftOfSpeaker(double robotY, double tolerance) { if (!DriverStation.getAlliance().isEmpty()) { switch (DriverStation.getAlliance().get()) { case Blue: - return robotY > FieldConstants.fieldToBlueSpeaker.getY() + tolerance; + return robotY > ConstantsLoader.FieldConstants.fieldToBlueSpeaker.getY() + tolerance; case Red: - return robotY < FieldConstants.fieldToRedSpeaker.getY() - tolerance; + return robotY < ConstantsLoader.FieldConstants.fieldToRedSpeaker.getY() - tolerance; } } - return robotY < FieldConstants.fieldToRedSpeaker.getY() - tolerance; + return robotY < ConstantsLoader.FieldConstants.fieldToRedSpeaker.getY() - tolerance; } /** Returns whether the speaker is significantly to the robot's right */ @@ -120,11 +120,11 @@ public static boolean isRightOfSpeaker(double robotY, double tolerance) { if (!DriverStation.getAlliance().isEmpty()) { switch (DriverStation.getAlliance().get()) { case Blue: - return robotY < FieldConstants.fieldToBlueSpeaker.getY() - tolerance; + return robotY < ConstantsLoader.FieldConstants.fieldToBlueSpeaker.getY() - tolerance; case Red: - return robotY > FieldConstants.fieldToRedSpeaker.getY() + tolerance; + return robotY > ConstantsLoader.FieldConstants.fieldToRedSpeaker.getY() + tolerance; } } - return robotY > FieldConstants.fieldToRedSpeaker.getY() + tolerance; + return robotY > ConstantsLoader.FieldConstants.fieldToRedSpeaker.getY() + tolerance; } } From d132d0baeca9ad10d83f6d611f8ae6b7875bbb72 Mon Sep 17 00:00:00 2001 From: Jack Lingle Date: Thu, 21 Nov 2024 08:33:42 -0500 Subject: [PATCH 2/5] make field constants regular, add static final back to some classes that wont be switched --- .../frc/robot/constants/ConstantsLoader.java | 11 --- .../frc/robot/constants/FieldConstants.java | 72 +++++++++++++++++++ .../robot/constants/FieldConstantsSchema.java | 72 ------------------- .../frc/robot/constants/IntakeConstants.java | 8 +-- .../frc/robot/constants/VisionConstants.java | 26 +++---- .../robot/subsystems/drive/PhoenixDrive.java | 32 ++++----- .../java/frc/robot/utils/AllianceUtil.java | 68 +++++++++--------- 7 files changed, 139 insertions(+), 150 deletions(-) create mode 100644 src/main/java/frc/robot/constants/FieldConstants.java delete mode 100644 src/main/java/frc/robot/constants/FieldConstantsSchema.java diff --git a/src/main/java/frc/robot/constants/ConstantsLoader.java b/src/main/java/frc/robot/constants/ConstantsLoader.java index 1159605..ce62668 100644 --- a/src/main/java/frc/robot/constants/ConstantsLoader.java +++ b/src/main/java/frc/robot/constants/ConstantsLoader.java @@ -14,7 +14,6 @@ public class ConstantsLoader { */ public static DriverConstantsSchema DriverConstants; public static FeatureFlagsSchema FeatureFlags; - public static FieldConstantsSchema FieldConstants; public static void loadDriverConstants() { JSONSync synced = @@ -35,14 +34,4 @@ public static void loadFeatureFlags() { synced.loadData(); FeatureFlags = synced.getObject(); } - - public static void loadFieldConstants() { - JSONSync synced = - new JSONSync( - new FieldConstantsSchema(), - "./DriverConstants.json", - new JSONSync.JSONSyncConfigBuilder().build()); - synced.loadData(); - FieldConstants = synced.getObject(); - } } diff --git a/src/main/java/frc/robot/constants/FieldConstants.java b/src/main/java/frc/robot/constants/FieldConstants.java new file mode 100644 index 0000000..10a8036 --- /dev/null +++ b/src/main/java/frc/robot/constants/FieldConstants.java @@ -0,0 +1,72 @@ +package frc.robot.constants; + +import edu.wpi.first.math.geometry.Pose2d; +import edu.wpi.first.math.geometry.Rotation2d; +import edu.wpi.first.math.geometry.Translation2d; +import edu.wpi.first.math.util.Units; + +public class FieldConstants { + public static final double lengthM = 16.451; + public static final double widthM = 8.211; + + public static final double midfieldLowThresholdM = 5.87; + public static final double midfieldHighThresholdM = 10.72; + + public static final Rotation2d ampHeading = new Rotation2d(Math.PI / 2); + + public static final Rotation2d blueUpHeading = Rotation2d.fromRadians(0.0); + public static final Rotation2d blueDownHeading = Rotation2d.fromRadians(Math.PI); + public static final Rotation2d blueLeftHeading = Rotation2d.fromRadians(Math.PI / 2.0); + public static final Rotation2d blueRightHeading = Rotation2d.fromRadians(-Math.PI / 2.0); + + public static final Rotation2d redUpHeading = Rotation2d.fromRadians(Math.PI); + public static final Rotation2d redDownHeading = Rotation2d.fromRadians(0.0); + public static final Rotation2d redLeftHeading = Rotation2d.fromRadians(-Math.PI / 2.0); + public static final Rotation2d redRightHeading = Rotation2d.fromRadians(Math.PI / 2.0); + + public static final Rotation2d redSourceHeading = new Rotation2d(Math.PI * 4 / 3); // 60 degrees + public static final Rotation2d blueSourceHeading = + new Rotation2d(Math.PI * 5 / 3); // 120 degrees + + public static final Translation2d fieldToRedSpeaker = + new Translation2d(Units.inchesToMeters(652.73), Units.inchesToMeters(218.42)); + + public static final Translation2d fieldToBlueSpeaker = + new Translation2d(Units.inchesToMeters(-1.5), Units.inchesToMeters(218.42)); + + public static final Translation2d fieldToBluePass = + new Translation2d(Units.inchesToMeters(25), Units.inchesToMeters(230)); + + public static final Translation2d fieldToRedPass = + new Translation2d(Units.inchesToMeters(625), Units.inchesToMeters(230)); + + public static final Pose2d robotAgainstBlueSpeaker = + new Pose2d(1.39, 5.56, Rotation2d.fromDegrees(180)); + + public static final Pose2d robotAgainstRedSpeaker = + new Pose2d(15.19, 5.56, Rotation2d.fromDegrees(0)); + + public static final Pose2d robotAgainstBlueSpeakerRight = + new Pose2d(0.7, 4.38, Rotation2d.fromDegrees(120)); + + public static final Pose2d robotAgainstRedSpeakerRight = + new Pose2d(15.83, 6.73, Rotation2d.fromDegrees(-60)); + + public static final Pose2d robotAgainstBlueSpeakerLeft = + new Pose2d(0.7, 6.73, Rotation2d.fromDegrees(-120)); + + public static final Pose2d robotAgainstRedSpeakerLeft = + new Pose2d(15.83, 4.38, Rotation2d.fromDegrees(60)); + + public static final Pose2d robotAgainstBluePodium = + new Pose2d(2.57, 4.09, Rotation2d.fromDegrees(180)); + + public static final Pose2d robotAgainstRedPodium = + new Pose2d(13.93, 4.09, Rotation2d.fromDegrees(0)); + + public static final Pose2d robotAgainstBlueAmpZone = + new Pose2d(2.85, 7.68, Rotation2d.fromDegrees(-90)); + + public static final Pose2d robotAgainstRedAmpZone = + new Pose2d(13.74, 7.68, Rotation2d.fromDegrees(-90)); +} diff --git a/src/main/java/frc/robot/constants/FieldConstantsSchema.java b/src/main/java/frc/robot/constants/FieldConstantsSchema.java deleted file mode 100644 index 1956a39..0000000 --- a/src/main/java/frc/robot/constants/FieldConstantsSchema.java +++ /dev/null @@ -1,72 +0,0 @@ -package frc.robot.constants; - -import edu.wpi.first.math.geometry.Pose2d; -import edu.wpi.first.math.geometry.Rotation2d; -import edu.wpi.first.math.geometry.Translation2d; -import edu.wpi.first.math.util.Units; - -public class FieldConstantsSchema { - public double lengthM = 16.451; - public double widthM = 8.211; - - public double midfieldLowThresholdM = 5.87; - public double midfieldHighThresholdM = 10.72; - - public Rotation2d ampHeading = new Rotation2d(Math.PI / 2); - - public Rotation2d blueUpHeading = Rotation2d.fromRadians(0.0); - public Rotation2d blueDownHeading = Rotation2d.fromRadians(Math.PI); - public Rotation2d blueLeftHeading = Rotation2d.fromRadians(Math.PI / 2.0); - public Rotation2d blueRightHeading = Rotation2d.fromRadians(-Math.PI / 2.0); - - public Rotation2d redUpHeading = Rotation2d.fromRadians(Math.PI); - public Rotation2d redDownHeading = Rotation2d.fromRadians(0.0); - public Rotation2d redLeftHeading = Rotation2d.fromRadians(-Math.PI / 2.0); - public Rotation2d redRightHeading = Rotation2d.fromRadians(Math.PI / 2.0); - - public Rotation2d redSourceHeading = new Rotation2d(Math.PI * 4 / 3); // 60 degrees - public Rotation2d blueSourceHeading = - new Rotation2d(Math.PI * 5 / 3); // 120 degrees - - public Translation2d fieldToRedSpeaker = - new Translation2d(Units.inchesToMeters(652.73), Units.inchesToMeters(218.42)); - - public Translation2d fieldToBlueSpeaker = - new Translation2d(Units.inchesToMeters(-1.5), Units.inchesToMeters(218.42)); - - public Translation2d fieldToBluePass = - new Translation2d(Units.inchesToMeters(25), Units.inchesToMeters(230)); - - public Translation2d fieldToRedPass = - new Translation2d(Units.inchesToMeters(625), Units.inchesToMeters(230)); - - public Pose2d robotAgainstBlueSpeaker = - new Pose2d(1.39, 5.56, Rotation2d.fromDegrees(180)); - - public Pose2d robotAgainstRedSpeaker = - new Pose2d(15.19, 5.56, Rotation2d.fromDegrees(0)); - - public Pose2d robotAgainstBlueSpeakerRight = - new Pose2d(0.7, 4.38, Rotation2d.fromDegrees(120)); - - public Pose2d robotAgainstRedSpeakerRight = - new Pose2d(15.83, 6.73, Rotation2d.fromDegrees(-60)); - - public Pose2d robotAgainstBlueSpeakerLeft = - new Pose2d(0.7, 6.73, Rotation2d.fromDegrees(-120)); - - public Pose2d robotAgainstRedSpeakerLeft = - new Pose2d(15.83, 4.38, Rotation2d.fromDegrees(60)); - - public Pose2d robotAgainstBluePodium = - new Pose2d(2.57, 4.09, Rotation2d.fromDegrees(180)); - - public Pose2d robotAgainstRedPodium = - new Pose2d(13.93, 4.09, Rotation2d.fromDegrees(0)); - - public Pose2d robotAgainstBlueAmpZone = - new Pose2d(2.85, 7.68, Rotation2d.fromDegrees(-90)); - - public Pose2d robotAgainstRedAmpZone = - new Pose2d(13.74, 7.68, Rotation2d.fromDegrees(-90)); -} diff --git a/src/main/java/frc/robot/constants/IntakeConstants.java b/src/main/java/frc/robot/constants/IntakeConstants.java index 31ac5e7..f2f7d79 100644 --- a/src/main/java/frc/robot/constants/IntakeConstants.java +++ b/src/main/java/frc/robot/constants/IntakeConstants.java @@ -1,9 +1,9 @@ package frc.robot.constants; public class IntakeConstants { - public int intakeMotorID = 6; - public int centeringMotorID = 4; + public static final int intakeMotorID = 6; + public static final int centeringMotorID = 4; - public double intakePower = 12.0; - public double beltPower = 12.0; + public static final double intakePower = 12.0; + public static final double beltPower = 12.0; } diff --git a/src/main/java/frc/robot/constants/VisionConstants.java b/src/main/java/frc/robot/constants/VisionConstants.java index 0761a5f..3715035 100644 --- a/src/main/java/frc/robot/constants/VisionConstants.java +++ b/src/main/java/frc/robot/constants/VisionConstants.java @@ -17,26 +17,26 @@ import java.util.List; public class VisionConstants { - public String tagLayoutName = "Pairs-Only"; - public AprilTagFieldLayout fieldLayout = initLayout(tagLayoutName); + public static final String tagLayoutName = "Pairs-Only"; + public static final AprilTagFieldLayout fieldLayout = initLayout(tagLayoutName); - public double lowUncertaintyCutoffDistance = 6.5; + public static final double lowUncertaintyCutoffDistance = 6.5; - public double skewCutoffDistance = 5.8; - public double skewCutoffRotation = Units.degreesToRadians(50); + public static final double skewCutoffDistance = 5.8; + public static final double skewCutoffRotation = Units.degreesToRadians(50); // Maximum average tag distance before a measurement is fully ignored - public double maxTagDistance = 8.0; + public static final double maxTagDistance = 8.0; - public Matrix teleopCameraUncertainty = VecBuilder.fill(0.35, 0.35, 3.5); + public static final Matrix teleopCameraUncertainty = VecBuilder.fill(0.35, 0.35, 3.5); - public Matrix lowCameraUncertainty = VecBuilder.fill(0.6, 1.0, 4); + public static final Matrix lowCameraUncertainty = VecBuilder.fill(0.6, 1.0, 4); - public Matrix highCameraUncertainty = VecBuilder.fill(12.0, 16.0, 40); + public static final Matrix highCameraUncertainty = VecBuilder.fill(12.0, 16.0, 40); - public Matrix driveUncertainty = VecBuilder.fill(0.1, 0.1, 0.1); + public static final Matrix driveUncertainty = VecBuilder.fill(0.1, 0.1, 0.1); - public List cameras = List.of(); + public static final List cameras = List.of(); // new CameraParams( // "Front-Left", // Front Right @@ -92,7 +92,7 @@ public class VisionConstants { // new Rotation3d(0.0, -0.349, 0.524)), // CameraTrustZone.MIDDLE)); - public record CameraParams( + public static final record CameraParams( String name, int xResolution, int yResolution, @@ -101,7 +101,7 @@ public record CameraParams( Transform3d robotToCamera, CameraTrustZone zone) {} - private AprilTagFieldLayout initLayout(String name) { + private static final AprilTagFieldLayout initLayout(String name) { AprilTagFieldLayout layout; // AprilTagFieldLayout's constructor throws an IOException, so we must catch it // in order to initialize our layout as a constant diff --git a/src/main/java/frc/robot/subsystems/drive/PhoenixDrive.java b/src/main/java/frc/robot/subsystems/drive/PhoenixDrive.java index 62ec5c0..f68af5b 100644 --- a/src/main/java/frc/robot/subsystems/drive/PhoenixDrive.java +++ b/src/main/java/frc/robot/subsystems/drive/PhoenixDrive.java @@ -31,7 +31,7 @@ import edu.wpi.first.wpilibj2.command.Commands; import edu.wpi.first.wpilibj2.command.Subsystem; import edu.wpi.first.wpilibj2.command.sysid.SysIdRoutine; -import frc.robot.constants.ConstantsLoader.FieldConstants; +import frc.robot.constants.FieldConstants; import frc.robot.constants.ConstantsLoader; import frc.robot.constants.PhoenixDriveConstants; import frc.robot.constants.PhoenixDriveConstants.AlignTarget; @@ -338,57 +338,57 @@ public Optional getAlignment() { && DriverStation.getAlliance().get() == Alliance.Blue) { return Optional.of( getTargetHeading( - new Pose2d(ConstantsLoader.FieldConstants.fieldToBlueSpeaker, new Rotation2d()), + new Pose2d(FieldConstants.fieldToBlueSpeaker, new Rotation2d()), false)); } else { return Optional.of( getTargetHeading( - new Pose2d(ConstantsLoader.FieldConstants.fieldToRedSpeaker, new Rotation2d()), + new Pose2d(FieldConstants.fieldToRedSpeaker, new Rotation2d()), true)); } case AMP: - return Optional.of(ConstantsLoader.FieldConstants.ampHeading); + return Optional.of(FieldConstants.ampHeading); case SOURCE: if (DriverStation.getAlliance().get() == Alliance.Blue) { - return Optional.of(ConstantsLoader.FieldConstants.blueSourceHeading); + return Optional.of(FieldConstants.blueSourceHeading); } else { - return Optional.of(ConstantsLoader.FieldConstants.redSourceHeading); + return Optional.of(FieldConstants.redSourceHeading); } case UP: if (DriverStation.getAlliance().get() == Alliance.Blue) { - return Optional.of(ConstantsLoader.FieldConstants.blueUpHeading); + return Optional.of(FieldConstants.blueUpHeading); } else { - return Optional.of(ConstantsLoader.FieldConstants.redUpHeading); + return Optional.of(FieldConstants.redUpHeading); } case DOWN: if (DriverStation.getAlliance().get() == Alliance.Blue) { - return Optional.of(ConstantsLoader.FieldConstants.blueDownHeading); + return Optional.of(FieldConstants.blueDownHeading); } else { - return Optional.of(ConstantsLoader.FieldConstants.redDownHeading); + return Optional.of(FieldConstants.redDownHeading); } case LEFT: if (DriverStation.getAlliance().get() == Alliance.Blue) { - return Optional.of(ConstantsLoader.FieldConstants.blueLeftHeading); + return Optional.of(FieldConstants.blueLeftHeading); } else { - return Optional.of(ConstantsLoader.FieldConstants.redLeftHeading); + return Optional.of(FieldConstants.redLeftHeading); } case RIGHT: if (DriverStation.getAlliance().get() == Alliance.Blue) { - return Optional.of(ConstantsLoader.FieldConstants.blueRightHeading); + return Optional.of(FieldConstants.blueRightHeading); } else { - return Optional.of(ConstantsLoader.FieldConstants.redRightHeading); + return Optional.of(FieldConstants.redRightHeading); } case PASS: if (!DriverStation.getAlliance().isEmpty() && DriverStation.getAlliance().get() == Alliance.Blue) { return Optional.of( getTargetHeading( - new Pose2d(ConstantsLoader.FieldConstants.fieldToBluePass, new Rotation2d()), + new Pose2d(FieldConstants.fieldToBluePass, new Rotation2d()), false)); } else { return Optional.of( getTargetHeading( - new Pose2d(ConstantsLoader.FieldConstants.fieldToRedPass, new Rotation2d()), + new Pose2d(FieldConstants.fieldToRedPass, new Rotation2d()), true)); } default: diff --git a/src/main/java/frc/robot/utils/AllianceUtil.java b/src/main/java/frc/robot/utils/AllianceUtil.java index 8656dc5..c862caf 100644 --- a/src/main/java/frc/robot/utils/AllianceUtil.java +++ b/src/main/java/frc/robot/utils/AllianceUtil.java @@ -4,7 +4,7 @@ import edu.wpi.first.math.geometry.Rotation2d; import edu.wpi.first.math.geometry.Translation2d; import edu.wpi.first.wpilibj.DriverStation; -import frc.robot.constants.ConstantsLoader.FieldConstants; +import frc.robot.constants.FieldConstants; import org.littletonrobotics.junction.Logger; public class AllianceUtil { @@ -13,93 +13,93 @@ public static Translation2d getFieldToSpeaker() { if (!DriverStation.getAlliance().isEmpty()) { switch (DriverStation.getAlliance().get()) { case Blue: - Logger.recordOutput("Field/speaker", ConstantsLoader.FieldConstants.fieldToBlueSpeaker); - return ConstantsLoader.FieldConstants.fieldToBlueSpeaker; + Logger.recordOutput("Field/speaker", FieldConstants.fieldToBlueSpeaker); + return FieldConstants.fieldToBlueSpeaker; case Red: - Logger.recordOutput("Field/speaker", ConstantsLoader.FieldConstants.fieldToRedSpeaker); - return ConstantsLoader.FieldConstants.fieldToRedSpeaker; + Logger.recordOutput("Field/speaker", FieldConstants.fieldToRedSpeaker); + return FieldConstants.fieldToRedSpeaker; } } - return ConstantsLoader.FieldConstants.fieldToRedSpeaker; + return FieldConstants.fieldToRedSpeaker; } public static Rotation2d getAmpHeading() { - Logger.recordOutput("Field/amp", ConstantsLoader.FieldConstants.ampHeading); - return ConstantsLoader.FieldConstants.ampHeading; + Logger.recordOutput("Field/amp", FieldConstants.ampHeading); + return FieldConstants.ampHeading; } public static Pose2d getPoseAgainstSpeaker() { if (!DriverStation.getAlliance().isEmpty()) { switch (DriverStation.getAlliance().get()) { case Blue: - return ConstantsLoader.FieldConstants.robotAgainstBlueSpeaker; + return FieldConstants.robotAgainstBlueSpeaker; case Red: - return ConstantsLoader.FieldConstants.robotAgainstRedSpeaker; + return FieldConstants.robotAgainstRedSpeaker; } } - return ConstantsLoader.FieldConstants.robotAgainstRedSpeaker; + return FieldConstants.robotAgainstRedSpeaker; } public static Pose2d getPoseAgainstSpeakerLeft() { if (!DriverStation.getAlliance().isEmpty()) { switch (DriverStation.getAlliance().get()) { case Blue: - return ConstantsLoader.FieldConstants.robotAgainstBlueSpeakerLeft; + return FieldConstants.robotAgainstBlueSpeakerLeft; case Red: - return ConstantsLoader.FieldConstants.robotAgainstRedSpeakerLeft; + return FieldConstants.robotAgainstRedSpeakerLeft; } } - return ConstantsLoader.FieldConstants.robotAgainstRedSpeakerLeft; + return FieldConstants.robotAgainstRedSpeakerLeft; } public static Pose2d getPoseAgainstSpeakerRight() { if (!DriverStation.getAlliance().isEmpty()) { switch (DriverStation.getAlliance().get()) { case Blue: - return ConstantsLoader.FieldConstants.robotAgainstBlueSpeakerRight; + return FieldConstants.robotAgainstBlueSpeakerRight; case Red: - return ConstantsLoader.FieldConstants.robotAgainstRedSpeakerRight; + return FieldConstants.robotAgainstRedSpeakerRight; } } - return ConstantsLoader.FieldConstants.robotAgainstRedSpeakerRight; + return FieldConstants.robotAgainstRedSpeakerRight; } public static Pose2d getPoseAgainstPodium() { if (!DriverStation.getAlliance().isEmpty()) { switch (DriverStation.getAlliance().get()) { case Blue: - return ConstantsLoader.FieldConstants.robotAgainstBluePodium; + return FieldConstants.robotAgainstBluePodium; case Red: - return ConstantsLoader.FieldConstants.robotAgainstRedPodium; + return FieldConstants.robotAgainstRedPodium; } } - return ConstantsLoader.FieldConstants.robotAgainstRedPodium; + return FieldConstants.robotAgainstRedPodium; } public static Pose2d getPoseAgainstAmpZone() { if (!DriverStation.getAlliance().isEmpty()) { switch (DriverStation.getAlliance().get()) { case Blue: - return ConstantsLoader.FieldConstants.robotAgainstRedAmpZone; + return FieldConstants.robotAgainstRedAmpZone; case Red: - return ConstantsLoader.FieldConstants.robotAgainstBlueAmpZone; + return FieldConstants.robotAgainstBlueAmpZone; } } - return ConstantsLoader.FieldConstants.robotAgainstRedAmpZone; + return FieldConstants.robotAgainstRedAmpZone; } public static Rotation2d getSourceHeading() { if (!DriverStation.getAlliance().isEmpty()) { switch (DriverStation.getAlliance().get()) { case Blue: - Logger.recordOutput("Field/source", ConstantsLoader.FieldConstants.blueSourceHeading); - return ConstantsLoader.FieldConstants.blueSourceHeading; + Logger.recordOutput("Field/source", FieldConstants.blueSourceHeading); + return FieldConstants.blueSourceHeading; case Red: - Logger.recordOutput("Field/source", ConstantsLoader.FieldConstants.redSourceHeading); - return ConstantsLoader.FieldConstants.redSourceHeading; + Logger.recordOutput("Field/source", FieldConstants.redSourceHeading); + return FieldConstants.redSourceHeading; } } - return ConstantsLoader.FieldConstants.redSourceHeading; + return FieldConstants.redSourceHeading; } /** Returns whether the speaker is significantly to the robot's left */ @@ -107,12 +107,12 @@ public static boolean isLeftOfSpeaker(double robotY, double tolerance) { if (!DriverStation.getAlliance().isEmpty()) { switch (DriverStation.getAlliance().get()) { case Blue: - return robotY > ConstantsLoader.FieldConstants.fieldToBlueSpeaker.getY() + tolerance; + return robotY > FieldConstants.fieldToBlueSpeaker.getY() + tolerance; case Red: - return robotY < ConstantsLoader.FieldConstants.fieldToRedSpeaker.getY() - tolerance; + return robotY < FieldConstants.fieldToRedSpeaker.getY() - tolerance; } } - return robotY < ConstantsLoader.FieldConstants.fieldToRedSpeaker.getY() - tolerance; + return robotY < FieldConstants.fieldToRedSpeaker.getY() - tolerance; } /** Returns whether the speaker is significantly to the robot's right */ @@ -120,11 +120,11 @@ public static boolean isRightOfSpeaker(double robotY, double tolerance) { if (!DriverStation.getAlliance().isEmpty()) { switch (DriverStation.getAlliance().get()) { case Blue: - return robotY < ConstantsLoader.FieldConstants.fieldToBlueSpeaker.getY() - tolerance; + return robotY < FieldConstants.fieldToBlueSpeaker.getY() - tolerance; case Red: - return robotY > ConstantsLoader.FieldConstants.fieldToRedSpeaker.getY() + tolerance; + return robotY > FieldConstants.fieldToRedSpeaker.getY() + tolerance; } } - return robotY > ConstantsLoader.FieldConstants.fieldToRedSpeaker.getY() + tolerance; + return robotY > FieldConstants.fieldToRedSpeaker.getY() + tolerance; } } From 4a1a89ca269db706c4eb9388dc543da4b20633af Mon Sep 17 00:00:00 2001 From: Jack Lingle Date: Thu, 21 Nov 2024 09:13:03 -0500 Subject: [PATCH 3/5] add scoring schema and add map loading to the ConstantsLoader class --- src/main/java/frc/robot/RobotContainer.java | 43 +++--- .../frc/robot/constants/ConstantsLoader.java | 33 +++++ .../frc/robot/constants/ScoringConstants.java | 132 ----------------- .../constants/ScoringConstantsSchema.java | 133 ++++++++++++++++++ .../subsystems/scoring/AimerIORoboRio.java | 50 +++---- .../robot/subsystems/scoring/AimerIOSim.java | 24 ++-- .../subsystems/scoring/ScoringSubsystem.java | 62 ++++---- .../subsystems/scoring/ShooterIOSim.java | 22 +-- .../subsystems/scoring/ShooterIOTalonFX.java | 27 ++-- 9 files changed, 280 insertions(+), 246 deletions(-) delete mode 100644 src/main/java/frc/robot/constants/ScoringConstants.java create mode 100644 src/main/java/frc/robot/constants/ScoringConstantsSchema.java diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index be9e3a4..588ff76 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -19,7 +19,6 @@ import frc.robot.constants.ModeConstants; import frc.robot.constants.PhoenixDriveConstants; import frc.robot.constants.PhoenixDriveConstants.AlignTarget; -import frc.robot.constants.ScoringConstants; import frc.robot.constants.VisionConstants; import frc.robot.subsystems.LED; import frc.robot.subsystems.OrchestraSubsystem; @@ -500,9 +499,9 @@ public void testInit() { case "tuning": break; case "tuning-shooter": - SmartDashboard.putNumber("Test-Mode/shooter/kP", ScoringConstants.shooterkP); - SmartDashboard.putNumber("Test-Mode/shooter/kI", ScoringConstants.shooterkI); - SmartDashboard.putNumber("Test-Mode/shooter/kD", ScoringConstants.shooterkD); + SmartDashboard.putNumber("Test-Mode/shooter/kP", ConstantsLoader.ScoringConstants.shooterkP); + SmartDashboard.putNumber("Test-Mode/shooter/kI", ConstantsLoader.ScoringConstants.shooterkI); + SmartDashboard.putNumber("Test-Mode/shooter/kD", ConstantsLoader.ScoringConstants.shooterkD); SmartDashboard.putNumber("Test-Mode/shooter/setpointPosition", 0.25); SmartDashboard.putNumber("Test-Mode/shooter/volts", 2.0); @@ -521,13 +520,13 @@ public void testInit() { scoringSubsystem.setPID( SmartDashboard.getNumber( "Test-Mode/shooter/kP", - ScoringConstants.shooterkP), + ConstantsLoader.ScoringConstants.shooterkP), SmartDashboard.getNumber( "Test-Mode/shooter/kI", - ScoringConstants.shooterkI), + ConstantsLoader.ScoringConstants.shooterkI), SmartDashboard.getNumber( "Test-Mode/shooter/kD", - ScoringConstants.shooterkD), + ConstantsLoader.ScoringConstants.shooterkD), 1))) .onTrue( new InstantCommand( @@ -585,18 +584,18 @@ public void testInit() { new InstantCommand(() -> scoringSubsystem.setTuningKickerVolts(0))); break; case "tuning-aimer": - SmartDashboard.putNumber("Test-Mode/aimer/kP", ScoringConstants.aimerkP); - SmartDashboard.putNumber("Test-Mode/aimer/kI", ScoringConstants.aimerkI); - SmartDashboard.putNumber("Test-Mode/aimer/kD", ScoringConstants.aimerkD); + SmartDashboard.putNumber("Test-Mode/aimer/kP", ConstantsLoader.ScoringConstants.aimerkP); + SmartDashboard.putNumber("Test-Mode/aimer/kI", ConstantsLoader.ScoringConstants.aimerkI); + SmartDashboard.putNumber("Test-Mode/aimer/kD", ConstantsLoader.ScoringConstants.aimerkD); - SmartDashboard.putNumber("Test-Mode/aimer/kG", ScoringConstants.aimerkG); - SmartDashboard.putNumber("Test-Mode/aimer/kS", ScoringConstants.aimerkS); + SmartDashboard.putNumber("Test-Mode/aimer/kG", ConstantsLoader.ScoringConstants.aimerkG); + SmartDashboard.putNumber("Test-Mode/aimer/kS", ConstantsLoader.ScoringConstants.aimerkS); SmartDashboard.putNumber( - "Test-Mode/aimer/profileMaxVelocity", ScoringConstants.aimerCruiseVelocity); + "Test-Mode/aimer/profileMaxVelocity", ConstantsLoader.ScoringConstants.aimerCruiseVelocity); SmartDashboard.putNumber( "Test-Mode/aimer/profileMaxAcceleration", - ScoringConstants.aimerAcceleration); + ConstantsLoader.ScoringConstants.aimerAcceleration); SmartDashboard.putNumber("Test-Mode/aimer/setpointPosition", 0.25); SmartDashboard.putNumber("Test-Mode/aimer/volts", 2.0); @@ -614,13 +613,13 @@ public void testInit() { scoringSubsystem.setPID( SmartDashboard.getNumber( "Test-Mode/aimer/kP", - ScoringConstants.aimerkP), + ConstantsLoader.ScoringConstants.aimerkP), SmartDashboard.getNumber( "Test-Mode/aimer/kI", - ScoringConstants.aimerkI), + ConstantsLoader.ScoringConstants.aimerkI), SmartDashboard.getNumber( "Test-Mode/aimer/kD", - ScoringConstants.aimerkD), + ConstantsLoader.ScoringConstants.aimerkD), 0))) .onTrue( new InstantCommand( @@ -628,11 +627,11 @@ public void testInit() { scoringSubsystem.setMaxProfileProperties( SmartDashboard.getNumber( "Test-Mode/aimer/profileMaxVelocity", - ScoringConstants + ConstantsLoader.ScoringConstants .aimerCruiseVelocity), SmartDashboard.getNumber( "Test-Mode/aimer/profileMaxAcceleration", - ScoringConstants.aimerAcceleration), + ConstantsLoader.ScoringConstants.aimerAcceleration), 0))) .onTrue( new InstantCommand( @@ -640,12 +639,12 @@ public void testInit() { scoringSubsystem.setFF( SmartDashboard.getNumber( "Test-Mode/aimer/kS", - ScoringConstants.aimerkS), + ConstantsLoader.ScoringConstants.aimerkS), 0.0, 0.0, SmartDashboard.getNumber( "Test-Mode/aimer/kG", - ScoringConstants.aimerkG), + ConstantsLoader.ScoringConstants.aimerkG), 0))) .onTrue( new InstantCommand( @@ -709,7 +708,7 @@ public void testInit() { case "tuning-amp": SmartDashboard.putNumber( "Test-Mode/amp/aimerSetpointPosition", - ScoringConstants.ampAimerAngleRotations); + ConstantsLoader.ScoringConstants.ampAimerAngleRotations); // Let us drive CommandScheduler.getInstance().cancelAll(); diff --git a/src/main/java/frc/robot/constants/ConstantsLoader.java b/src/main/java/frc/robot/constants/ConstantsLoader.java index ce62668..2b2fdc8 100644 --- a/src/main/java/frc/robot/constants/ConstantsLoader.java +++ b/src/main/java/frc/robot/constants/ConstantsLoader.java @@ -1,5 +1,7 @@ package frc.robot.constants; +import java.util.HashMap; + import coppercore.parameter_tools.JSONSync; public class ConstantsLoader { @@ -14,6 +16,7 @@ public class ConstantsLoader { */ public static DriverConstantsSchema DriverConstants; public static FeatureFlagsSchema FeatureFlags; + public static ScoringConstantsSchema ScoringConstants; public static void loadDriverConstants() { JSONSync synced = @@ -34,4 +37,34 @@ public static void loadFeatureFlags() { synced.loadData(); FeatureFlags = synced.getObject(); } + + public static void loadScoringConstants() { + JSONSync synced = + new JSONSync( + new ScoringConstantsSchema(), + "./DriverConstants.json", + new JSONSync.JSONSyncConfigBuilder().build()); + synced.loadData(); + ScoringConstants = synced.getObject(); + ScoringConstants.aimerMap = new HashMap(); + ScoringConstants.shooterMap = new HashMap(); + ScoringConstants.timeToGoalMap = new HashMap(); + ScoringConstants.aimerToleranceMap = new HashMap(); + + for (int i = 0; i < ScoringConstants.aimerDistance.length; i++) { + ScoringConstants.aimerMap.put(ScoringConstants.aimerDistance[i], ScoringConstants.aimerPosition[i]); + } + + for (int i = 0; i < ScoringConstants.shooterDistance.length; i++) { + ScoringConstants.shooterMap.put(ScoringConstants.shooterDistance[i], ScoringConstants.shooterRPM[i]); + } + + for (int i = 0; i < ScoringConstants.timeToGoalDistance.length; i++) { + ScoringConstants.timeToGoalMap.put(ScoringConstants.timeToGoalDistance[i], ScoringConstants.timeToGoal[i]); + } + + for (int i = 0; i < ScoringConstants.aimerToleranceDistance.length; i++) { + ScoringConstants.aimerToleranceMap.put(ScoringConstants.aimerToleranceDistance[i], ScoringConstants.aimerTolerance[i]); + } + } } diff --git a/src/main/java/frc/robot/constants/ScoringConstants.java b/src/main/java/frc/robot/constants/ScoringConstants.java deleted file mode 100644 index fc7e86b..0000000 --- a/src/main/java/frc/robot/constants/ScoringConstants.java +++ /dev/null @@ -1,132 +0,0 @@ -package frc.robot.constants; - -import edu.wpi.first.math.util.Units; -import java.util.HashMap; - -public class ScoringConstants { - public final double aimerkP = 100.0; - public final double aimerkI = 2.0; // 5.0 - public final double aimerkD = 0.0; - - public final double aimerkS = 0.3; // 0.25; - public final double aimerkG = 0.955; // 0.2; - public final double aimerkV = 0.0; - public final double aimerkA = 0.0; - - public final double shooterkP = 0.05; - public final double shooterkI = 1; - public final double shooterkD = 0.0; - - public final double shooterkS = 0.0; // TODO: Find Imperically - public final double shooterkV = 0.005; - public final double shooterkA = 0.0; - - public final int aimerMotorId = 9; - - public final int kickerMotorId = 5; // 1 - - // TODO: REPLACE THIS WHEN THE ACTUAL SHOOTER IO IS MERGED - public final int shooterLeftMotorId = 10; - public final int shooterRightMotorId = 11; - - public final double shooterCurrentLimit = 120; - public final double kickerCurrentLimit = 120; - public final double aimerCurrentLimit = 60; - - public final int aimerEncoderId = 13; - - public final double aimerEncoderOffset = -0.087402; // Armencoder is zeroed - - public final double aimerEncoderToMechanismRatio = 1.0; - public final double aimerRotorToSensorRatio = 90.0; - - public final double kickerIntakeVolts = 2.0; - - // public final double aimPositionTolerance = 0.003; - - public final double aimerAcceleration = 3.5; - public final double aimerCruiseVelocity = 0.8; - - public final double shooterLowerVelocityMarginRPM = 50; - public final double shooterUpperVelocityMarginRPM = 150; - public final double aimAngleMarginRotations = Units.degreesToRotations(1); - public final double aimAngleVelocityMargin = 2.0; - - public final double minDistanceAlignmentNeeded = 1.3; // TODO: Tune this value - - public final double intakeAngleToleranceRotations = 0.05; // Todo: tune this value - - public final double aimerAmpPositionRot = 0.145; - - public final double aimMaxAngleRotations = 0.361328 - 0.184570; - public final double aimMinAngleRotations = -0.212285; - public final double aimLockVoltage = -0.5; - - public final double aimAngleTolerance = 0.015; - - public final double ampAimerAngleRotations = 0.14; - - public final double maxAimIntake = 0.0; - public final double minAimIntake = 0.0; - - public final double shooterOffsetAdjustment = 0.6; - - public final double maxElevatorPosition = 0.45; - public final double maxAimAngleElevatorLimit = Math.PI / 2; - - // NOTE - This should be monotonically increasing - // Key - Distance in meters - // Value - Aimer angle in rotations - public HashMap getAimerMap() { - HashMap map = new HashMap(); - // map.put(1.284, -0.1); - map.put(1.428, -0.1); - // map.put(1.7, -0.13); - // map.put(2.30, -0.15); - // map.put(3.32, -0.175); - // map.put(4.23, -0.195); - // map.put(5.811, -0.2); - - return map; - } - - public final double passLocationRot = -0.14; - public final double passAngleToleranceRot = 0.005; - public final double passShooterRpm = 2800.0; - - public final double aimerOffset = 0.0; - - // NOTE - This should be monotonically increasing - // Key - Distance in meters - // Value - Shooter RPM - public HashMap getShooterMap() { - HashMap map = new HashMap(); - map.put(1.284, 3500.0); - map.put(1.7, 4000.0); - - return map; - } - - // NOTE - This should be monotonically increasing - // Key - Distance in meters - // Value - Time in seconds - public HashMap timeToGoalMap() { - HashMap map = new HashMap(); - map.put(0.0, 0.15); - map.put(1.3, 0.15); - map.put(1.4, 0.16); - // map.put(1.3, 0.15); - - return map; - } - - // NOTE - This should be monotonically increasing - // Key - Distance to goal in meters - // Value - Aimer angle tolerance in rotations - public HashMap aimerToleranceTable() { - HashMap map = new HashMap(); - map.put(0.0, 0.003); // Todo: tune this value after conversion from radians to rotations - - return map; - } -} diff --git a/src/main/java/frc/robot/constants/ScoringConstantsSchema.java b/src/main/java/frc/robot/constants/ScoringConstantsSchema.java new file mode 100644 index 0000000..47004a4 --- /dev/null +++ b/src/main/java/frc/robot/constants/ScoringConstantsSchema.java @@ -0,0 +1,133 @@ +package frc.robot.constants; + +import edu.wpi.first.math.util.Units; +import java.util.HashMap; + +public class ScoringConstantsSchema { + public double aimerkP = 100.0; + public double aimerkI = 2.0; // 5.0 + public double aimerkD = 0.0; + + public double aimerkS = 0.3; // 0.25; + public double aimerkG = 0.955; // 0.2; + public double aimerkV = 0.0; + public double aimerkA = 0.0; + + public double shooterkP = 0.05; + public double shooterkI = 1; + public double shooterkD = 0.0; + + public double shooterkS = 0.0; // TODO: Find Imperically + public double shooterkV = 0.005; + public double shooterkA = 0.0; + + public int aimerMotorId = 9; + + public int kickerMotorId = 5; // 1 + + // TODO: REPLACE THIS WHEN THE ACTUAL SHOOTER IO IS MERGED + public int shooterLeftMotorId = 10; + public int shooterRightMotorId = 11; + + public double shooterCurrentLimit = 120; + public double kickerCurrentLimit = 120; + public double aimerCurrentLimit = 60; + + public int aimerEncoderId = 13; + + public double aimerEncoderOffset = -0.087402; // Armencoder is zeroed + + public double aimerEncoderToMechanismRatio = 1.0; + public double aimerRotorToSensorRatio = 90.0; + + public double kickerIntakeVolts = 2.0; + + // public double aimPositionTolerance = 0.003; + + public double aimerAcceleration = 3.5; + public double aimerCruiseVelocity = 0.8; + + public double shooterLowerVelocityMarginRPM = 50; + public double shooterUpperVelocityMarginRPM = 150; + public double aimAngleMarginRotations = Units.degreesToRotations(1); + public double aimAngleVelocityMargin = 2.0; + + public double minDistanceAlignmentNeeded = 1.3; // TODO: Tune this value + + public double intakeAngleToleranceRotations = 0.05; // Todo: tune this value + + public double aimerAmpPositionRot = 0.145; + + public double aimMaxAngleRotations = 0.361328 - 0.184570; + public double aimMinAngleRotations = -0.212285; + public double aimLockVoltage = -0.5; + + public double aimAngleTolerance = 0.015; + + public double ampAimerAngleRotations = 0.14; + + public double maxAimIntake = 0.0; + public double minAimIntake = 0.0; + + public double shooterOffsetAdjustment = 0.6; + + public double maxElevatorPosition = 0.45; + public double maxAimAngleElevatorLimit = Math.PI / 2; + + public double[] aimerDistance; + public double[] aimerPosition; + + public double passLocationRot = -0.14; + public double passAngleToleranceRot = 0.005; + public double passShooterRpm = 2800.0; + + public double aimerOffset = 0.0; + + public double[] shooterDistance; + public double[] shooterRPM; + + // NOTE - This should be monotonically increasing + // Key - Distance in meters + // Value - Shooter RPM + public HashMap getShooterMap() { + HashMap map = new HashMap(); + map.put(1.284, 3500.0); + map.put(1.7, 4000.0); + + return map; + } + + public double[] timeToGoalDistance; + public double[] timeToGoal; + + // NOTE - This should be monotonically increasing + // Key - Distance in meters + // Value - Time in seconds + public HashMap timeToGoalMap() { + HashMap map = new HashMap(); + map.put(0.0, 0.15); + map.put(1.3, 0.15); + map.put(1.4, 0.16); + // map.put(1.3, 0.15); + + return map; + } + + public double[] aimerToleranceDistance; + public double[] aimerTolerance; + + // NOTE - This should be monotonically increasing + // Key - Distance to goal in meters + // Value - Aimer angle tolerance in rotations + public HashMap aimerToleranceTable() { + HashMap map = new HashMap(); + map.put(0.0, 0.003); // Todo: tune this value after conversion from radians to rotations + + return map; + } + + public HashMap aimerMap; + public HashMap shooterMap; + public HashMap timeToGoalMap; + public HashMap aimerToleranceMap; +} diff --git a/src/main/java/frc/robot/subsystems/scoring/AimerIORoboRio.java b/src/main/java/frc/robot/subsystems/scoring/AimerIORoboRio.java index 2119c19..2f5535a 100644 --- a/src/main/java/frc/robot/subsystems/scoring/AimerIORoboRio.java +++ b/src/main/java/frc/robot/subsystems/scoring/AimerIORoboRio.java @@ -22,18 +22,18 @@ import com.ctre.phoenix6.signals.SensorDirectionValue; import edu.wpi.first.math.MathUtil; import edu.wpi.first.wpilibj.Timer; -import frc.robot.constants.ScoringConstants; +import frc.robot.constants.ConstantsLoader; import java.util.ArrayList; import java.util.List; import org.littletonrobotics.junction.Logger; public class AimerIORoboRio implements AimerIO { - private final TalonFX aimerMotor = new TalonFX(ScoringConstants.aimerMotorId); + private final TalonFX aimerMotor = new TalonFX(ConstantsLoader.ScoringConstants.aimerMotorId); MotionMagicVoltage motionMagicVoltage = new MotionMagicVoltage(0.0); private ControlRequest request; - private final CANcoder aimerEncoder = new CANcoder(ScoringConstants.aimerEncoderId); + private final CANcoder aimerEncoder = new CANcoder(ConstantsLoader.ScoringConstants.aimerEncoderId); private final Timer timer = new Timer(); @@ -63,7 +63,7 @@ public class AimerIORoboRio implements AimerIO { public AimerIORoboRio() { aimerMotor.setNeutralMode(NeutralModeValue.Coast); - setStatorCurrentLimit(ScoringConstants.aimerCurrentLimit); + setStatorCurrentLimit(ConstantsLoader.ScoringConstants.aimerCurrentLimit); CANcoderConfiguration cancoderConfiguration = new CANcoderConfiguration(); // TODO: Check all of these values @@ -71,34 +71,34 @@ public AimerIORoboRio() { AbsoluteSensorRangeValue.Signed_PlusMinusHalf; cancoderConfiguration.MagnetSensor.SensorDirection = SensorDirectionValue.CounterClockwise_Positive; - cancoderConfiguration.MagnetSensor.MagnetOffset = ScoringConstants.aimerEncoderOffset; + cancoderConfiguration.MagnetSensor.MagnetOffset = ConstantsLoader.ScoringConstants.aimerEncoderOffset; aimerEncoder.getConfigurator().apply(cancoderConfiguration); talonFXConfigs.Feedback.FeedbackRemoteSensorID = aimerEncoder.getDeviceID(); talonFXConfigs.Feedback.FeedbackSensorSource = FeedbackSensorSourceValue.FusedCANcoder; talonFXConfigs.Feedback.SensorToMechanismRatio = - ScoringConstants.aimerEncoderToMechanismRatio; - talonFXConfigs.Feedback.RotorToSensorRatio = ScoringConstants.aimerRotorToSensorRatio; + ConstantsLoader.ScoringConstants.aimerEncoderToMechanismRatio; + talonFXConfigs.Feedback.RotorToSensorRatio = ConstantsLoader.ScoringConstants.aimerRotorToSensorRatio; talonFXConfigs.MotorOutput.NeutralMode = NeutralModeValue.Coast; talonFXConfigs.CurrentLimits.StatorCurrentLimitEnable = true; - talonFXConfigs.CurrentLimits.StatorCurrentLimit = ScoringConstants.aimerCurrentLimit; + talonFXConfigs.CurrentLimits.StatorCurrentLimit = ConstantsLoader.ScoringConstants.aimerCurrentLimit; Slot0Configs slot0Configs = talonFXConfigs.Slot0; slot0Configs.GravityType = GravityTypeValue.Arm_Cosine; - slot0Configs.kS = ScoringConstants.aimerkS; - slot0Configs.kV = ScoringConstants.aimerkV; - slot0Configs.kA = ScoringConstants.aimerkA; - slot0Configs.kG = ScoringConstants.aimerkG; + slot0Configs.kS = ConstantsLoader.ScoringConstants.aimerkS; + slot0Configs.kV = ConstantsLoader.ScoringConstants.aimerkV; + slot0Configs.kA = ConstantsLoader.ScoringConstants.aimerkA; + slot0Configs.kG = ConstantsLoader.ScoringConstants.aimerkG; - slot0Configs.kP = ScoringConstants.aimerkP; - slot0Configs.kI = ScoringConstants.aimerkI; - slot0Configs.kD = ScoringConstants.aimerkD; + slot0Configs.kP = ConstantsLoader.ScoringConstants.aimerkP; + slot0Configs.kI = ConstantsLoader.ScoringConstants.aimerkI; + slot0Configs.kD = ConstantsLoader.ScoringConstants.aimerkD; MotionMagicConfigs motionMagicConfigs = talonFXConfigs.MotionMagic; - motionMagicConfigs.MotionMagicCruiseVelocity = ScoringConstants.aimerCruiseVelocity; - motionMagicConfigs.MotionMagicAcceleration = ScoringConstants.aimerAcceleration; + motionMagicConfigs.MotionMagicCruiseVelocity = ConstantsLoader.ScoringConstants.aimerCruiseVelocity; + motionMagicConfigs.MotionMagicAcceleration = ConstantsLoader.ScoringConstants.aimerAcceleration; aimerMotor.getConfigurator().apply(talonFXConfigs); } @@ -130,13 +130,13 @@ public void setAngleClampsRot(double minAngleClamp, double maxAngleClamp) { this.minAngleClamp = MathUtil.clamp( minAngleClamp, - ScoringConstants.aimMinAngleRotations, - ScoringConstants.aimMaxAngleRotations); + ConstantsLoader.ScoringConstants.aimMinAngleRotations, + ConstantsLoader.ScoringConstants.aimMaxAngleRotations); this.maxAngleClamp = MathUtil.clamp( maxAngleClamp, - ScoringConstants.aimMinAngleRotations, - ScoringConstants.aimMaxAngleRotations); + ConstantsLoader.ScoringConstants.aimMinAngleRotations, + ConstantsLoader.ScoringConstants.aimMaxAngleRotations); } @Override @@ -249,15 +249,15 @@ public void applyOutputs(AimerOutputs outputs) { if (lockNegativeAtHome && getEncoderPosition() - < ScoringConstants.aimMinAngleRotations - + ScoringConstants.intakeAngleToleranceRotations + < ConstantsLoader.ScoringConstants.aimMinAngleRotations + + ConstantsLoader.ScoringConstants.intakeAngleToleranceRotations && !override) { // talonFXConfigs.CurrentLimits.StatorCurrentLimit = // ScoringConstants.aimerCurrentLimit / 5; // aimerMotor.getConfigurator().apply(talonFXConfigs); - request = new VoltageOut(ScoringConstants.aimLockVoltage); + request = new VoltageOut(ConstantsLoader.ScoringConstants.aimLockVoltage); } else { - talonFXConfigs.CurrentLimits.StatorCurrentLimit = ScoringConstants.aimerCurrentLimit; + talonFXConfigs.CurrentLimits.StatorCurrentLimit = ConstantsLoader.ScoringConstants.aimerCurrentLimit; // aimerMotor.getConfigurator().apply(talonFXConfigs); motionMagicVoltage.withPosition(goalAngleRot); request = motionMagicVoltage; diff --git a/src/main/java/frc/robot/subsystems/scoring/AimerIOSim.java b/src/main/java/frc/robot/subsystems/scoring/AimerIOSim.java index 4e10338..9746c49 100644 --- a/src/main/java/frc/robot/subsystems/scoring/AimerIOSim.java +++ b/src/main/java/frc/robot/subsystems/scoring/AimerIOSim.java @@ -9,7 +9,7 @@ import edu.wpi.first.math.util.Units; import edu.wpi.first.wpilibj.Timer; import edu.wpi.first.wpilibj.simulation.SingleJointedArmSim; -import frc.robot.constants.ScoringConstants; +import frc.robot.constants.ConstantsLoader; import frc.robot.constants.SimConstants; public class AimerIOSim implements AimerIO { @@ -25,18 +25,18 @@ public class AimerIOSim implements AimerIO { 3 / 2 * Math.PI); private final PIDController controller = new PIDController( - ScoringConstants.aimerkP, ScoringConstants.aimerkI, ScoringConstants.aimerkD); + ConstantsLoader.ScoringConstants.aimerkP, ConstantsLoader.ScoringConstants.aimerkI, ConstantsLoader.ScoringConstants.aimerkD); private final ArmFeedforward feedforward = new ArmFeedforward( - ScoringConstants.aimerkS, - ScoringConstants.aimerkG, - ScoringConstants.aimerkV, - ScoringConstants.aimerkA); + ConstantsLoader.ScoringConstants.aimerkS, + ConstantsLoader.ScoringConstants.aimerkG, + ConstantsLoader.ScoringConstants.aimerkV, + ConstantsLoader.ScoringConstants.aimerkA); private final TrapezoidProfile profile = new TrapezoidProfile( new TrapezoidProfile.Constraints( - ScoringConstants.aimerCruiseVelocity, - ScoringConstants.aimerAcceleration)); + ConstantsLoader.ScoringConstants.aimerCruiseVelocity, + ConstantsLoader.ScoringConstants.aimerAcceleration)); private final Timer timer = new Timer(); @@ -83,9 +83,9 @@ public void setAngleClampsRot(double minAngleClamp, double maxAngleClamp) { // These two lines look like they should make the minimum angle 0.0 rotations, // which would mean that the arm could no longer go below horizontal. this.minAngleClamp = - MathUtil.clamp(minAngleClamp, 0.0, ScoringConstants.aimMaxAngleRotations); + MathUtil.clamp(minAngleClamp, 0.0, ConstantsLoader.ScoringConstants.aimMaxAngleRotations); this.maxAngleClamp = - MathUtil.clamp(maxAngleClamp, 0.0, ScoringConstants.aimMaxAngleRotations); + MathUtil.clamp(maxAngleClamp, 0.0, ConstantsLoader.ScoringConstants.aimMaxAngleRotations); } @Override @@ -130,8 +130,8 @@ public void applyOutputs(AimerOutputs outputs) { double controlSetpoint = MathUtil.clamp( trapezoidSetpoint.position, - ScoringConstants.aimMinAngleRotations, - ScoringConstants.aimMaxAngleRotations); + ConstantsLoader.ScoringConstants.aimMinAngleRotations, + ConstantsLoader.ScoringConstants.aimMaxAngleRotations); double velocitySetpoint = trapezoidSetpoint.velocity; if (override) { diff --git a/src/main/java/frc/robot/subsystems/scoring/ScoringSubsystem.java b/src/main/java/frc/robot/subsystems/scoring/ScoringSubsystem.java index 25e3a72..2294bc3 100644 --- a/src/main/java/frc/robot/subsystems/scoring/ScoringSubsystem.java +++ b/src/main/java/frc/robot/subsystems/scoring/ScoringSubsystem.java @@ -19,7 +19,7 @@ import edu.wpi.first.wpilibj2.command.SubsystemBase; import frc.robot.constants.ModeConstants; import frc.robot.constants.ModeConstants.Mode; -import frc.robot.constants.ScoringConstants; +import frc.robot.constants.ConstantsLoader; import frc.robot.utils.AllianceUtil; import frc.robot.utils.FieldFinder; import frc.robot.utils.FieldFinder.FieldLocations; @@ -111,15 +111,15 @@ public ScoringSubsystem(ShooterIO shooterIo, AimerIO aimerIo) { this.shooterIo = shooterIo; this.aimerIo = aimerIo; - shooterInterpolated = new InterpolateDouble(ScoringConstants.getShooterMap()); + shooterInterpolated = new InterpolateDouble(ConstantsLoader.ScoringConstants.getShooterMap()); aimerInterpolated = new InterpolateDouble( - ScoringConstants.getAimerMap(), - ScoringConstants.aimMinAngleRotations, - ScoringConstants.aimMaxAngleRotations); + ConstantsLoader.ScoringConstants.getAimerMap(), + ConstantsLoader.ScoringConstants.aimMinAngleRotations, + ConstantsLoader.ScoringConstants.aimMaxAngleRotations); - aimerAngleTolerance = new InterpolateDouble(ScoringConstants.aimerToleranceTable()); + aimerAngleTolerance = new InterpolateDouble(ConstantsLoader.ScoringConstants.aimerToleranceTable()); if (ModeConstants.currentMode == Mode.SIM) { mechanism = new Mechanism2d(2.2, 2.0); @@ -143,7 +143,7 @@ public boolean atAimerGoalPosition() { private void idle() { // aimerIo.setAimAngleRad(ScoringConstants.aimMinAngleRotations + 0.001); // Todo: if we can trust aimer, figure out if we really need to add 0.001 to the min angle - aimerIo.setAimAngleRot(ScoringConstants.aimMinAngleRotations); + aimerIo.setAimAngleRot(ConstantsLoader.ScoringConstants.aimMinAngleRotations); shooterIo.setShooterVelocityRPM(0); shooterIo.setKickerVolts(0); @@ -183,11 +183,11 @@ private void idle() { } private void intake() { - aimerIo.setAimAngleRot(ScoringConstants.aimMinAngleRotations); + aimerIo.setAimAngleRot(ConstantsLoader.ScoringConstants.aimMinAngleRotations); aimerIo.setNegativeHomeLockMode(true); if (!hasNote()) { - shooterIo.setKickerVolts(ScoringConstants.kickerIntakeVolts); + shooterIo.setKickerVolts(ConstantsLoader.ScoringConstants.kickerIntakeVolts); } else { shooterIo.setKickerVolts(0.0); } @@ -235,24 +235,24 @@ private void prime() { shooterIo.setShooterVelocityRPM(shooterInterpolated.getValue(distanceToGoal)); aimerIo.setAimAngleRot(getAimerAngle(distanceToGoal)); if (!overrideBeamBreak) { - shooterIo.setKickerVolts(hasNote() ? 0.0 : ScoringConstants.kickerIntakeVolts); + shooterIo.setKickerVolts(hasNote() ? 0.0 : ConstantsLoader.ScoringConstants.kickerIntakeVolts); } boolean shooterReady = shooterInputs.shooterLeftVelocityRPM < (shooterOutputs.shooterLeftGoalVelocityRPM - + ScoringConstants.shooterUpperVelocityMarginRPM) + + ConstantsLoader.ScoringConstants.shooterUpperVelocityMarginRPM) && shooterInputs.shooterLeftVelocityRPM > (shooterOutputs.shooterLeftGoalVelocityRPM - - ScoringConstants.shooterLowerVelocityMarginRPM); + - ConstantsLoader.ScoringConstants.shooterLowerVelocityMarginRPM); boolean aimReady = Math.abs(aimerInputs.aimAngleRot - aimerInputs.aimGoalAngleRot) < aimerAngleTolerance.getValue(distanceToGoal) && Math.abs(aimerInputs.aimVelocityErrorRotPerSec) - < ScoringConstants.aimAngleVelocityMargin; + < ConstantsLoader.ScoringConstants.aimAngleVelocityMargin; boolean driveReady = (driveAlignedSupplier.get() - || distanceToGoal < ScoringConstants.minDistanceAlignmentNeeded); + || distanceToGoal < ConstantsLoader.ScoringConstants.minDistanceAlignmentNeeded); boolean fieldLocationReady = true; if (!DriverStation.getAlliance().isPresent()) { @@ -302,7 +302,7 @@ private void prime() { private void ampPrime() { // shooterIo.setShooterVelocityRPM(ScoringConstants.shooterAmpVelocityRPM); // TODO: Test this out - aimerIo.setAimAngleRot(ScoringConstants.ampAimerAngleRotations); + aimerIo.setAimAngleRot(ConstantsLoader.ScoringConstants.ampAimerAngleRotations); if (action != ScoringAction.SHOOT && action != ScoringAction.AMP_AIM) { state = ScoringState.IDLE; } else if (action == ScoringAction.SHOOT) { @@ -314,23 +314,23 @@ private void ampPrime() { } private void passPrime() { - aimerIo.setAimAngleRot(ScoringConstants.passLocationRot); - shooterIo.setShooterVelocityRPM(ScoringConstants.passShooterRpm); + aimerIo.setAimAngleRot(ConstantsLoader.ScoringConstants.passLocationRot); + shooterIo.setShooterVelocityRPM(ConstantsLoader.ScoringConstants.passShooterRpm); boolean notePresent = overrideBeamBreak ? true : hasNote(); boolean shooterReady = shooterInputs.shooterLeftVelocityRPM < (shooterOutputs.shooterLeftGoalVelocityRPM - + ScoringConstants.shooterUpperVelocityMarginRPM) + + ConstantsLoader.ScoringConstants.shooterUpperVelocityMarginRPM) && shooterInputs.shooterLeftVelocityRPM > (shooterOutputs.shooterLeftGoalVelocityRPM - - ScoringConstants.shooterLowerVelocityMarginRPM); + - ConstantsLoader.ScoringConstants.shooterLowerVelocityMarginRPM); boolean aimReady = - Math.abs(aimerInputs.aimAngleRot - ScoringConstants.passLocationRot) - < ScoringConstants.passAngleToleranceRot + Math.abs(aimerInputs.aimAngleRot - ConstantsLoader.ScoringConstants.passLocationRot) + < ConstantsLoader.ScoringConstants.passAngleToleranceRot && Math.abs(aimerInputs.aimVelocityErrorRotPerSec) - < ScoringConstants.aimAngleVelocityMargin; + < ConstantsLoader.ScoringConstants.aimAngleVelocityMargin; boolean driveReady = driveAlignedSupplier.get(); @@ -373,7 +373,7 @@ private void shoot() { private void ampShoot() { shooterIo.setKickerVolts(-12); // TODO: Test if this kicks note forward or backward - aimerIo.setAimAngleRot(ScoringConstants.ampAimerAngleRotations); + aimerIo.setAimAngleRot(ConstantsLoader.ScoringConstants.ampAimerAngleRotations); // Revert to amp prime after we shoot the note // This will keep the arm up and allow the driver to drive away from the amp before the arm @@ -394,8 +394,8 @@ private void ampShoot() { } private void passShoot() { - aimerIo.setAimAngleRot(ScoringConstants.passLocationRot); - shooterIo.setShooterVelocityRPM(ScoringConstants.passShooterRpm); + aimerIo.setAimAngleRot(ConstantsLoader.ScoringConstants.passLocationRot); + shooterIo.setShooterVelocityRPM(ConstantsLoader.ScoringConstants.passShooterRpm); shooterIo.setKickerVolts(12); @@ -471,8 +471,8 @@ public boolean hasNote() { public boolean aimerAtIntakePosition() { return aimerInputs.aimAngleRot - < ScoringConstants.aimMinAngleRotations - + ScoringConstants.intakeAngleToleranceRotations; + < ConstantsLoader.ScoringConstants.aimMinAngleRotations + + ConstantsLoader.ScoringConstants.intakeAngleToleranceRotations; // return true;\][] } @@ -510,7 +510,7 @@ public void periodic() { // Intake will set it to true later in the loop. if (!SmartDashboard.containsKey("Aimer Offset")) { - SmartDashboard.putNumber("Aimer Offset", ScoringConstants.aimerStaticOffset); + SmartDashboard.putNumber("Aimer Offset", ConstantsLoader.ScoringConstants.aimerStaticOffset); } if (!SmartDashboard.containsKey("Beam Break Overridden")) { @@ -521,13 +521,13 @@ public void periodic() { if (state == ScoringState.TEMPORARY_SETPOINT) { aimerIo.setAngleClampsRot( - ScoringConstants.aimMinAngleRotations, ScoringConstants.aimMaxAngleRotations); + ConstantsLoader.ScoringConstants.aimMinAngleRotations, ConstantsLoader.ScoringConstants.aimMaxAngleRotations); } else if (state != ScoringState.TUNING && state != ScoringState.ENDGAME && state != ScoringState.IDLE // && Math.abs(elevatorPositionSupplier.getAsDouble()) < 0.2 && !overrideStageAvoidance) { - aimerIo.setAngleClampsRot(ScoringConstants.aimMinAngleRotations, 0); + aimerIo.setAngleClampsRot(ConstantsLoader.ScoringConstants.aimMinAngleRotations, 0); } aimerIo.controlAimAngleRot(); @@ -642,7 +642,7 @@ public void setArmDisabled(boolean disabled) { public double getAimerAngle(double distance) { return aimerInterpolated.getValue(distance) - + SmartDashboard.getNumber("Aimer Offset", ScoringConstants.aimerStaticOffset); + + SmartDashboard.getNumber("Aimer Offset", ConstantsLoader.ScoringConstants.aimerStaticOffset); } @Override diff --git a/src/main/java/frc/robot/subsystems/scoring/ShooterIOSim.java b/src/main/java/frc/robot/subsystems/scoring/ShooterIOSim.java index 38eb390..1684866 100644 --- a/src/main/java/frc/robot/subsystems/scoring/ShooterIOSim.java +++ b/src/main/java/frc/robot/subsystems/scoring/ShooterIOSim.java @@ -5,7 +5,7 @@ import edu.wpi.first.math.system.plant.DCMotor; import edu.wpi.first.wpilibj.simulation.FlywheelSim; import frc.robot.constants.ConversionConstants; -import frc.robot.constants.ScoringConstants; +import frc.robot.constants.ConstantsLoader; import frc.robot.constants.SimConstants; public class ShooterIOSim implements ShooterIO { @@ -17,20 +17,20 @@ public class ShooterIOSim implements ShooterIO { private final PIDController shooterLeftController = new PIDController( - ScoringConstants.shooterkP, - ScoringConstants.shooterkI, - ScoringConstants.shooterkD); + ConstantsLoader.ScoringConstants.shooterkP, + ConstantsLoader.ScoringConstants.shooterkI, + ConstantsLoader.ScoringConstants.shooterkD); private final PIDController shooterRightController = new PIDController( - ScoringConstants.shooterkP, - ScoringConstants.shooterkI, - ScoringConstants.shooterkD); + ConstantsLoader.ScoringConstants.shooterkP, + ConstantsLoader.ScoringConstants.shooterkI, + ConstantsLoader.ScoringConstants.shooterkD); private final SimpleMotorFeedforward shooterFeedforward = new SimpleMotorFeedforward( - ScoringConstants.shooterkS, - ScoringConstants.shooterkV, - ScoringConstants.shooterkA); + ConstantsLoader.ScoringConstants.shooterkS, + ConstantsLoader.ScoringConstants.shooterkV, + ConstantsLoader.ScoringConstants.shooterkA); private boolean override = false; @@ -45,7 +45,7 @@ public class ShooterIOSim implements ShooterIO { @Override public void setShooterVelocityRPM(double velocity) { shooterLeftGoalVelRPM = velocity; - shooterRightGoalVelRPM = velocity * ScoringConstants.shooterOffsetAdjustment; + shooterRightGoalVelRPM = velocity * ConstantsLoader.ScoringConstants.shooterOffsetAdjustment; } @Override diff --git a/src/main/java/frc/robot/subsystems/scoring/ShooterIOTalonFX.java b/src/main/java/frc/robot/subsystems/scoring/ShooterIOTalonFX.java index f7a2f0f..fe475a9 100644 --- a/src/main/java/frc/robot/subsystems/scoring/ShooterIOTalonFX.java +++ b/src/main/java/frc/robot/subsystems/scoring/ShooterIOTalonFX.java @@ -9,17 +9,18 @@ import com.revrobotics.CANSparkFlex; import com.revrobotics.CANSparkLowLevel.MotorType; import com.revrobotics.SparkLimitSwitch; + +import frc.robot.constants.ConstantsLoader; import frc.robot.constants.ConversionConstants; -import frc.robot.constants.ScoringConstants; import java.util.ArrayList; import java.util.List; public class ShooterIOTalonFX implements ShooterIO { private final CANSparkFlex kicker = - new CANSparkFlex(ScoringConstants.kickerMotorId, MotorType.kBrushless); + new CANSparkFlex(ConstantsLoader.ScoringConstants.kickerMotorId, MotorType.kBrushless); - private final TalonFX shooterLeft = new TalonFX(ScoringConstants.shooterLeftMotorId); - private final TalonFX shooterRight = new TalonFX(ScoringConstants.shooterRightMotorId); + private final TalonFX shooterLeft = new TalonFX(ConstantsLoader.ScoringConstants.shooterLeftMotorId); + private final TalonFX shooterRight = new TalonFX(ConstantsLoader.ScoringConstants.shooterRightMotorId); private final Slot0Configs slot0 = new Slot0Configs(); @@ -43,22 +44,22 @@ public ShooterIOTalonFX() { TalonFXConfigurator shooterLeftConfig = shooterLeft.getConfigurator(); shooterLeftConfig.apply( new CurrentLimitsConfigs() - .withStatorCurrentLimit(ScoringConstants.shooterCurrentLimit) + .withStatorCurrentLimit(ConstantsLoader.ScoringConstants.shooterCurrentLimit) .withStatorCurrentLimitEnable(true)); TalonFXConfigurator shooterRightConfig = shooterRight.getConfigurator(); shooterRightConfig.apply( new CurrentLimitsConfigs() - .withStatorCurrentLimit(ScoringConstants.shooterCurrentLimit) + .withStatorCurrentLimit(ConstantsLoader.ScoringConstants.shooterCurrentLimit) .withStatorCurrentLimitEnable(true)); - slot0.withKP(ScoringConstants.shooterkP); - slot0.withKI(ScoringConstants.shooterkI); - slot0.withKD(ScoringConstants.shooterkD); + slot0.withKP(ConstantsLoader.ScoringConstants.shooterkP); + slot0.withKI(ConstantsLoader.ScoringConstants.shooterkI); + slot0.withKD(ConstantsLoader.ScoringConstants.shooterkD); - slot0.withKS(ScoringConstants.shooterkS); - slot0.withKV(ScoringConstants.shooterkV); - slot0.withKA(ScoringConstants.shooterkA); + slot0.withKS(ConstantsLoader.ScoringConstants.shooterkS); + slot0.withKV(ConstantsLoader.ScoringConstants.shooterkV); + slot0.withKA(ConstantsLoader.ScoringConstants.shooterkA); shooterLeft.getConfigurator().apply(slot0); shooterRight.getConfigurator().apply(slot0); @@ -67,7 +68,7 @@ public ShooterIOTalonFX() { @Override public void setShooterVelocityRPM(double velocity) { goalLeftVelocityRPM = velocity; - goalRightVelocityRPM = velocity * ScoringConstants.shooterOffsetAdjustment; + goalRightVelocityRPM = velocity * ConstantsLoader.ScoringConstants.shooterOffsetAdjustment; } @Override From 5c4e56b37b0f22322dd2032a68127538b9b9ccd5 Mon Sep 17 00:00:00 2001 From: Jack Lingle Date: Thu, 21 Nov 2024 09:20:38 -0500 Subject: [PATCH 4/5] add static back to schemas (i think they can be there) --- .../constants/DriverConstantsSchema.java | 2 +- .../robot/constants/FeatureFlagsSchema.java | 14 +- .../constants/ScoringConstantsSchema.java | 154 +++++++----------- 3 files changed, 68 insertions(+), 102 deletions(-) diff --git a/src/main/java/frc/robot/constants/DriverConstantsSchema.java b/src/main/java/frc/robot/constants/DriverConstantsSchema.java index 660ad91..9deb6d1 100644 --- a/src/main/java/frc/robot/constants/DriverConstantsSchema.java +++ b/src/main/java/frc/robot/constants/DriverConstantsSchema.java @@ -1,5 +1,5 @@ package frc.robot.constants; public class DriverConstantsSchema { - public double joystickDeadband = 0.16; + public static double joystickDeadband = 0.16; } diff --git a/src/main/java/frc/robot/constants/FeatureFlagsSchema.java b/src/main/java/frc/robot/constants/FeatureFlagsSchema.java index 9a6455a..c76e266 100644 --- a/src/main/java/frc/robot/constants/FeatureFlagsSchema.java +++ b/src/main/java/frc/robot/constants/FeatureFlagsSchema.java @@ -1,13 +1,13 @@ package frc.robot.constants; public class FeatureFlagsSchema { - public boolean runDrive = true; - public boolean runVision = false; + public static boolean runDrive = true; + public static boolean runVision = false; // NOTE: A featureflag "runLocalizer" was removed from here recently. // TODO: Figure out if we need this and add it back if necessary. - public boolean runLocalizer = false; - public boolean runIntake = true; - public boolean runScoring = true; - public boolean runLEDS = true; - public boolean runOrchestra = true; + public static boolean runLocalizer = false; + public static boolean runIntake = true; + public static boolean runScoring = true; + public static boolean runLEDS = true; + public static boolean runOrchestra = true; } diff --git a/src/main/java/frc/robot/constants/ScoringConstantsSchema.java b/src/main/java/frc/robot/constants/ScoringConstantsSchema.java index 47004a4..092cd97 100644 --- a/src/main/java/frc/robot/constants/ScoringConstantsSchema.java +++ b/src/main/java/frc/robot/constants/ScoringConstantsSchema.java @@ -4,130 +4,96 @@ import java.util.HashMap; public class ScoringConstantsSchema { - public double aimerkP = 100.0; - public double aimerkI = 2.0; // 5.0 - public double aimerkD = 0.0; + public static double aimerkP = 100.0; + public static double aimerkI = 2.0; // 5.0 + public static double aimerkD = 0.0; - public double aimerkS = 0.3; // 0.25; - public double aimerkG = 0.955; // 0.2; - public double aimerkV = 0.0; - public double aimerkA = 0.0; + public static double aimerkS = 0.3; // 0.25; + public static double aimerkG = 0.955; // 0.2; + public static double aimerkV = 0.0; + public static double aimerkA = 0.0; - public double shooterkP = 0.05; - public double shooterkI = 1; - public double shooterkD = 0.0; + public static double shooterkP = 0.05; + public static double shooterkI = 1; + public static double shooterkD = 0.0; - public double shooterkS = 0.0; // TODO: Find Imperically - public double shooterkV = 0.005; - public double shooterkA = 0.0; + public static double shooterkS = 0.0; // TODO: Find Imperically + public static double shooterkV = 0.005; + public static double shooterkA = 0.0; - public int aimerMotorId = 9; + public static int aimerMotorId = 9; - public int kickerMotorId = 5; // 1 + public static int kickerMotorId = 5; // 1 // TODO: REPLACE THIS WHEN THE ACTUAL SHOOTER IO IS MERGED - public int shooterLeftMotorId = 10; - public int shooterRightMotorId = 11; + public static int shooterLeftMotorId = 10; + public static int shooterRightMotorId = 11; - public double shooterCurrentLimit = 120; - public double kickerCurrentLimit = 120; - public double aimerCurrentLimit = 60; + public static double shooterCurrentLimit = 120; + public static double kickerCurrentLimit = 120; + public static double aimerCurrentLimit = 60; - public int aimerEncoderId = 13; + public static int aimerEncoderId = 13; - public double aimerEncoderOffset = -0.087402; // Armencoder is zeroed + public static double aimerEncoderOffset = -0.087402; // Armencoder is zeroed - public double aimerEncoderToMechanismRatio = 1.0; - public double aimerRotorToSensorRatio = 90.0; + public static double aimerEncoderToMechanismRatio = 1.0; + public static double aimerRotorToSensorRatio = 90.0; - public double kickerIntakeVolts = 2.0; + public static double kickerIntakeVolts = 2.0; // public double aimPositionTolerance = 0.003; - public double aimerAcceleration = 3.5; - public double aimerCruiseVelocity = 0.8; + public static double aimerAcceleration = 3.5; + public static double aimerCruiseVelocity = 0.8; - public double shooterLowerVelocityMarginRPM = 50; - public double shooterUpperVelocityMarginRPM = 150; - public double aimAngleMarginRotations = Units.degreesToRotations(1); - public double aimAngleVelocityMargin = 2.0; + public static double shooterLowerVelocityMarginRPM = 50; + public static double shooterUpperVelocityMarginRPM = 150; + public static double aimAngleMarginRotations = Units.degreesToRotations(1); + public static double aimAngleVelocityMargin = 2.0; - public double minDistanceAlignmentNeeded = 1.3; // TODO: Tune this value + public static double minDistanceAlignmentNeeded = 1.3; // TODO: Tune this value - public double intakeAngleToleranceRotations = 0.05; // Todo: tune this value + public static double intakeAngleToleranceRotations = 0.05; // Todo: tune this value - public double aimerAmpPositionRot = 0.145; + public static double aimerAmpPositionRot = 0.145; - public double aimMaxAngleRotations = 0.361328 - 0.184570; - public double aimMinAngleRotations = -0.212285; - public double aimLockVoltage = -0.5; + public static double aimMaxAngleRotations = 0.361328 - 0.184570; + public static double aimMinAngleRotations = -0.212285; + public static double aimLockVoltage = -0.5; - public double aimAngleTolerance = 0.015; + public static double aimAngleTolerance = 0.015; - public double ampAimerAngleRotations = 0.14; + public static double ampAimerAngleRotations = 0.14; - public double maxAimIntake = 0.0; - public double minAimIntake = 0.0; + public static double maxAimIntake = 0.0; + public static double minAimIntake = 0.0; - public double shooterOffsetAdjustment = 0.6; + public static double shooterOffsetAdjustment = 0.6; - public double maxElevatorPosition = 0.45; - public double maxAimAngleElevatorLimit = Math.PI / 2; + public static double maxElevatorPosition = 0.45; + public static double maxAimAngleElevatorLimit = Math.PI / 2; - public double[] aimerDistance; - public double[] aimerPosition; + public static double[] aimerDistance; + public static double[] aimerPosition; - public double passLocationRot = -0.14; - public double passAngleToleranceRot = 0.005; - public double passShooterRpm = 2800.0; + public static double passLocationRot = -0.14; + public static double passAngleToleranceRot = 0.005; + public static double passShooterRpm = 2800.0; - public double aimerOffset = 0.0; + public static double aimerOffset = 0.0; - public double[] shooterDistance; - public double[] shooterRPM; + public static double[] shooterDistance; + public static double[] shooterRPM; - // NOTE - This should be monotonically increasing - // Key - Distance in meters - // Value - Shooter RPM - public HashMap getShooterMap() { - HashMap map = new HashMap(); - map.put(1.284, 3500.0); - map.put(1.7, 4000.0); + public static double[] timeToGoalDistance; + public static double[] timeToGoal; - return map; - } + public static double[] aimerToleranceDistance; + public static double[] aimerTolerance; - public double[] timeToGoalDistance; - public double[] timeToGoal; - - // NOTE - This should be monotonically increasing - // Key - Distance in meters - // Value - Time in seconds - public HashMap timeToGoalMap() { - HashMap map = new HashMap(); - map.put(0.0, 0.15); - map.put(1.3, 0.15); - map.put(1.4, 0.16); - // map.put(1.3, 0.15); - - return map; - } - - public double[] aimerToleranceDistance; - public double[] aimerTolerance; - - // NOTE - This should be monotonically increasing - // Key - Distance to goal in meters - // Value - Aimer angle tolerance in rotations - public HashMap aimerToleranceTable() { - HashMap map = new HashMap(); - map.put(0.0, 0.003); // Todo: tune this value after conversion from radians to rotations - - return map; - } - - public HashMap aimerMap; - public HashMap shooterMap; - public HashMap timeToGoalMap; - public HashMap aimerToleranceMap; + public static HashMap aimerMap; + public static HashMap shooterMap; + public static HashMap timeToGoalMap; + public static HashMap aimerToleranceMap; } From 266385e28d951b560811c2b4ef429616a138b4c6 Mon Sep 17 00:00:00 2001 From: Jack Lingle Date: Fri, 22 Nov 2024 16:31:42 -0500 Subject: [PATCH 5/5] drive constants have been added, done adding constants as this should be plenty for checking, now build just fialing due to jitpack --- src/main/java/frc/robot/RobotContainer.java | 33 +++-- .../robot/commands/DriveWithJoysticks.java | 7 +- .../frc/robot/commands/ShootWithGamepad.java | 3 +- .../frc/robot/constants/ConstantsLoader.java | 110 +++++++++++++++ .../robot/constants/ConversionConstants.java | 12 +- .../constants/DriverConstantsSchema.java | 2 +- .../robot/constants/FeatureFlagsSchema.java | 14 +- .../frc/robot/constants/LEDConstants.java | 4 +- .../frc/robot/constants/ModeConstants.java | 6 +- ....java => PhoenixDriveConstantsSchema.java} | 128 ++++++++++-------- .../constants/ScoringConstantsSchema.java | 124 +++++++++-------- .../frc/robot/constants/SimConstants.java | 2 +- .../robot/subsystems/drive/PhoenixDrive.java | 17 ++- .../localization/CameraContainerSim.java | 4 +- .../subsystems/scoring/ScoringSubsystem.java | 6 +- 15 files changed, 301 insertions(+), 171 deletions(-) rename src/main/java/frc/robot/constants/{PhoenixDriveConstants.java => PhoenixDriveConstantsSchema.java} (65%) diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 588ff76..90764ff 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -17,9 +17,8 @@ import frc.robot.commands.ShootWithGamepad; import frc.robot.constants.ConstantsLoader; import frc.robot.constants.ModeConstants; -import frc.robot.constants.PhoenixDriveConstants; -import frc.robot.constants.PhoenixDriveConstants.AlignTarget; import frc.robot.constants.VisionConstants; +import frc.robot.constants.PhoenixDriveConstantsSchema.AlignTarget; import frc.robot.subsystems.LED; import frc.robot.subsystems.OrchestraSubsystem; import frc.robot.subsystems.drive.PhoenixDrive; @@ -134,32 +133,32 @@ private void initDrive() { case REAL: drive = new PhoenixDrive( - PhoenixDriveConstants.DrivetrainConstants, - PhoenixDriveConstants.FrontLeft, - PhoenixDriveConstants.FrontRight, - PhoenixDriveConstants.BackLeft, - PhoenixDriveConstants.BackRight); + ConstantsLoader.PhoenixDriveConstants.DrivetrainConstants, + ConstantsLoader.PhoenixDriveConstants.FrontLeft, + ConstantsLoader.PhoenixDriveConstants.FrontRight, + ConstantsLoader.PhoenixDriveConstants.BackLeft, + ConstantsLoader.PhoenixDriveConstants.BackRight); logger = new Telemetry(6); break; case SIM: drive = new PhoenixDrive( - PhoenixDriveConstants.DrivetrainConstants, - PhoenixDriveConstants.FrontLeft, - PhoenixDriveConstants.FrontRight, - PhoenixDriveConstants.BackLeft, - PhoenixDriveConstants.BackRight); + ConstantsLoader.PhoenixDriveConstants.DrivetrainConstants, + ConstantsLoader.PhoenixDriveConstants.FrontLeft, + ConstantsLoader.PhoenixDriveConstants.FrontRight, + ConstantsLoader.PhoenixDriveConstants.BackLeft, + ConstantsLoader.PhoenixDriveConstants.BackRight); logger = new Telemetry(6); break; case REPLAY: drive = new PhoenixDrive( - PhoenixDriveConstants.DrivetrainConstants, - PhoenixDriveConstants.FrontLeft, - PhoenixDriveConstants.FrontRight, - PhoenixDriveConstants.BackLeft, - PhoenixDriveConstants.BackRight); + ConstantsLoader.PhoenixDriveConstants.DrivetrainConstants, + ConstantsLoader.PhoenixDriveConstants.FrontLeft, + ConstantsLoader.PhoenixDriveConstants.FrontRight, + ConstantsLoader.PhoenixDriveConstants.BackLeft, + ConstantsLoader.PhoenixDriveConstants.BackRight); logger = new Telemetry(6); break; diff --git a/src/main/java/frc/robot/commands/DriveWithJoysticks.java b/src/main/java/frc/robot/commands/DriveWithJoysticks.java index 30fe952..c7b9860 100644 --- a/src/main/java/frc/robot/commands/DriveWithJoysticks.java +++ b/src/main/java/frc/robot/commands/DriveWithJoysticks.java @@ -8,7 +8,6 @@ import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.button.CommandJoystick; import frc.robot.constants.ConstantsLoader; -import frc.robot.constants.PhoenixDriveConstants; import frc.robot.subsystems.drive.PhoenixDrive; public class DriveWithJoysticks extends Command { @@ -52,14 +51,14 @@ public void execute() { leftJoystick.getX(), leftJoystick.getY(), ConstantsLoader.DriverConstants.joystickDeadband); - double filteredVY = -leftJoystickDeadbanded[1] * PhoenixDriveConstants.maxSpeedMetPerSec; - double filteredVX = -leftJoystickDeadbanded[0] * PhoenixDriveConstants.maxSpeedMetPerSec; + double filteredVY = -leftJoystickDeadbanded[1] * ConstantsLoader.PhoenixDriveConstants.maxSpeedMetPerSec; + double filteredVX = -leftJoystickDeadbanded[0] * ConstantsLoader.PhoenixDriveConstants.maxSpeedMetPerSec; double rightJoystickDeadbanded = Deadband.oneAxisDeadband( rightJoystick.getX(), ConstantsLoader.DriverConstants.joystickDeadband); double filteredOmega = - -rightJoystickDeadbanded * PhoenixDriveConstants.MaxAngularRateRadPerSec; + -rightJoystickDeadbanded * ConstantsLoader.PhoenixDriveConstants.MaxAngularRateRadPerSec; chassisSpeeds = new ChassisSpeeds(filteredVY, filteredVX, filteredOmega); drivetrain.setGoalSpeeds(chassisSpeeds, true); diff --git a/src/main/java/frc/robot/commands/ShootWithGamepad.java b/src/main/java/frc/robot/commands/ShootWithGamepad.java index a86681f..177a995 100644 --- a/src/main/java/frc/robot/commands/ShootWithGamepad.java +++ b/src/main/java/frc/robot/commands/ShootWithGamepad.java @@ -2,7 +2,8 @@ import edu.wpi.first.wpilibj.DriverStation; import edu.wpi.first.wpilibj2.command.Command; -import frc.robot.constants.PhoenixDriveConstants.AlignTarget; +import frc.robot.constants.ConstantsLoader; +import frc.robot.constants.PhoenixDriveConstantsSchema.AlignTarget; import frc.robot.subsystems.scoring.ScoringSubsystem; import frc.robot.subsystems.scoring.ScoringSubsystem.ScoringAction; import java.util.function.BooleanSupplier; diff --git a/src/main/java/frc/robot/constants/ConstantsLoader.java b/src/main/java/frc/robot/constants/ConstantsLoader.java index 2b2fdc8..48b6b32 100644 --- a/src/main/java/frc/robot/constants/ConstantsLoader.java +++ b/src/main/java/frc/robot/constants/ConstantsLoader.java @@ -2,7 +2,15 @@ import java.util.HashMap; +import com.ctre.phoenix6.configs.Slot0Configs; +import com.ctre.phoenix6.mechanisms.swerve.SwerveDrivetrainConstants; +import com.ctre.phoenix6.mechanisms.swerve.SwerveModuleConstants.SteerFeedbackType; +import com.ctre.phoenix6.mechanisms.swerve.SwerveModuleConstantsFactory; + import coppercore.parameter_tools.JSONSync; +import edu.wpi.first.math.geometry.Translation2d; +import edu.wpi.first.math.kinematics.SwerveDriveKinematics; +import edu.wpi.first.math.util.Units; public class ConstantsLoader { /* @@ -17,6 +25,7 @@ public class ConstantsLoader { public static DriverConstantsSchema DriverConstants; public static FeatureFlagsSchema FeatureFlags; public static ScoringConstantsSchema ScoringConstants; + public static PhoenixDriveConstantsSchema PhoenixDriveConstants; public static void loadDriverConstants() { JSONSync synced = @@ -67,4 +76,105 @@ public static void loadScoringConstants() { ScoringConstants.aimerToleranceMap.put(ScoringConstants.aimerToleranceDistance[i], ScoringConstants.aimerTolerance[i]); } } + + public static void loadPhoenixDriveConstants() { + JSONSync synced = + new JSONSync( + new PhoenixDriveConstantsSchema(), + "./DriverConstants.json", + new JSONSync.JSONSyncConfigBuilder().build()); + synced.loadData(); + PhoenixDriveConstants = synced.getObject(); + + PhoenixDriveConstants.steerGains = + new Slot0Configs() + .withKP(PhoenixDriveConstants.steerKP) + .withKI(PhoenixDriveConstants.steerKI) + .withKD(PhoenixDriveConstants.steerKD) + .withKS(PhoenixDriveConstants.steerKS) + .withKV(PhoenixDriveConstants.steerKV) + .withKA(PhoenixDriveConstants.steerKA); + + PhoenixDriveConstants.driveGains = + new Slot0Configs() + .withKP(PhoenixDriveConstants.driveKP) + .withKI(PhoenixDriveConstants.driveKI) + .withKD(PhoenixDriveConstants.driveKD) + .withKS(PhoenixDriveConstants.driveKS) + .withKV(PhoenixDriveConstants.driveKV) + .withKA(PhoenixDriveConstants.driveKA); + + PhoenixDriveConstants.DrivetrainConstants = + new SwerveDrivetrainConstants().withPigeon2Id(PhoenixDriveConstants.kPigeonId).withCANbusName(PhoenixDriveConstants.kCANbusName); + + PhoenixDriveConstants.ConstantCreator = + new SwerveModuleConstantsFactory() + .withDriveMotorGearRatio(PhoenixDriveConstants.kDriveGearRatio) + .withSteerMotorGearRatio(PhoenixDriveConstants.kSteerGearRatio) + .withWheelRadius(PhoenixDriveConstants.kWheelRadiusInches) + .withSlipCurrent(PhoenixDriveConstants.kSlipCurrentA) + .withSteerMotorGains(PhoenixDriveConstants.steerGains) + .withDriveMotorGains(PhoenixDriveConstants.driveGains) + .withSteerMotorClosedLoopOutput(PhoenixDriveConstants.steerClosedLoopOutput) + .withDriveMotorClosedLoopOutput(PhoenixDriveConstants.driveClosedLoopOutput) + .withSpeedAt12VoltsMps(PhoenixDriveConstants.kSpeedAt12VoltsMps) + .withSteerInertia(PhoenixDriveConstants.kSteerInertia) + .withDriveInertia(PhoenixDriveConstants.kDriveInertia) + .withSteerFrictionVoltage(PhoenixDriveConstants.kSteerFrictionVoltage) + .withDriveFrictionVoltage(PhoenixDriveConstants.kDriveFrictionVoltage) + .withFeedbackSource(SteerFeedbackType.FusedCANcoder) + .withCouplingGearRatio(PhoenixDriveConstants.kCoupleRatio); + + PhoenixDriveConstants.FrontLeft = + PhoenixDriveConstants.ConstantCreator.createModuleConstants( + PhoenixDriveConstants.kFrontLeftSteerMotorId, + PhoenixDriveConstants.kFrontLeftDriveMotorId, + PhoenixDriveConstants.kFrontLeftEncoderId, + PhoenixDriveConstants.kFrontLeftEncoderOffset, + Units.inchesToMeters(PhoenixDriveConstants.kFrontLeftXPosInches), + Units.inchesToMeters(PhoenixDriveConstants.kFrontLeftYPosInches), + PhoenixDriveConstants.kInvertLeftSide) + .withSteerMotorInverted(PhoenixDriveConstants.kFrontLeftSteerInvert); + + PhoenixDriveConstants.FrontRight = + PhoenixDriveConstants.ConstantCreator.createModuleConstants( + PhoenixDriveConstants.kFrontRightSteerMotorId, + PhoenixDriveConstants.kFrontRightDriveMotorId, + PhoenixDriveConstants.kFrontRightEncoderId, + PhoenixDriveConstants.kFrontRightEncoderOffset, + Units.inchesToMeters(PhoenixDriveConstants.kFrontRightXPosInches), + Units.inchesToMeters(PhoenixDriveConstants.kFrontRightYPosInches), + PhoenixDriveConstants.kInvertRightSide) + .withSteerMotorInverted(PhoenixDriveConstants.kFrontRightSteerInvert); + + PhoenixDriveConstants.BackLeft = + PhoenixDriveConstants.ConstantCreator.createModuleConstants( + PhoenixDriveConstants.kBackLeftSteerMotorId, + PhoenixDriveConstants.kBackLeftDriveMotorId, + PhoenixDriveConstants.kBackLeftEncoderId, + PhoenixDriveConstants.kBackLeftEncoderOffset, + Units.inchesToMeters(PhoenixDriveConstants.kBackLeftXPosInches), + Units.inchesToMeters(PhoenixDriveConstants.kBackLeftYPosInches), + PhoenixDriveConstants.kInvertLeftSide) + .withSteerMotorInverted(PhoenixDriveConstants.kBackLeftSteerInvert) + .withDriveMotorInverted(PhoenixDriveConstants.kBackLeftDriveInvert); + + PhoenixDriveConstants.BackRight = + PhoenixDriveConstants.ConstantCreator.createModuleConstants( + PhoenixDriveConstants.kBackRightSteerMotorId, + PhoenixDriveConstants.kBackRightDriveMotorId, + PhoenixDriveConstants.kBackRightEncoderId, + PhoenixDriveConstants.kBackRightEncoderOffset, + Units.inchesToMeters(PhoenixDriveConstants.kBackRightXPosInches), + Units.inchesToMeters(PhoenixDriveConstants.kBackRightYPosInches), + PhoenixDriveConstants.kInvertRightSide) + .withSteerMotorInverted(PhoenixDriveConstants.kBackRightSteerInvert); + + PhoenixDriveConstants.kinematics = + new SwerveDriveKinematics( + new Translation2d(PhoenixDriveConstants.FrontLeft.LocationX, PhoenixDriveConstants.FrontLeft.LocationY), + new Translation2d(PhoenixDriveConstants.FrontLeft.LocationX, PhoenixDriveConstants.FrontRight.LocationY), + new Translation2d(PhoenixDriveConstants.BackLeft.LocationX, PhoenixDriveConstants.BackLeft.LocationY), + new Translation2d(PhoenixDriveConstants.BackRight.LocationX, PhoenixDriveConstants.BackRight.LocationY)); + } } diff --git a/src/main/java/frc/robot/constants/ConversionConstants.java b/src/main/java/frc/robot/constants/ConversionConstants.java index 3acb6e4..79bae18 100644 --- a/src/main/java/frc/robot/constants/ConversionConstants.java +++ b/src/main/java/frc/robot/constants/ConversionConstants.java @@ -1,12 +1,12 @@ package frc.robot.constants; public class ConversionConstants { - public double kRadiansPerSecondToRPM = 60.0 / (2.0 * Math.PI); - public double kRPMToRadiansPerSecond = 1.0 / kRadiansPerSecondToRPM; + public static double kRadiansPerSecondToRPM = 60.0 / (2.0 * Math.PI); + public static double kRPMToRadiansPerSecond = 1.0 / kRadiansPerSecondToRPM; - public double kSecondsToMinutes = 1.0 / 60.0; - public double kMinutesToSeconds = 60.0; + public static double kSecondsToMinutes = 1.0 / 60.0; + public static double kMinutesToSeconds = 60.0; - public double kDegreesToRadians = Math.PI / 180.0; - public double kRadiansToDegrees = 180.0 / Math.PI; + public static double kDegreesToRadians = Math.PI / 180.0; + public static double kRadiansToDegrees = 180.0 / Math.PI; } diff --git a/src/main/java/frc/robot/constants/DriverConstantsSchema.java b/src/main/java/frc/robot/constants/DriverConstantsSchema.java index 9deb6d1..660ad91 100644 --- a/src/main/java/frc/robot/constants/DriverConstantsSchema.java +++ b/src/main/java/frc/robot/constants/DriverConstantsSchema.java @@ -1,5 +1,5 @@ package frc.robot.constants; public class DriverConstantsSchema { - public static double joystickDeadband = 0.16; + public double joystickDeadband = 0.16; } diff --git a/src/main/java/frc/robot/constants/FeatureFlagsSchema.java b/src/main/java/frc/robot/constants/FeatureFlagsSchema.java index c76e266..9a6455a 100644 --- a/src/main/java/frc/robot/constants/FeatureFlagsSchema.java +++ b/src/main/java/frc/robot/constants/FeatureFlagsSchema.java @@ -1,13 +1,13 @@ package frc.robot.constants; public class FeatureFlagsSchema { - public static boolean runDrive = true; - public static boolean runVision = false; + public boolean runDrive = true; + public boolean runVision = false; // NOTE: A featureflag "runLocalizer" was removed from here recently. // TODO: Figure out if we need this and add it back if necessary. - public static boolean runLocalizer = false; - public static boolean runIntake = true; - public static boolean runScoring = true; - public static boolean runLEDS = true; - public static boolean runOrchestra = true; + public boolean runLocalizer = false; + public boolean runIntake = true; + public boolean runScoring = true; + public boolean runLEDS = true; + public boolean runOrchestra = true; } diff --git a/src/main/java/frc/robot/constants/LEDConstants.java b/src/main/java/frc/robot/constants/LEDConstants.java index 2ccd44b..5bd36d6 100644 --- a/src/main/java/frc/robot/constants/LEDConstants.java +++ b/src/main/java/frc/robot/constants/LEDConstants.java @@ -2,7 +2,7 @@ public class LEDConstants { - public int ledPort = 0; + public static int ledPort = 0; - public int ledLength = 30; + public static int ledLength = 30; } diff --git a/src/main/java/frc/robot/constants/ModeConstants.java b/src/main/java/frc/robot/constants/ModeConstants.java index 94c6b36..55583aa 100644 --- a/src/main/java/frc/robot/constants/ModeConstants.java +++ b/src/main/java/frc/robot/constants/ModeConstants.java @@ -3,7 +3,7 @@ import frc.robot.Robot; public class ModeConstants { - public enum Mode { + public static enum Mode { REAL, SIM, REPLAY @@ -11,7 +11,7 @@ public enum Mode { // Whether sim should be treated as sim or replay mode. // Will automatically be overridden by Mode.REAL if running on real hardware. - public Mode simMode = Mode.SIM; // Mode.SIM or Mode.REPLAY + public static Mode simMode = Mode.SIM; // Mode.SIM or Mode.REPLAY - public Mode currentMode = Robot.isReal() ? Mode.REAL : simMode; + public static Mode currentMode = Robot.isReal() ? Mode.REAL : simMode; } diff --git a/src/main/java/frc/robot/constants/PhoenixDriveConstants.java b/src/main/java/frc/robot/constants/PhoenixDriveConstantsSchema.java similarity index 65% rename from src/main/java/frc/robot/constants/PhoenixDriveConstants.java rename to src/main/java/frc/robot/constants/PhoenixDriveConstantsSchema.java index 8e64331..008ecf3 100644 --- a/src/main/java/frc/robot/constants/PhoenixDriveConstants.java +++ b/src/main/java/frc/robot/constants/PhoenixDriveConstantsSchema.java @@ -5,13 +5,16 @@ import com.ctre.phoenix6.mechanisms.swerve.SwerveModule.ClosedLoopOutputType; import com.ctre.phoenix6.mechanisms.swerve.SwerveModuleConstants; import com.ctre.phoenix6.mechanisms.swerve.SwerveModuleConstants.SteerFeedbackType; + +import coppercore.parameter_tools.JSONExclude; + 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 class PhoenixDriveConstants { - public enum AlignTarget { +public class PhoenixDriveConstantsSchema { + public enum AlignTarget { NONE, AMP, SPEAKER, @@ -26,9 +29,24 @@ public enum AlignTarget { // Both sets of gains need to be tuned to your individual robot. + public double steerKP = 150; + public double steerKI = 50; + public double steerKD = 0.2; + public double steerKS = 0.25; + public double steerKV = 1.5; + public double steerKA = 0; + + public double driveKP = 0.1; + public double driveKI = 12.0; + public double driveKD = 0.0; + public double driveKS = 0.1; + public double driveKV = 0.12; + public double driveKA = 0.01; + + // The steer motor uses any SwerveModule.SteerRequestType control request with the // output type specified by SwerveModuleConstants.SteerMotorClosedLoopOutput - public Slot0Configs steerGains = + @JSONExclude public Slot0Configs steerGains = new Slot0Configs() .withKP(150) .withKI(50) @@ -38,7 +56,7 @@ public enum AlignTarget { .withKA(0); // When using closed-loop control, the drive motor uses the control // output type specified by SwerveModuleConstants.DriveMotorClosedLoopOutput - public Slot0Configs driveGains = + @JSONExclude public Slot0Configs driveGains = new Slot0Configs() .withKP(0.1) .withKI(12.0) @@ -54,12 +72,12 @@ public enum AlignTarget { // The closed-loop output type to use for the steer motors; // This affects the PID/FF gains for the steer motors - private ClosedLoopOutputType steerClosedLoopOutput = ClosedLoopOutputType.Voltage; + @JSONExclude public 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 ClosedLoopOutputType driveClosedLoopOutput = ClosedLoopOutputType.Voltage; + @JSONExclude public ClosedLoopOutputType driveClosedLoopOutput = ClosedLoopOutputType.Voltage; - private double kSlipCurrentA = 80; + public double kSlipCurrentA = 80; public double kSpeedAt12VoltsMps = 5.02; // 5.21 OR 5.02 public double maxSpeedMetPerSec = 6; @@ -71,29 +89,29 @@ public enum AlignTarget { // Every 1 rotation of the azimuth results in kCoupleRatio drive motor turns; // This may need to be tuned to your individual robot - private double kCoupleRatio = 3.5714285714285716; + public double kCoupleRatio = 3.5714285714285716; - private double kDriveGearRatio = 5.142857; - private double kSteerGearRatio = 25; - private double kWheelRadiusInches = 2; + public double kDriveGearRatio = 5.142857; + public double kSteerGearRatio = 25; + public double kWheelRadiusInches = 2; - private boolean kInvertLeftSide = false; - private boolean kInvertRightSide = true; + public boolean kInvertLeftSide = false; + public boolean kInvertRightSide = true; - private String kCANbusName = "Canivore"; - private int kPigeonId = 20; + public String kCANbusName = "Canivore"; + public int kPigeonId = 20; // These are only used for simulation - private double kSteerInertia = 0.00001; - private double kDriveInertia = 0.001; + public double kSteerInertia = 0.00001; + public double kDriveInertia = 0.001; // Simulated voltage necessary to overcome friction - private double kSteerFrictionVoltage = 0.25; - private double kDriveFrictionVoltage = 0.25; + public double kSteerFrictionVoltage = 0.25; + public double kDriveFrictionVoltage = 0.25; - public SwerveDrivetrainConstants DrivetrainConstants = + @JSONExclude public SwerveDrivetrainConstants DrivetrainConstants = new SwerveDrivetrainConstants().withPigeon2Id(kPigeonId).withCANbusName(kCANbusName); - private SwerveModuleConstantsFactory ConstantCreator = + @JSONExclude public SwerveModuleConstantsFactory ConstantCreator = new SwerveModuleConstantsFactory() .withDriveMotorGearRatio(kDriveGearRatio) .withSteerMotorGearRatio(kSteerGearRatio) @@ -115,50 +133,50 @@ public enum AlignTarget { public double kSimLoopPeriod = 0.005; // Front Left - private int kFrontLeftDriveMotorId = 7; - private int kFrontLeftSteerMotorId = 8; - private int kFrontLeftEncoderId = 9; - private double kFrontLeftEncoderOffset = -0.267822265625; - private boolean kFrontLeftSteerInvert = false; + public int kFrontLeftDriveMotorId = 7; + public int kFrontLeftSteerMotorId = 8; + public int kFrontLeftEncoderId = 9; + public double kFrontLeftEncoderOffset = -0.267822265625; + public boolean kFrontLeftSteerInvert = false; - private double kFrontLeftXPosInches = 10.75; - private double kFrontLeftYPosInches = 11.5; + public double kFrontLeftXPosInches = 10.75; + public double kFrontLeftYPosInches = 11.5; // Front Right - private int kFrontRightDriveMotorId = 1; - private int kFrontRightSteerMotorId = 2; - private int kFrontRightEncoderId = 12; - private double kFrontRightEncoderOffset = -0.228271484375; - private boolean kFrontRightSteerInvert = false; + public int kFrontRightDriveMotorId = 1; + public int kFrontRightSteerMotorId = 2; + public int kFrontRightEncoderId = 12; + public double kFrontRightEncoderOffset = -0.228271484375; + public boolean kFrontRightSteerInvert = false; - private double kFrontRightXPosInches = 10.75; - private double kFrontRightYPosInches = -11.5; + public double kFrontRightXPosInches = 10.75; + public double kFrontRightYPosInches = -11.5; // Back Left - private int kBackLeftDriveMotorId = 5; - private int kBackLeftSteerMotorId = 6; - private int kBackLeftEncoderId = 10; - private double kBackLeftEncoderOffset = .084716796875; - private boolean kBackLeftSteerInvert = false; - private boolean kBackLeftDriveInvert = true; + public int kBackLeftDriveMotorId = 5; + public int kBackLeftSteerMotorId = 6; + public int kBackLeftEncoderId = 10; + public double kBackLeftEncoderOffset = .084716796875; + public boolean kBackLeftSteerInvert = false; + public boolean kBackLeftDriveInvert = true; - private double kBackLeftXPosInches = -10.75; - private double kBackLeftYPosInches = 11.5; + public double kBackLeftXPosInches = -10.75; + public double kBackLeftYPosInches = 11.5; // Back Right - private int kBackRightDriveMotorId = 3; - private int kBackRightSteerMotorId = 4; - private int kBackRightEncoderId = 11; - private double kBackRightEncoderOffset = 0.39990234375; - private boolean kBackRightSteerInvert = true; + public int kBackRightDriveMotorId = 3; + public int kBackRightSteerMotorId = 4; + public int kBackRightEncoderId = 11; + public double kBackRightEncoderOffset = 0.39990234375; + public boolean kBackRightSteerInvert = true; - private double kBackRightXPosInches = -10.75; - private double kBackRightYPosInches = -11.5; + public double kBackRightXPosInches = -10.75; + public double kBackRightYPosInches = -11.5; public double kModuleRadiusMeters = Units.inchesToMeters(Math.hypot(kFrontLeftXPosInches, kFrontLeftYPosInches)); - public SwerveModuleConstants FrontLeft = + @JSONExclude public SwerveModuleConstants FrontLeft = ConstantCreator.createModuleConstants( kFrontLeftSteerMotorId, kFrontLeftDriveMotorId, @@ -168,7 +186,7 @@ public enum AlignTarget { Units.inchesToMeters(kFrontLeftYPosInches), kInvertLeftSide) .withSteerMotorInverted(kFrontLeftSteerInvert); - public SwerveModuleConstants FrontRight = + @JSONExclude public SwerveModuleConstants FrontRight = ConstantCreator.createModuleConstants( kFrontRightSteerMotorId, kFrontRightDriveMotorId, @@ -178,7 +196,7 @@ public enum AlignTarget { Units.inchesToMeters(kFrontRightYPosInches), kInvertRightSide) .withSteerMotorInverted(kFrontRightSteerInvert); - public SwerveModuleConstants BackLeft = + @JSONExclude public SwerveModuleConstants BackLeft = ConstantCreator.createModuleConstants( kBackLeftSteerMotorId, kBackLeftDriveMotorId, @@ -189,7 +207,7 @@ public enum AlignTarget { kInvertLeftSide) .withSteerMotorInverted(kBackLeftSteerInvert) .withDriveMotorInverted(kBackLeftDriveInvert); - public SwerveModuleConstants BackRight = + @JSONExclude public SwerveModuleConstants BackRight = ConstantCreator.createModuleConstants( kBackRightSteerMotorId, kBackRightDriveMotorId, @@ -200,7 +218,7 @@ public enum AlignTarget { kInvertRightSide) .withSteerMotorInverted(kBackRightSteerInvert); - public SwerveDriveKinematics kinematics = + @JSONExclude public SwerveDriveKinematics kinematics = new SwerveDriveKinematics( new Translation2d(FrontLeft.LocationX, FrontLeft.LocationY), new Translation2d(FrontLeft.LocationX, FrontRight.LocationY), diff --git a/src/main/java/frc/robot/constants/ScoringConstantsSchema.java b/src/main/java/frc/robot/constants/ScoringConstantsSchema.java index 092cd97..8b3cdf0 100644 --- a/src/main/java/frc/robot/constants/ScoringConstantsSchema.java +++ b/src/main/java/frc/robot/constants/ScoringConstantsSchema.java @@ -3,97 +3,101 @@ import edu.wpi.first.math.util.Units; import java.util.HashMap; +import coppercore.parameter_tools.JSONExclude; + public class ScoringConstantsSchema { - public static double aimerkP = 100.0; - public static double aimerkI = 2.0; // 5.0 - public static double aimerkD = 0.0; + public double aimerkP = 100.0; + public double aimerkI = 2.0; // 5.0 + public double aimerkD = 0.0; - public static double aimerkS = 0.3; // 0.25; - public static double aimerkG = 0.955; // 0.2; - public static double aimerkV = 0.0; - public static double aimerkA = 0.0; + public double aimerkS = 0.3; // 0.25; + public double aimerkG = 0.955; // 0.2; + public double aimerkV = 0.0; + public double aimerkA = 0.0; - public static double shooterkP = 0.05; - public static double shooterkI = 1; - public static double shooterkD = 0.0; + public double shooterkP = 0.05; + public double shooterkI = 1; + public double shooterkD = 0.0; - public static double shooterkS = 0.0; // TODO: Find Imperically - public static double shooterkV = 0.005; - public static double shooterkA = 0.0; + public double shooterkS = 0.0; // TODO: Find Imperically + public double shooterkV = 0.005; + public double shooterkA = 0.0; - public static int aimerMotorId = 9; + public int aimerMotorId = 9; - public static int kickerMotorId = 5; // 1 + public int kickerMotorId = 5; // 1 // TODO: REPLACE THIS WHEN THE ACTUAL SHOOTER IO IS MERGED - public static int shooterLeftMotorId = 10; - public static int shooterRightMotorId = 11; + public int shooterLeftMotorId = 10; + public int shooterRightMotorId = 11; + + public double shooterCurrentLimit = 120; + public double kickerCurrentLimit = 120; + public double aimerCurrentLimit = 60; - public static double shooterCurrentLimit = 120; - public static double kickerCurrentLimit = 120; - public static double aimerCurrentLimit = 60; + public int aimerEncoderId = 13; - public static int aimerEncoderId = 13; + public double aimerEncoderOffset = -0.087402; // Armencoder is zeroed - public static double aimerEncoderOffset = -0.087402; // Armencoder is zeroed + public double aimerEncoderToMechanismRatio = 1.0; + public double aimerRotorToSensorRatio = 90.0; - public static double aimerEncoderToMechanismRatio = 1.0; - public static double aimerRotorToSensorRatio = 90.0; + public double kickerIntakeVolts = 2.0; - public static double kickerIntakeVolts = 2.0; + public double aimerStaticOffset = 0.1; // public double aimPositionTolerance = 0.003; - public static double aimerAcceleration = 3.5; - public static double aimerCruiseVelocity = 0.8; + public double aimerAcceleration = 3.5; + public double aimerCruiseVelocity = 0.8; - public static double shooterLowerVelocityMarginRPM = 50; - public static double shooterUpperVelocityMarginRPM = 150; - public static double aimAngleMarginRotations = Units.degreesToRotations(1); - public static double aimAngleVelocityMargin = 2.0; + public double shooterLowerVelocityMarginRPM = 50; + public double shooterUpperVelocityMarginRPM = 150; + public double aimAngleMarginRotations = Units.degreesToRotations(1); + public double aimAngleVelocityMargin = 2.0; - public static double minDistanceAlignmentNeeded = 1.3; // TODO: Tune this value + public double minDistanceAlignmentNeeded = 1.3; // TODO: Tune this value - public static double intakeAngleToleranceRotations = 0.05; // Todo: tune this value + public double intakeAngleToleranceRotations = 0.05; // Todo: tune this value - public static double aimerAmpPositionRot = 0.145; + public double aimerAmpPositionRot = 0.145; - public static double aimMaxAngleRotations = 0.361328 - 0.184570; - public static double aimMinAngleRotations = -0.212285; - public static double aimLockVoltage = -0.5; + public double aimMaxAngleRotations = 0.361328 - 0.184570; + public double aimMinAngleRotations = -0.212285; + public double aimLockVoltage = -0.5; - public static double aimAngleTolerance = 0.015; + public double aimAngleTolerance = 0.015; - public static double ampAimerAngleRotations = 0.14; + public double ampAimerAngleRotations = 0.14; - public static double maxAimIntake = 0.0; - public static double minAimIntake = 0.0; + public double maxAimIntake = 0.0; + public double minAimIntake = 0.0; - public static double shooterOffsetAdjustment = 0.6; + public double shooterOffsetAdjustment = 0.6; - public static double maxElevatorPosition = 0.45; - public static double maxAimAngleElevatorLimit = Math.PI / 2; + public double maxElevatorPosition = 0.45; + public double maxAimAngleElevatorLimit = Math.PI / 2; - public static double[] aimerDistance; - public static double[] aimerPosition; + public double[] aimerDistance; + public double[] aimerPosition; - public static double passLocationRot = -0.14; - public static double passAngleToleranceRot = 0.005; - public static double passShooterRpm = 2800.0; + public double passLocationRot = -0.14; + public double passAngleToleranceRot = 0.005; + public double passShooterRpm = 2800.0; - public static double aimerOffset = 0.0; + public double aimerOffset = 0.0; - public static double[] shooterDistance; - public static double[] shooterRPM; + public double[] shooterDistance; + public double[] shooterRPM; - public static double[] timeToGoalDistance; - public static double[] timeToGoal; + public double[] timeToGoalDistance; + public double[] timeToGoal; - public static double[] aimerToleranceDistance; - public static double[] aimerTolerance; + public double[] aimerToleranceDistance; + public double[] aimerTolerance; - public static HashMap aimerMap; - public static HashMap shooterMap; - public static HashMap timeToGoalMap; - public static HashMap aimerToleranceMap; + @JSONExclude public HashMap aimerMap; + @JSONExclude public HashMap shooterMap; + @JSONExclude public HashMap timeToGoalMap; + @JSONExclude public HashMap aimerToleranceMap; } diff --git a/src/main/java/frc/robot/constants/SimConstants.java b/src/main/java/frc/robot/constants/SimConstants.java index bf2d2c6..8097e1d 100644 --- a/src/main/java/frc/robot/constants/SimConstants.java +++ b/src/main/java/frc/robot/constants/SimConstants.java @@ -1,5 +1,5 @@ package frc.robot.constants; public class SimConstants { - public double loopTime = 0.02; + public static double loopTime = 0.02; } diff --git a/src/main/java/frc/robot/subsystems/drive/PhoenixDrive.java b/src/main/java/frc/robot/subsystems/drive/PhoenixDrive.java index f68af5b..afe1bd8 100644 --- a/src/main/java/frc/robot/subsystems/drive/PhoenixDrive.java +++ b/src/main/java/frc/robot/subsystems/drive/PhoenixDrive.java @@ -32,9 +32,8 @@ import edu.wpi.first.wpilibj2.command.Subsystem; import edu.wpi.first.wpilibj2.command.sysid.SysIdRoutine; import frc.robot.constants.FieldConstants; +import frc.robot.constants.PhoenixDriveConstantsSchema.AlignTarget; import frc.robot.constants.ConstantsLoader; -import frc.robot.constants.PhoenixDriveConstants; -import frc.robot.constants.PhoenixDriveConstants.AlignTarget; import java.util.Optional; import org.littletonrobotics.junction.Logger; @@ -62,9 +61,9 @@ public enum SysIdRoutineType { private Rotation2d goalRotation = new Rotation2d(); private PIDController thetaController = new PIDController( - PhoenixDriveConstants.alignmentkP, - PhoenixDriveConstants.alignmentkI, - PhoenixDriveConstants.alignmentkD); + ConstantsLoader.PhoenixDriveConstants.alignmentkP, + ConstantsLoader.PhoenixDriveConstants.alignmentkI, + ConstantsLoader.PhoenixDriveConstants.alignmentkD); private final SwerveRequest.SysIdSwerveTranslation TranslationCharacterization = new SwerveRequest.SysIdSwerveTranslation(); @@ -131,7 +130,7 @@ public PhoenixDrive( } thetaController.enableContinuousInput(-Math.PI, Math.PI); - thetaController.setTolerance(PhoenixDriveConstants.alignToleranceRadians); + thetaController.setTolerance(ConstantsLoader.PhoenixDriveConstants.alignToleranceRadians); CommandScheduler.getInstance().registerSubsystem(this); } @@ -145,7 +144,7 @@ public PhoenixDrive( } thetaController.enableContinuousInput(-Math.PI, Math.PI); - thetaController.setTolerance(PhoenixDriveConstants.alignToleranceRadians); + thetaController.setTolerance(ConstantsLoader.PhoenixDriveConstants.alignToleranceRadians); CommandScheduler.getInstance().registerSubsystem(this); } @@ -167,7 +166,7 @@ public void configurePathPlanner() { new HolonomicPathFollowerConfig( new PIDConstants(0.1), new PIDConstants(0.1, 0, 0), - PhoenixDriveConstants.kSpeedAt12VoltsMps, + ConstantsLoader.PhoenixDriveConstants.kSpeedAt12VoltsMps, driveBaseRadius, new ReplanningConfig(false, false)), () -> DriverStation.getAlliance().get() == Alliance.Red, @@ -199,7 +198,7 @@ private void startSimThread() { updateSimState(deltaTime, RobotController.getBatteryVoltage()); }); - simNotifier.startPeriodic(PhoenixDriveConstants.kSimLoopPeriod); + simNotifier.startPeriodic(ConstantsLoader.PhoenixDriveConstants.kSimLoopPeriod); } public ChassisSpeeds getCurrentSpeeds() { diff --git a/src/main/java/frc/robot/subsystems/localization/CameraContainerSim.java b/src/main/java/frc/robot/subsystems/localization/CameraContainerSim.java index 4627d00..1676fb6 100644 --- a/src/main/java/frc/robot/subsystems/localization/CameraContainerSim.java +++ b/src/main/java/frc/robot/subsystems/localization/CameraContainerSim.java @@ -7,7 +7,7 @@ import edu.wpi.first.math.kinematics.SwerveModuleState; import edu.wpi.first.wpilibj.Timer; import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; -import frc.robot.constants.PhoenixDriveConstants; +import frc.robot.constants.ConstantsLoader; // import frc.robot.constants.DriveConstants; import frc.robot.constants.VisionConstants; import frc.robot.constants.VisionConstants.CameraParams; @@ -88,7 +88,7 @@ private void updateOdometry() { .getRadians())); } - Twist2d twist = PhoenixDriveConstants.kinematics.toTwist2d(deltas); + Twist2d twist = ConstantsLoader.PhoenixDriveConstants.kinematics.toTwist2d(deltas); latestOdometryPose = latestOdometryPose.exp(twist); Logger.recordOutput("Vision/GroundTruth", latestOdometryPose); diff --git a/src/main/java/frc/robot/subsystems/scoring/ScoringSubsystem.java b/src/main/java/frc/robot/subsystems/scoring/ScoringSubsystem.java index 2294bc3..216a53d 100644 --- a/src/main/java/frc/robot/subsystems/scoring/ScoringSubsystem.java +++ b/src/main/java/frc/robot/subsystems/scoring/ScoringSubsystem.java @@ -111,15 +111,15 @@ public ScoringSubsystem(ShooterIO shooterIo, AimerIO aimerIo) { this.shooterIo = shooterIo; this.aimerIo = aimerIo; - shooterInterpolated = new InterpolateDouble(ConstantsLoader.ScoringConstants.getShooterMap()); + shooterInterpolated = new InterpolateDouble(ConstantsLoader.ScoringConstants.shooterMap); aimerInterpolated = new InterpolateDouble( - ConstantsLoader.ScoringConstants.getAimerMap(), + ConstantsLoader.ScoringConstants.aimerMap, ConstantsLoader.ScoringConstants.aimMinAngleRotations, ConstantsLoader.ScoringConstants.aimMaxAngleRotations); - aimerAngleTolerance = new InterpolateDouble(ConstantsLoader.ScoringConstants.aimerToleranceTable()); + aimerAngleTolerance = new InterpolateDouble(ConstantsLoader.ScoringConstants.aimerToleranceMap); if (ModeConstants.currentMode == Mode.SIM) { mechanism = new Mechanism2d(2.2, 2.0);