From fc54d4ce2f6c219198ad9c9bedf876eec54caf33 Mon Sep 17 00:00:00 2001 From: caTrixLiu Date: Wed, 28 Aug 2024 21:33:52 +0800 Subject: [PATCH] moved configs to separate classes and use tuner configs for wheels calibrations --- src/main/java/frc/robot/Constants.java | 264 ------------------ src/main/java/frc/robot/Robot.java | 1 + src/main/java/frc/robot/RobotContainer.java | 60 ++-- .../commands/drive/ChassisFaceToRotation.java | 4 +- .../commands/drive/CustomFollowPath.java | 2 +- .../frc/robot/commands/drive/DriveToPose.java | 15 +- .../robot/commands/drive/FollowPathPP.java | 8 +- .../robot/commands/drive/JoystickDrive.java | 10 +- .../drive/JoystickDriveAndAimAtTarget.java | 5 +- .../java/frc/robot/constants/Constants.java | 60 ++++ .../robot/constants/DriveControlLoops.java | 50 ++++ .../robot/constants/DriveTrainConstants.java | 104 +++++++ .../frc/robot/constants/FieldConstants.java | 71 +++++ .../frc/robot/constants/JoystickConfigs.java | 28 ++ .../java/frc/robot/constants/LogPaths.java | 7 + .../frc/robot/constants/TunerConstants.java | 175 ++++++++++++ .../frc/robot/subsystems/MapleSubsystem.java | 5 +- .../drive/HolonomicDriveSubsystem.java | 17 +- .../subsystems/drive/IO/GyroIOPigeon2.java | 29 +- .../robot/subsystems/drive/IO/GyroIOSim.java | 9 +- .../robot/subsystems/drive/IO/ModuleIO.java | 1 + .../subsystems/drive/IO/ModuleIOSim.java | 23 +- ...duleIOSparkMax.java => ModuleIOSpark.java} | 11 +- ...oduleIOTalonFX.java => ModuleIOTalon.java} | 33 +-- .../subsystems/drive/IO/OdometryThread.java | 11 +- .../subsystems/drive/OdometryThreadReal.java | 20 +- .../robot/subsystems/drive/SwerveDrive.java | 45 ++- .../robot/subsystems/drive/SwerveModule.java | 6 +- .../robot/subsystems/led/LEDStatusLight.java | 2 +- .../vision/apriltags/AprilTagVision.java | 4 +- .../apriltags/MapleMultiTagPoseEstimator.java | 6 +- .../robot/tests/WheelsCalibrationCTRE.java | 172 ------------ .../Objects/Crescendo2024FieldObjects.java | 4 +- .../CompetitionFieldSimulation.java | 6 +- .../Crescendo2024FieldSimulation.java | 2 +- .../HolonomicChassisSimulation.java | 51 ++-- .../Simulations/OpponentRobotSimulation.java | 16 +- .../Simulations/SwerveDriveSimulation.java | 34 +-- .../robot/utils/MapleJoystickDriveInput.java | 14 +- 39 files changed, 724 insertions(+), 661 deletions(-) delete mode 100644 src/main/java/frc/robot/Constants.java create mode 100644 src/main/java/frc/robot/constants/Constants.java create mode 100644 src/main/java/frc/robot/constants/DriveControlLoops.java create mode 100644 src/main/java/frc/robot/constants/DriveTrainConstants.java create mode 100644 src/main/java/frc/robot/constants/FieldConstants.java create mode 100644 src/main/java/frc/robot/constants/JoystickConfigs.java create mode 100644 src/main/java/frc/robot/constants/LogPaths.java create mode 100644 src/main/java/frc/robot/constants/TunerConstants.java rename src/main/java/frc/robot/subsystems/drive/IO/{ModuleIOSparkMax.java => ModuleIOSpark.java} (96%) rename src/main/java/frc/robot/subsystems/drive/IO/{ModuleIOTalonFX.java => ModuleIOTalon.java} (79%) delete mode 100644 src/main/java/frc/robot/tests/WheelsCalibrationCTRE.java diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java deleted file mode 100644 index 4a6e320..0000000 --- a/src/main/java/frc/robot/Constants.java +++ /dev/null @@ -1,264 +0,0 @@ -// Original Source: -// https://github.com/Mechanical-Advantage/AdvantageKit/tree/main/example_projects/advanced_swerve_drive/src/main, Copyright 2021-2024 FRC 6328 -// Modified by 5516 Iron Maple https://github.com/Shenzhen-Robotics-Alliance/ - -package frc.robot; - -import com.pathplanner.lib.path.PathPlannerPath; -import edu.wpi.first.apriltag.AprilTagFieldLayout; -import edu.wpi.first.apriltag.AprilTagFields; -import edu.wpi.first.math.controller.SimpleMotorFeedforward; -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.geometry.Translation3d; -import edu.wpi.first.math.system.plant.DCMotor; -import edu.wpi.first.math.trajectory.TrapezoidProfile; -import edu.wpi.first.math.util.Units; -import edu.wpi.first.wpilibj.DriverStation; -import frc.robot.utils.CustomPIDs.MaplePIDController; -import org.photonvision.PhotonPoseEstimator; - -import java.util.Optional; -import java.util.function.Supplier; - -/** - * The Constants class provides a convenient place for teams to hold robot-wide numerical or boolean - * constants. This class should not be used for any other purpose. All constants should be declared - * globally (i.e. public static). Do not put anything functional in this class. - * - *

It is advised to statically import this class (or one of its inner classes) wherever the - * constants are needed, to reduce verbosity. - */ -public final class Constants { - public enum RobotMode { - /** - * Running on a real robot. - */ - REAL, - - /** - * Running a physics simulator. - */ - SIM, - - /** - * Replaying from a log file. - */ - REPLAY - } - - public static final String ROBOT_NAME = "5516-2024-OffSeason"; - - // avoid typo errors - public static final class LogConfigs { - public static final String - SYSTEM_PERFORMANCE_PATH = "SystemPerformance/", - PHYSICS_SIMULATION_PATH = "MaplePhysicsSimulation/", - APRIL_TAGS_VISION_PATH = "Vision/AprilTags/"; - } - - public static final class CrescendoField2024Constants { - public static final double FIELD_WIDTH = 16.54; - public static final double FIELD_HEIGHT = 8.21; - - public static final Translation3d SPEAKER_POSE_BLUE = new Translation3d(0.1, 5.55, 2.2); - - public static final Supplier SPEAKER_POSITION_SUPPLIER = () -> toCurrentAllianceTranslation(SPEAKER_POSE_BLUE.toTranslation2d()); - } - - public static final class DriverJoystickConfigs { - public static final double nonUsageTimeResetWheels = 1; - - public static final double deadBandWhenOtherAxisEmpty = 0.02; - public static final double deadBandWhenOtherAxisFull = 0.1; - public static final double linearSpeedInputExponent = 1.6; - public static final double rotationSpeedInputExponent = 2; - - /** the amount of time that the chassis needs to accelerate to the maximum linear velocity */ - public static final double linearAccelerationSmoothOutSeconds = 0.1; - /** the amount of time that the chassis needs to accelerate to the maximum angular velocity */ - public static final double angularAccelerationSmoothOutSeconds = 0.1; - - public static final double timeActivateRotationMaintenanceAfterNoRotationalInputSeconds = 0.3; - } - - public static final class SwerveDriveChassisConfigs { - public enum SwerveDriveType { - REV, - CTRE_ON_RIO, - CTRE_ON_CANIVORE - } - public static final SwerveDriveType SWERVE_DRIVE_TYPE = SwerveDriveType.CTRE_ON_CANIVORE; - - public static final String CHASSIS_CANBUS = "ChassisCanivore"; - - public static final int ODOMETRY_CACHE_CAPACITY = 10; - public static final double ODOMETRY_FREQUENCY = 250; - public static final double ODOMETRY_WAIT_TIMEOUT_SECONDS = 0.02; - - public static final MaplePIDController.MaplePIDConfig chassisRotationalPIDConfig = new MaplePIDController.MaplePIDConfig( - Math.toRadians(360), - Math.toRadians(60), - 0.02, - Math.toRadians(3), - 0.05, - true, - 0 - ); - public static final TrapezoidProfile.Constraints chassisRotationalConstraints = new TrapezoidProfile.Constraints(Math.toRadians(540), Math.toRadians(540) / 0.6); - - public static final MaplePIDController.MaplePIDConfig chassisTranslationPIDConfig = new MaplePIDController.MaplePIDConfig( - 3, - 0.6, - 0.01, - 0.03, - 0.3, - false, - 0 - ); - - public static final MaplePIDController.MaplePIDConfig chassisTranslationPIDConfigPathFollowing = new MaplePIDController.MaplePIDConfig( - 2, - 1.2, - 0, - 0.03, - 0, - false, - 0 - ); - } - - public static final class SwerveModuleConfigs { - public static final double WHEEL_RADIUS = Units.inchesToMeters(2.0); - public static final MaplePIDController.MaplePIDConfig STEER_CLOSE_LOOP = new MaplePIDController.MaplePIDConfig( - 0.5, - Math.toRadians(90), - 0.02, - Math.toRadians(1.5), - 0, - true, - 0 - ); - - public static final SimpleMotorFeedforward DRIVE_OPEN_LOOP = new SimpleMotorFeedforward(0.01, 2.7); - public static final MaplePIDController.MaplePIDConfig DRIVE_CLOSE_LOOP = new MaplePIDController.MaplePIDConfig( - 5, - 2, - 0, - 0, - 0, - false, 0 - ); - - public static final double STEERING_CURRENT_LIMIT = 20; - public static final double DRIVING_CURRENT_LIMIT = 40; - } - - public static final class DriveTrainPhysicsSimulationConstants { - public static final int SIM_ITERATIONS_PER_ROBOT_PERIOD = 5; - - /* Swerve Module Simulation */ - public static final double DRIVE_MOTOR_FREE_FINAL_SPEED_RPM = 4800; - public static final DCMotor - DRIVE_MOTOR = DCMotor.getKrakenX60(1), - STEER_MOTOR = DCMotor.getFalcon500(1); - public static final double DRIVE_WHEEL_ROTTER_INERTIA = 0.012; - public static final double STEER_INERTIA = 0.015; - public static final double STEER_GEAR_RATIO = 150.0 / 7.0; - - public static final double FLOOR_FRICTION_ACCELERATION_METERS_PER_SEC_SQ = 15; - public static final double MAX_ANGULAR_ACCELERATION_RAD_PER_SEC_SQ = Math.toRadians(1200); - public static final double TIME_CHASSIS_STOPS_ROTATING_NO_POWER_SEC = 0.3; - public static final double DEFAULT_ROBOT_MASS = 40; - public static final double DEFAULT_BUMPER_WIDTH_METERS = Units.inchesToMeters(34.5); - public static final double DEFAULT_BUMPER_LENGTH_METERS = Units.inchesToMeters(36); - - /* https://en.wikipedia.org/wiki/Friction#Coefficient_of_friction */ - public static final double ROBOT_BUMPER_COEFFICIENT_OF_FRICTION = 0.85; - /* https://en.wikipedia.org/wiki/Coefficient_of_restitution */ - public static final double ROBOT_BUMPER_COEFFICIENT_OF_RESTITUTION = 0.05; - - /* Gyro Sim */ - public static final double GYRO_ANGULAR_ACCELERATION_THRESHOLD_SKIDDING_RAD_PER_SEC_SQ = 100; - public static final double SKIDDING_AMOUNT_AT_THRESHOLD_RAD = Math.toRadians(1.2); - /* - * https://store.ctr-electronics.com/pigeon-2/ - * for a well-installed one with vibration reduction, only 0.4 degree - * but most teams just install it directly on the rigid chassis frame (including my team :D) - * so at least 1.2 degrees of drifting in 1 minutes for an average angular velocity of 60 degrees/second - * which is the average velocity during normal swerve-circular-offense - * */ - public static final double NORMAL_GYRO_DRIFT_IN_1_MIN_Std_Dev_RAD = Math.toRadians(1.2); - public static final double AVERAGE_VELOCITY_RAD_PER_SEC_DURING_TEST = Math.toRadians(60); - } - - public static final class VisionConfigs { - public static final AprilTagFieldLayout fieldLayout = AprilTagFields.kDefaultField.loadAprilTagLayoutField(); - public static final PhotonPoseEstimator.PoseStrategy poseEstimatorStrategy = PhotonPoseEstimator.PoseStrategy.AVERAGE_BEST_TARGETS; - /* default standard error for vision observation, if only one apriltag observed */ - public static final double - TRANSLATIONAL_STANDARD_ERROR_METERS_FOR_SINGLE_OBSERVATION = 0.6, - ROTATIONAL_STANDARD_ERROR_RADIANS_FOR_SINGLE_OBSERVATION = 0.5, - - // only do odometry calibration if translational standard error if it is not greater than - TRANSLATIONAL_STANDARD_ERROR_THRESHOLD = 0.5, - // only do gyro calibration if rotational standard error is very, very small - ROTATIONAL_STANDARD_ERROR_THRESHOLD = Math.toRadians(5), - - ODOMETRY_TRANSLATIONAL_STANDARD_ERROR_METERS = 0.02, - // we trust the IMU very much (recommend 0.1 for Pigeon2, 0.5 for NavX) - GYRO_ROTATIONAL_STANDARD_ERROR_RADIANS = Math.toRadians(0.1); - } - - public static Rotation2d toCurrentAllianceRotation(Rotation2d rotationAtBlueSide) { - final Rotation2d - yAxis = Rotation2d.fromDegrees(90), - differenceFromYAxisAtBlueSide = rotationAtBlueSide.minus(yAxis), - differenceFromYAxisNew = differenceFromYAxisAtBlueSide.times(isSidePresentedAsRed() ? -1:1); - return yAxis.rotateBy(differenceFromYAxisNew); - } - - public static Translation2d toCurrentAllianceTranslation(Translation2d translationAtBlueSide) { - if (isSidePresentedAsRed()) - return new Translation2d( - CrescendoField2024Constants.FIELD_WIDTH - translationAtBlueSide.getX(), - translationAtBlueSide.getY() - ); - return translationAtBlueSide; - } - - public static Translation3d toCurrentAllianceTranslation(Translation3d translation3dAtBlueSide) { - final Translation2d translation3dAtCurrentAlliance = toCurrentAllianceTranslation(translation3dAtBlueSide.toTranslation2d()); - if (isSidePresentedAsRed()) - return new Translation3d( - translation3dAtCurrentAlliance.getX(), - translation3dAtCurrentAlliance.getY(), - translation3dAtBlueSide.getZ() - ); - return translation3dAtBlueSide; - } - - public static Pose2d toCurrentAlliancePose(Pose2d poseAtBlueSide) { - return new Pose2d( - toCurrentAllianceTranslation(poseAtBlueSide.getTranslation()), - toCurrentAllianceRotation(poseAtBlueSide.getRotation()) - ); - } - - public static PathPlannerPath toCurrentAlliancePath(PathPlannerPath pathAtBlueAlliance) { - return isSidePresentedAsRed() ? pathAtBlueAlliance.flipPath() : pathAtBlueAlliance; - } - - public static boolean isSidePresentedAsRed() { - final Optional alliance = DriverStation.getAlliance(); - return alliance.isPresent() && alliance.get().equals(DriverStation.Alliance.Red); - } - - public static Rotation2d getDriverStationFacing() { - return switch (DriverStation.getAlliance().orElse(DriverStation.Alliance.Red)) { - case Red -> new Rotation2d(Math.PI); - case Blue -> new Rotation2d(0); - }; - } -} diff --git a/src/main/java/frc/robot/Robot.java b/src/main/java/frc/robot/Robot.java index a8c51e8..db021d7 100644 --- a/src/main/java/frc/robot/Robot.java +++ b/src/main/java/frc/robot/Robot.java @@ -6,6 +6,7 @@ import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.CommandScheduler; +import frc.robot.constants.Constants; import frc.robot.subsystems.MapleSubsystem; import org.littletonrobotics.junction.LogFileUtil; import org.littletonrobotics.junction.LoggedRobot; diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 3b40631..c81e7a8 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -21,29 +21,29 @@ import frc.robot.autos.ExampleAuto; import frc.robot.autos.PathPlannerAuto; import frc.robot.commands.drive.*; +import frc.robot.constants.Constants; +import frc.robot.constants.FieldConstants; +import frc.robot.constants.TunerConstants; import frc.robot.subsystems.MapleSubsystem; import frc.robot.subsystems.drive.*; import frc.robot.subsystems.drive.IO.GyroIOPigeon2; import frc.robot.subsystems.drive.IO.GyroIOSim; import frc.robot.subsystems.drive.IO.ModuleIOSim; -import frc.robot.subsystems.drive.IO.ModuleIOTalonFX; +import frc.robot.subsystems.drive.IO.ModuleIOTalon; import frc.robot.subsystems.led.LEDStatusLight; import frc.robot.subsystems.vision.apriltags.AprilTagVision; import frc.robot.subsystems.vision.apriltags.AprilTagVisionIOReal; import frc.robot.subsystems.vision.apriltags.ApriltagVisionIOSim; -import frc.robot.tests.*; import frc.robot.utils.CompetitionFieldUtils.CompetitionFieldVisualizer; import frc.robot.utils.CompetitionFieldUtils.Simulations.CompetitionFieldSimulation; import frc.robot.utils.CompetitionFieldUtils.Simulations.Crescendo2024FieldSimulation; import frc.robot.utils.CompetitionFieldUtils.Simulations.OpponentRobotSimulation; import frc.robot.utils.CompetitionFieldUtils.Simulations.SwerveDriveSimulation; -import frc.robot.utils.CustomConfigs.MapleConfigFile; import frc.robot.utils.CustomConfigs.PhotonCameraProperties; import frc.robot.utils.MapleJoystickDriveInput; import frc.robot.utils.MapleShooterOptimization; import org.littletonrobotics.junction.networktables.LoggedDashboardChooser; -import java.io.IOException; import java.util.List; import java.util.function.Supplier; @@ -85,17 +85,7 @@ public enum JoystickMode { * The container for the robot. Contains subsystems, OI devices, and commands. */ public RobotContainer() { - final MapleConfigFile chassisCalibrationFile; - try { - chassisCalibrationFile = MapleConfigFile.fromDeployedConfig( - "ChassisWheelsCalibration", - Constants.ROBOT_NAME - ); - } catch (IOException e) { - throw new RuntimeException(e); - } final List camerasProperties = PhotonCameraProperties.loadCamerasPropertiesFromConfig(Constants.ROBOT_NAME); - final MapleConfigFile.ConfigBlock chassisGeneralConfigBlock = chassisCalibrationFile.getBlock("GeneralInformation"); this.ledStatusLight = new LEDStatusLight(0, 155); switch (Robot.CURRENT_ROBOT_MODE) { @@ -103,12 +93,12 @@ public RobotContainer() { // Real robot, instantiate hardware IO implementations powerDistribution = new PowerDistribution(0, PowerDistribution.ModuleType.kCTRE); drive = new SwerveDrive( - new GyroIOPigeon2(), - new ModuleIOTalonFX(chassisCalibrationFile.getBlock("FrontLeft"), chassisGeneralConfigBlock), - new ModuleIOTalonFX(chassisCalibrationFile.getBlock("FrontRight"), chassisGeneralConfigBlock), - new ModuleIOTalonFX(chassisCalibrationFile.getBlock("BackLeft"), chassisGeneralConfigBlock), - new ModuleIOTalonFX(chassisCalibrationFile.getBlock("BackRight"), chassisGeneralConfigBlock), - chassisGeneralConfigBlock + SwerveDrive.DriveType.CTRE_ON_CANIVORE, + new GyroIOPigeon2(TunerConstants.DrivetrainConstants), + new ModuleIOTalon(TunerConstants.DrivetrainConstants, TunerConstants.FrontLeft, "FrontLeft"), + new ModuleIOTalon(TunerConstants.DrivetrainConstants, TunerConstants.FrontRight, "FrontRight"), + new ModuleIOTalon(TunerConstants.DrivetrainConstants, TunerConstants.BackLeft, "BackLeft"), + new ModuleIOTalon(TunerConstants.DrivetrainConstants, TunerConstants.BackRight, "BackRight") ); aprilTagVision = new AprilTagVision( @@ -124,18 +114,17 @@ public RobotContainer() { powerDistribution = new PowerDistribution(); // Sim robot, instantiate physics sim IO implementations final ModuleIOSim - frontLeft = new ModuleIOSim(chassisGeneralConfigBlock), - frontRight = new ModuleIOSim(chassisGeneralConfigBlock), - backLeft = new ModuleIOSim(chassisGeneralConfigBlock), - backRight = new ModuleIOSim(chassisGeneralConfigBlock); + frontLeft = new ModuleIOSim(), + frontRight = new ModuleIOSim(), + backLeft = new ModuleIOSim(), + backRight = new ModuleIOSim(); final GyroIOSim gyroIOSim = new GyroIOSim(); drive = new SwerveDrive( + SwerveDrive.DriveType.GENERIC, gyroIOSim, - frontLeft, frontRight, backLeft, backRight, - chassisGeneralConfigBlock + frontLeft, frontRight, backLeft, backRight ); final SwerveDriveSimulation driveSimulation = new SwerveDriveSimulation( - chassisGeneralConfigBlock, gyroIOSim, frontLeft, frontRight, backLeft, backRight, drive.kinematics, @@ -166,12 +155,12 @@ public RobotContainer() { powerDistribution = new PowerDistribution(); // Replayed robot, disable IO implementations drive = new SwerveDrive( + SwerveDrive.DriveType.GENERIC, (inputs) -> {}, (inputs) -> {}, (inputs) -> {}, (inputs) -> {}, - (inputs) -> {}, - chassisGeneralConfigBlock + (inputs) -> {} ); aprilTagVision = new AprilTagVision( @@ -221,12 +210,11 @@ private static LoggedDashboardChooser buildAutoChooser() { private static SendableChooser> buildTestsChooser() { final SendableChooser> testsChooser = new SendableChooser<>(); testsChooser.setDefaultOption("None", Commands::none); - testsChooser.addOption("Wheels Calibration", WheelsCalibrationCTRE::new); // TODO add your tests here (system identification and etc.) return testsChooser; } - private boolean isDSPresentedAsRed = Constants.isSidePresentedAsRed(); + private boolean isDSPresentedAsRed = FieldConstants.isSidePresentedAsRed(); private boolean isLeftHanded = true; private Command autonomousCommand = Commands.none(); private Auto previouslySelectedAuto = null; @@ -236,9 +224,9 @@ private static SendableChooser> buildTestsChooser() { * */ public void checkForCommandChanges() { final boolean isLeftHandedSelected = !JoystickMode.RIGHT_HANDED.equals(driverModeChooser.get()); - if (Constants.isSidePresentedAsRed() != isDSPresentedAsRed || isLeftHanded != isLeftHandedSelected) + if (FieldConstants.isSidePresentedAsRed() != isDSPresentedAsRed || isLeftHanded != isLeftHandedSelected) configureButtonBindings(); - isDSPresentedAsRed = Constants.isSidePresentedAsRed(); + isDSPresentedAsRed = FieldConstants.isSidePresentedAsRed(); isLeftHanded = isLeftHandedSelected; final Auto selectedAuto = autoChooser.get(); @@ -252,7 +240,7 @@ public void checkForCommandChanges() { } private void resetFieldAndOdometryForAuto(Pose2d robotStartingPoseAtBlueAlliance) { - final Pose2d startingPose = Constants.toCurrentAlliancePose(robotStartingPoseAtBlueAlliance); + final Pose2d startingPose = FieldConstants.toCurrentAlliancePose(robotStartingPoseAtBlueAlliance); drive.setPose(startingPose); if (fieldSimulation == null) return; @@ -288,7 +276,7 @@ public void configureButtonBindings() { /* aim at target and drive example, delete it for your project */ final JoystickDriveAndAimAtTarget exampleFaceTargetWhileDriving = new JoystickDriveAndAimAtTarget( driveInput, drive, - Constants.CrescendoField2024Constants.SPEAKER_POSITION_SUPPLIER, + FieldConstants.SPEAKER_POSITION_SUPPLIER, exampleShooterOptimization, 0.5 ); @@ -298,7 +286,7 @@ public void configureButtonBindings() { final AutoAlignment exampleAutoAlignment = new AutoAlignment( drive, /* (position of AMP) */ - () -> Constants.toCurrentAlliancePose(new Pose2d(1.85, 7.6, Rotation2d.fromDegrees(90))) + () -> FieldConstants.toCurrentAlliancePose(new Pose2d(1.85, 7.6, Rotation2d.fromDegrees(90))) ); } diff --git a/src/main/java/frc/robot/commands/drive/ChassisFaceToRotation.java b/src/main/java/frc/robot/commands/drive/ChassisFaceToRotation.java index e759fd8..f0774c2 100644 --- a/src/main/java/frc/robot/commands/drive/ChassisFaceToRotation.java +++ b/src/main/java/frc/robot/commands/drive/ChassisFaceToRotation.java @@ -5,7 +5,7 @@ import edu.wpi.first.math.geometry.Translation2d; import edu.wpi.first.math.kinematics.ChassisSpeeds; import edu.wpi.first.wpilibj2.command.Command; -import frc.robot.Constants; +import frc.robot.constants.DriveControlLoops; import frc.robot.subsystems.drive.HolonomicDriveSubsystem; import frc.robot.utils.CustomPIDs.MaplePIDController; @@ -22,7 +22,7 @@ public ChassisFaceToRotation(HolonomicDriveSubsystem driveSubsystem, Supplier desiredPoseSupplier; private final HolonomicDriveSubsystem driveSubsystem; @@ -27,7 +29,7 @@ public DriveToPose(HolonomicDriveSubsystem driveSubsystem, Supplier desi public DriveToPose(HolonomicDriveSubsystem driveSubsystem, Supplier desiredPoseSupplier, Pose2d tolerance, double speedConstrainMPS) { this.desiredPoseSupplier = desiredPoseSupplier; this.driveSubsystem = driveSubsystem; - this.positionController = createPositionController(); + this.positionController = createPositionController(driveSubsystem); this.tolerance = tolerance; this.speedConstrainMPS = speedConstrainMPS; @@ -74,11 +76,12 @@ public boolean isFinished() { && Math.abs(speeds.omegaRadiansPerSecond) < Math.toRadians(30); } - public static HolonomicDriveController createPositionController() { + public static HolonomicDriveController createPositionController(HolonomicDriveSubsystem driveSubsystem) { + final TrapezoidProfile.Constraints chassisRotationalConstraints = new TrapezoidProfile.Constraints(driveSubsystem.getChassisMaxAngularVelocity(), driveSubsystem.getChassisMaxAngularAccelerationRadPerSecSq()); return new HolonomicDriveController( - new MaplePIDController(Constants.SwerveDriveChassisConfigs.chassisTranslationPIDConfig), - new MaplePIDController(Constants.SwerveDriveChassisConfigs.chassisTranslationPIDConfig), - new MapleProfiledPIDController(Constants.SwerveDriveChassisConfigs.chassisRotationalPIDConfig, Constants.SwerveDriveChassisConfigs.chassisRotationalConstraints) + new MaplePIDController(CHASSIS_TRANSLATION_CLOSE_LOOP), + new MaplePIDController(CHASSIS_TRANSLATION_CLOSE_LOOP), + new MapleProfiledPIDController(CHASSIS_ROTATION_CLOSE_LOOP, chassisRotationalConstraints) ); } } diff --git a/src/main/java/frc/robot/commands/drive/FollowPathPP.java b/src/main/java/frc/robot/commands/drive/FollowPathPP.java index 993a364..639d881 100644 --- a/src/main/java/frc/robot/commands/drive/FollowPathPP.java +++ b/src/main/java/frc/robot/commands/drive/FollowPathPP.java @@ -7,14 +7,16 @@ import com.pathplanner.lib.util.ReplanningConfig; import edu.wpi.first.math.geometry.Pose2d; import edu.wpi.first.math.geometry.Rotation2d; -import frc.robot.Constants; import frc.robot.Robot; +import frc.robot.constants.DriveControlLoops; import frc.robot.subsystems.drive.HolonomicDriveSubsystem; import java.util.ArrayList; import java.util.List; import java.util.function.BooleanSupplier; +import static frc.robot.constants.DriveControlLoops.*; + /** * this replaces AutoBuilder because in our code, opponent robots are simulated using path-planner drive commands * only the main robot should use AutoBuilder, opponent robots should use this @@ -38,8 +40,8 @@ public FollowPathPP(PathPlannerPath path, BooleanSupplier shouldFlipPath, Holono driveSubsystem::getPose, driveSubsystem::getMeasuredChassisSpeedsRobotRelative, driveSubsystem::runRobotCentricChassisSpeeds, - Constants.SwerveDriveChassisConfigs.chassisTranslationPIDConfigPathFollowing.toPathPlannerPIDConstants(), - Constants.SwerveDriveChassisConfigs.chassisRotationalPIDConfig.toPathPlannerPIDConstants(), + CHASSIS_TRANSLATION_CLOSE_LOOP.toPathPlannerPIDConstants(), + CHASSIS_ROTATION_CLOSE_LOOP.toPathPlannerPIDConstants(), driveSubsystem.getChassisMaxLinearVelocityMetersPerSec(), driveSubsystem.getChassisMaxLinearVelocityMetersPerSec() / driveSubsystem.getChassisMaxAngularVelocity(), Robot.defaultPeriodSecs, diff --git a/src/main/java/frc/robot/commands/drive/JoystickDrive.java b/src/main/java/frc/robot/commands/drive/JoystickDrive.java index 83c77e4..3e9c52a 100644 --- a/src/main/java/frc/robot/commands/drive/JoystickDrive.java +++ b/src/main/java/frc/robot/commands/drive/JoystickDrive.java @@ -5,8 +5,9 @@ import edu.wpi.first.math.kinematics.ChassisSpeeds; import edu.wpi.first.wpilibj.Timer; import edu.wpi.first.wpilibj2.command.Command; -import frc.robot.Constants; +import frc.robot.constants.Constants; import frc.robot.Robot; +import frc.robot.constants.DriveControlLoops; import frc.robot.subsystems.drive.HolonomicDriveSubsystem; import frc.robot.utils.MapleJoystickDriveInput; import frc.robot.utils.CustomPIDs.MaplePIDController; @@ -15,6 +16,7 @@ import java.util.function.BooleanSupplier; import static frc.robot.subsystems.drive.HolonomicDriveSubsystem.isZero; +import static frc.robot.constants.JoystickConfigs.*; public class JoystickDrive extends Command { protected MapleJoystickDriveInput input; @@ -36,7 +38,7 @@ public JoystickDrive(MapleJoystickDriveInput input, BooleanSupplier useDriverSta this.previousChassisUsageTimer.start(); this.previousRotationalInputTimer = new Timer(); this.previousRotationalInputTimer.start(); - this.chassisRotationController = new MaplePIDController(Constants.SwerveDriveChassisConfigs.chassisRotationalPIDConfig); + this.chassisRotationController = new MaplePIDController(DriveControlLoops.CHASSIS_ROTATION_CLOSE_LOOP); super.addRequirements(driveSubsystem); } @@ -70,7 +72,7 @@ public void execute() { previousRotationalInputTimer.reset(); Logger.recordOutput("JoystickDrive/current pilot input speeds", currentPilotInputSpeeds.toString()); - if (previousChassisUsageTimer.hasElapsed(Constants.DriverJoystickConfigs.nonUsageTimeResetWheels)) { + if (previousChassisUsageTimer.hasElapsed(NON_USAGE_TIME_RESET_WHEELS)) { driveSubsystem.stop(); return; } @@ -85,7 +87,7 @@ public void execute() { currentRotationMaintenanceSetpoint.getRadians() ); Logger.recordOutput("previousRotationalInputTimer.get()", previousRotationalInputTimer.get()); - if (previousRotationalInputTimer.get() > Constants.DriverJoystickConfigs.timeActivateRotationMaintenanceAfterNoRotationalInputSeconds) + if (previousRotationalInputTimer.get() > TIME_ACTIVATE_ROTATION_MAINTENANCE_AFTER_NO_ROTATIONAL_INPUT_SECONDS) chassisSpeedsWithRotationMaintenance = new ChassisSpeeds( currentPilotInputSpeeds.vxMetersPerSecond, currentPilotInputSpeeds.vyMetersPerSecond, rotationCorrectionAngularVelocity diff --git a/src/main/java/frc/robot/commands/drive/JoystickDriveAndAimAtTarget.java b/src/main/java/frc/robot/commands/drive/JoystickDriveAndAimAtTarget.java index 9ce70d2..72cee88 100644 --- a/src/main/java/frc/robot/commands/drive/JoystickDriveAndAimAtTarget.java +++ b/src/main/java/frc/robot/commands/drive/JoystickDriveAndAimAtTarget.java @@ -6,8 +6,9 @@ import edu.wpi.first.math.kinematics.ChassisSpeeds; import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import edu.wpi.first.wpilibj2.command.Command; -import frc.robot.Constants; +import frc.robot.constants.Constants; import frc.robot.Robot; +import frc.robot.constants.DriveControlLoops; import frc.robot.subsystems.drive.HolonomicDriveSubsystem; import frc.robot.utils.MapleJoystickDriveInput; import frc.robot.utils.MapleShooterOptimization; @@ -30,7 +31,7 @@ public JoystickDriveAndAimAtTarget(MapleJoystickDriveInput input, HolonomicDrive this.shooterOptimization = shooterOptimization; this.pilotInputMultiplier = pilotInputMultiplier; this.chassisRotationController = new MaplePIDController( - Constants.SwerveDriveChassisConfigs.chassisRotationalPIDConfig + DriveControlLoops.CHASSIS_ROTATION_CLOSE_LOOP ); this.driveSubsystem = driveSubsystem; diff --git a/src/main/java/frc/robot/constants/Constants.java b/src/main/java/frc/robot/constants/Constants.java new file mode 100644 index 0000000..c7b0696 --- /dev/null +++ b/src/main/java/frc/robot/constants/Constants.java @@ -0,0 +1,60 @@ +// Original Source: +// https://github.com/Mechanical-Advantage/AdvantageKit/tree/main/example_projects/advanced_swerve_drive/src/main, Copyright 2021-2024 FRC 6328 +// Modified by 5516 Iron Maple https://github.com/Shenzhen-Robotics-Alliance/ + +package frc.robot.constants; + +import edu.wpi.first.apriltag.AprilTagFieldLayout; +import edu.wpi.first.apriltag.AprilTagFields; +import edu.wpi.first.math.controller.SimpleMotorFeedforward; +import edu.wpi.first.math.system.plant.DCMotor; +import edu.wpi.first.math.util.Units; +import frc.robot.utils.CustomPIDs.MaplePIDController; +import org.photonvision.PhotonPoseEstimator; + +/** + * The Constants class provides a convenient place for teams to hold robot-wide numerical or boolean + * constants. This class should not be used for any other purpose. All constants should be declared + * globally (i.e. public static). Do not put anything functional in this class. + * + *

It is advised to statically import this class (or one of its inner classes) wherever the + * constants are needed, to reduce verbosity. + */ +public final class Constants { + public enum RobotMode { + /** + * Running on a real robot. + */ + REAL, + + /** + * Running a physics simulator. + */ + SIM, + + /** + * Replaying from a log file. + */ + REPLAY + } + + public static final String ROBOT_NAME = "5516-2024-OffSeason"; + + public static final class VisionConfigs { + public static final AprilTagFieldLayout fieldLayout = AprilTagFields.kDefaultField.loadAprilTagLayoutField(); + public static final PhotonPoseEstimator.PoseStrategy poseEstimatorStrategy = PhotonPoseEstimator.PoseStrategy.AVERAGE_BEST_TARGETS; + /* default standard error for vision observation, if only one apriltag observed */ + public static final double + TRANSLATIONAL_STANDARD_ERROR_METERS_FOR_SINGLE_OBSERVATION = 0.6, + ROTATIONAL_STANDARD_ERROR_RADIANS_FOR_SINGLE_OBSERVATION = 0.5, + + // only do odometry calibration if translational standard error if it is not greater than + TRANSLATIONAL_STANDARD_ERROR_THRESHOLD = 0.5, + // only do gyro calibration if rotational standard error is very, very small + ROTATIONAL_STANDARD_ERROR_THRESHOLD = Math.toRadians(5), + + ODOMETRY_TRANSLATIONAL_STANDARD_ERROR_METERS = 0.02, + // we trust the IMU very much (recommend 0.1 for Pigeon2, 0.5 for NavX) + GYRO_ROTATIONAL_STANDARD_ERROR_RADIANS = Math.toRadians(0.1); + } +} diff --git a/src/main/java/frc/robot/constants/DriveControlLoops.java b/src/main/java/frc/robot/constants/DriveControlLoops.java new file mode 100644 index 0000000..3900d69 --- /dev/null +++ b/src/main/java/frc/robot/constants/DriveControlLoops.java @@ -0,0 +1,50 @@ +package frc.robot.constants; + +import edu.wpi.first.math.controller.SimpleMotorFeedforward; +import frc.robot.utils.CustomPIDs.MaplePIDController; + +import static frc.robot.constants.DriveTrainConstants.DRIVE_FRICTION_VOLTAGE; + +public class DriveControlLoops { + public static final MaplePIDController.MaplePIDConfig CHASSIS_ROTATION_CLOSE_LOOP = new MaplePIDController.MaplePIDConfig( + Math.toRadians(360), + Math.toRadians(60), + 0.02, + Math.toRadians(3), + 0.05, + true, + 0 +); + + public static final MaplePIDController.MaplePIDConfig CHASSIS_TRANSLATION_CLOSE_LOOP = new MaplePIDController.MaplePIDConfig( + 2, + 1.2, + 0, + 0.03, + 0, + false, + 0 + ); + public static final MaplePIDController.MaplePIDConfig STEER_CLOSE_LOOP = new MaplePIDController.MaplePIDConfig( + 0.5, + Math.toRadians(90), + 0.02, + Math.toRadians(1.5), + 0, + true, + 0 + ); + + public static final SimpleMotorFeedforward DRIVE_OPEN_LOOP = new SimpleMotorFeedforward( + DRIVE_FRICTION_VOLTAGE, + 12/DriveTrainConstants.CHASSIS_MAX_VELOCITY + ); + public static final MaplePIDController.MaplePIDConfig DRIVE_CLOSE_LOOP = new MaplePIDController.MaplePIDConfig( + 5, + 2, + 0, + 0, + 0, + false, 0 + ); +} diff --git a/src/main/java/frc/robot/constants/DriveTrainConstants.java b/src/main/java/frc/robot/constants/DriveTrainConstants.java new file mode 100644 index 0000000..2019780 --- /dev/null +++ b/src/main/java/frc/robot/constants/DriveTrainConstants.java @@ -0,0 +1,104 @@ +package frc.robot.constants; + +import edu.wpi.first.math.geometry.Translation2d; +import edu.wpi.first.math.system.plant.DCMotor; +import edu.wpi.first.math.util.Units; + +/** + * stores the constants and PID configs for chassis + * because we want an all-real simulation for the chassis, the numbers are required to be considerably precise + * TODO: you need to change some of these numbers to fit your robot! + * TODO: If you are using REV Chassis, please remove ALL the references to {@link TunerConstants} and enter your numbers by hand + * */ +public class DriveTrainConstants { + /* dead configs, don't change them */ + public static final int ODOMETRY_CACHE_CAPACITY = 10; + public static final double ODOMETRY_FREQUENCY = 250; + public static final double ODOMETRY_WAIT_TIMEOUT_SECONDS = 0.02; + public static final int SIMULATION_TICKS_IN_1_PERIOD = 5; + + /* change motor type to match your robot */ + public static final DCMotor + DRIVE_MOTOR = DCMotor.getKrakenX60(1), + STEER_MOTOR = DCMotor.getFalcon500(1); + + /* adjust current limit */ + public static final double DRIVE_CURRENT_LIMIT = 60; + public static final double STEER_CURRENT_LIMIT = 20; + + + /* change these numbers to match your robot */ + public static final double + WHEEL_RADIUS_METERS = Units.inchesToMeters(TunerConstants.kWheelRadiusInches), + WHEEL_COEFFICIENT_OF_FRICTION = 0.8, + DRIVE_GEAR_RATIO = TunerConstants.kDriveGearRatio, + STEER_GEAR_RATIO = TunerConstants.kSteerGearRatio, + ROBOT_MASS_KG = 40; // with bumpers + + /* don't change the equations */ + private static final double GRAVITY_CONSTANT = 9.8; + public static final double + DRIVE_BASE_RADIUS = Math.hypot(TunerConstants.kFrontLeftXPosInches, TunerConstants.kFrontLeftYPosInches), + /* friction_force = normal_force * coefficient_of_friction */ + MAX_FRICTION_ACCELERATION = GRAVITY_CONSTANT * WHEEL_COEFFICIENT_OF_FRICTION, + /* force = torque / distance */ + MAX_PROPELLING_FORCE_NEWTONS = DRIVE_MOTOR.getTorque(DRIVE_CURRENT_LIMIT) * DRIVE_GEAR_RATIO / WHEEL_RADIUS_METERS, + /* floor_speed = wheel_angular_velocity * wheel_radius */ + CHASSIS_MAX_VELOCITY = DRIVE_MOTOR.freeSpeedRadPerSec / DRIVE_GEAR_RATIO * WHEEL_RADIUS_METERS , // calculate using choreo + CHASSIS_MAX_ACCELERATION_MPS_SQ = Math.min( + MAX_FRICTION_ACCELERATION, // cannot exceed max friction + MAX_PROPELLING_FORCE_NEWTONS / ROBOT_MASS_KG + ), + CHASSIS_MAX_ANGULAR_VELOCITY_RAD_PER_SEC = CHASSIS_MAX_VELOCITY / DRIVE_BASE_RADIUS, + CHASSIS_MAX_ANGULAR_ACCELERATION_RAD_PER_SEC_SQ = CHASSIS_MAX_ACCELERATION_MPS_SQ / DRIVE_BASE_RADIUS, + CHASSIS_FRICTIONAL_ANGULAR_ACCELERATION = MAX_FRICTION_ACCELERATION / DRIVE_BASE_RADIUS; + + /* translations of the modules to the robot center, in FL, FR, BL, BR */ + public static final Translation2d[] MODULE_TRANSLATIONS = new Translation2d[] { + new Translation2d( + Units.inchesToMeters(TunerConstants.kFrontLeftXPosInches), + Units.inchesToMeters(TunerConstants.kFrontLeftYPosInches) + ), + new Translation2d( + Units.inchesToMeters(TunerConstants.kFrontRightXPosInches), + Units.inchesToMeters(TunerConstants.kFrontRightYPosInches) + ), + new Translation2d( + Units.inchesToMeters(TunerConstants.kBackLeftXPosInches), + Units.inchesToMeters(TunerConstants.kBackLeftYPosInches) + ), + new Translation2d( + Units.inchesToMeters(TunerConstants.kBackRightXPosInches), + Units.inchesToMeters(TunerConstants.kBackRightYPosInches) + ) + }; + + /* for collision detection in simulation */ + public static final double + BUMPER_WIDTH_METERS = Units.inchesToMeters(34.5), + BUMPER_LENGTH_METERS = Units.inchesToMeters(36), + /* https://en.wikipedia.org/wiki/Friction#Coefficient_of_friction */ + BUMPER_COEFFICIENT_OF_FRICTION = 0.75, + /* https://simple.wikipedia.org/wiki/Coefficient_of_restitution */ + BUMPER_COEFFICIENT_OF_RESTITUTION = 0.08; + + public static final double STEER_FRICTION_VOLTAGE = 0.15; + public static final double DRIVE_FRICTION_VOLTAGE = 0.1; + public static final double STEER_INERTIA = 0.015; + public static final double DRIVE_INERTIA = 0.01; + + + + /* Gyro Sim */ + public static final double GYRO_ANGULAR_ACCELERATION_THRESHOLD_SKIDDING_RAD_PER_SEC_SQ = 100; + public static final double SKIDDING_AMOUNT_AT_THRESHOLD_RAD = Math.toRadians(1.2); + /* + * https://store.ctr-electronics.com/pigeon-2/ + * for a well-installed one with vibration reduction, only 0.4 degree + * but most teams just install it directly on the rigid chassis frame (including my team :D) + * so at least 1.2 degrees of drifting in 1 minutes for an average angular velocity of 60 degrees/second + * which is the average velocity during normal swerve-circular-offense + * */ + public static final double NORMAL_GYRO_DRIFT_IN_1_MIN_Std_Dev_RAD = Math.toRadians(1.2); + public static final double AVERAGE_VELOCITY_RAD_PER_SEC_DURING_TEST = Math.toRadians(60); +} 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..2ad86d5 --- /dev/null +++ b/src/main/java/frc/robot/constants/FieldConstants.java @@ -0,0 +1,71 @@ +package frc.robot.constants; + +import com.pathplanner.lib.path.PathPlannerPath; +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.geometry.Translation3d; +import edu.wpi.first.wpilibj.DriverStation; + +import java.util.Optional; +import java.util.function.Supplier; + +public class FieldConstants { + public static final double FIELD_WIDTH = 16.54; + public static final double FIELD_HEIGHT = 8.21; + + public static final Translation3d SPEAKER_POSE_BLUE = new Translation3d(0.1, 5.55, 2.2); + + public static final Supplier SPEAKER_POSITION_SUPPLIER = () -> toCurrentAllianceTranslation(SPEAKER_POSE_BLUE.toTranslation2d()); + + public static Rotation2d toCurrentAllianceRotation(Rotation2d rotationAtBlueSide) { + final Rotation2d + yAxis = Rotation2d.fromDegrees(90), + differenceFromYAxisAtBlueSide = rotationAtBlueSide.minus(yAxis), + differenceFromYAxisNew = differenceFromYAxisAtBlueSide.times(isSidePresentedAsRed() ? -1:1); + return yAxis.rotateBy(differenceFromYAxisNew); + } + + public static Translation2d toCurrentAllianceTranslation(Translation2d translationAtBlueSide) { + if (isSidePresentedAsRed()) + return new Translation2d( + FIELD_WIDTH - translationAtBlueSide.getX(), + translationAtBlueSide.getY() + ); + return translationAtBlueSide; + } + + public static Translation3d toCurrentAllianceTranslation(Translation3d translation3dAtBlueSide) { + final Translation2d translation3dAtCurrentAlliance = toCurrentAllianceTranslation(translation3dAtBlueSide.toTranslation2d()); + if (isSidePresentedAsRed()) + return new Translation3d( + translation3dAtCurrentAlliance.getX(), + translation3dAtCurrentAlliance.getY(), + translation3dAtBlueSide.getZ() + ); + return translation3dAtBlueSide; + } + + public static Pose2d toCurrentAlliancePose(Pose2d poseAtBlueSide) { + return new Pose2d( + toCurrentAllianceTranslation(poseAtBlueSide.getTranslation()), + toCurrentAllianceRotation(poseAtBlueSide.getRotation()) + ); + } + + public static PathPlannerPath toCurrentAlliancePath(PathPlannerPath pathAtBlueAlliance) { + return isSidePresentedAsRed() ? pathAtBlueAlliance.flipPath() : pathAtBlueAlliance; + } + + public static boolean isSidePresentedAsRed() { + final Optional alliance = DriverStation.getAlliance(); + return alliance.isPresent() && alliance.get().equals(DriverStation.Alliance.Red); + } + + public static Rotation2d getDriverStationFacing() { + return switch (DriverStation.getAlliance().orElse(DriverStation.Alliance.Red)) { + case Red -> new Rotation2d(Math.PI); + case Blue -> new Rotation2d(0); + }; + } +} diff --git a/src/main/java/frc/robot/constants/JoystickConfigs.java b/src/main/java/frc/robot/constants/JoystickConfigs.java new file mode 100644 index 0000000..321e310 --- /dev/null +++ b/src/main/java/frc/robot/constants/JoystickConfigs.java @@ -0,0 +1,28 @@ +package frc.robot.constants; + +/** + * Configs for the driver's joystick + * See {@link frc.robot.utils.MapleJoystickDriveInput} + * */ +public class JoystickConfigs { + /** the amount of time that the chassis waits after the pilot's last input, before it places all the swerve wheels to standby-state (facing forward) */ + public static final double NON_USAGE_TIME_RESET_WHEELS = 1; + + /** */ + public static final double DEAD_BAND_WHEN_OTHER_AXIS_EMPTY = 0.02; + public static final double DEAD_BAND_WHEN_OTHER_AXIS_FULL = 0.1; + public static final double LINEAR_SPEED_INPUT_EXPONENT = 1.6; + public static final double ROTATION_SPEED_INPUT_EXPONENT = 2; + + /** + * the amount of time that the chassis needs to shift to the desired pilot motion + * it's sort of a "smooth out" of the pilot's input + * this dramatically reduces over-current and brownouts + * */ + public static final double LINEAR_ACCELERATION_SMOOTH_OUT_SECONDS = 0.3; + /** same thing for rotation */ + public static final double ANGULAR_ACCELERATION_SMOOTH_OUT_SECONDS = 0.1; + + /** the amount of time that the chassis waits after the pilot's last rotational input, before it starts to "lock" its rotation with PID */ + public static final double TIME_ACTIVATE_ROTATION_MAINTENANCE_AFTER_NO_ROTATIONAL_INPUT_SECONDS = 0.3; +} diff --git a/src/main/java/frc/robot/constants/LogPaths.java b/src/main/java/frc/robot/constants/LogPaths.java new file mode 100644 index 0000000..f13eac5 --- /dev/null +++ b/src/main/java/frc/robot/constants/LogPaths.java @@ -0,0 +1,7 @@ +package frc.robot.constants; + +public class LogPaths { + public static final String SYSTEM_PERFORMANCE_PATH = "SystemPerformance/"; + public static final String PHYSICS_SIMULATION_PATH = "MaplePhysicsSimulation/"; + public static final String APRIL_TAGS_VISION_PATH = "Vision/AprilTags/"; +} diff --git a/src/main/java/frc/robot/constants/TunerConstants.java b/src/main/java/frc/robot/constants/TunerConstants.java new file mode 100644 index 0000000..b9f729f --- /dev/null +++ b/src/main/java/frc/robot/constants/TunerConstants.java @@ -0,0 +1,175 @@ +// TODO: For CTRE Chassis, generate this file using the Tuner +// after generated, you MUST replace all "private" labels with "public" +// you can do this with your IDE +// +// you MUST also change the numbers in +package frc.robot.constants; + +import com.ctre.phoenix6.configs.*; +import com.ctre.phoenix6.mechanisms.swerve.SwerveDrivetrainConstants; +import com.ctre.phoenix6.mechanisms.swerve.SwerveModule.ClosedLoopOutputType; +import com.ctre.phoenix6.mechanisms.swerve.SwerveModuleConstants; +import com.ctre.phoenix6.mechanisms.swerve.SwerveModuleConstants.SteerFeedbackType; +import com.ctre.phoenix6.mechanisms.swerve.SwerveModuleConstantsFactory; +import edu.wpi.first.math.util.Units; + +// Generated by the Tuner X Swerve Project Generator +// https://v6.docs.ctr-electronics.com/en/stable/docs/tuner/tuner-swerve/index.html +/** + *

For CTRE Chassis, generate this file using the Tuner

+ * + *

+ * TODO: after generated, you MUST replace all "private" labels with "public" + * (do it with the replace function of your IDE) + *

+ * + *

+ * TODO: you MUST delete the last two lines of this file + *

+ * + *

+ * TODO: you should also change the numbers in {@link DriveTrainConstants} + *

+ * */ +public class TunerConstants { + // Both sets of gains need to be tuned to your individual robot. + + // The steer motor uses any SwerveModule.SteerRequestType control request with the + // output type specified by SwerveModuleConstants.SteerMotorClosedLoopOutput + public static final Slot0Configs steerGains = new Slot0Configs() + .withKP(100).withKI(0).withKD(0.2) + .withKS(0).withKV(1.5).withKA(0); + // When using closed-loop control, the drive motor uses the control + // output type specified by SwerveModuleConstants.DriveMotorClosedLoopOutput + public static final Slot0Configs driveGains = new Slot0Configs() + .withKP(3).withKI(0).withKD(0) + .withKS(0).withKV(0).withKA(0); + + // The closed-loop output type to use for the steer motors; + // This affects the PID/FF gains for the steer motors + public static final ClosedLoopOutputType steerClosedLoopOutput = ClosedLoopOutputType.Voltage; + // The closed-loop output type to use for the drive motors; + // This affects the PID/FF gains for the drive motors + public static final ClosedLoopOutputType driveClosedLoopOutput = ClosedLoopOutputType.Voltage; + + // The stator current at which the wheels start to slip; + // This needs to be tuned to your individual robot + public static final double kSlipCurrentA = 150.0; + + // Initial configs for the drive and steer motors and the CANcoder; these cannot be null. + // TODO: ALL current limits here will be overridden + public static final TalonFXConfiguration driveInitialConfigs = new TalonFXConfiguration(); + public static final TalonFXConfiguration steerInitialConfigs = new TalonFXConfiguration(); + public static final CANcoderConfiguration cancoderInitialConfigs = new CANcoderConfiguration(); + // Configs for the Pigeon 2; leave this null to skip applying Pigeon 2 configs + public static final Pigeon2Configuration pigeonConfigs = null; + + // Theoretical free speed (m/s) at 12v applied output; + // This needs to be tuned to your individual robot + public static final double kSpeedAt12VoltsMps = 3.92; + + // Every 1 rotation of the azimuth results in kCoupleRatio drive motor turns; + // This may need to be tuned to your individual robot + public static final double kCoupleRatio = 3.5714285714285716; + + public static final double kDriveGearRatio = 8.142857142857142; + public static final double kSteerGearRatio = 12.8; + public static final double kWheelRadiusInches = 2; + + public static final boolean kInvertLeftSide = true; + public static final boolean kInvertRightSide = false; + + public static final String kCANbusName = "ChassisCanivore"; + public static final int kPigeonId = 15; + + + // These are only used for simulation + public static final double kSteerInertia = 0.00001; + public static final double kDriveInertia = 0.001; + // Simulated voltage necessary to overcome friction + public static final double kSteerFrictionVoltage = 0.25; + public static final double kDriveFrictionVoltage = 0.25; + + public static final SwerveDrivetrainConstants DrivetrainConstants = new SwerveDrivetrainConstants() + .withCANbusName(kCANbusName) + .withPigeon2Id(kPigeonId) + .withPigeon2Configs(pigeonConfigs); + + public static final SwerveModuleConstantsFactory ConstantCreator = new SwerveModuleConstantsFactory() + .withDriveMotorGearRatio(kDriveGearRatio) + .withSteerMotorGearRatio(kSteerGearRatio) + .withWheelRadius(kWheelRadiusInches) + .withSlipCurrent(kSlipCurrentA) + .withSteerMotorGains(steerGains) + .withDriveMotorGains(driveGains) + .withSteerMotorClosedLoopOutput(steerClosedLoopOutput) + .withDriveMotorClosedLoopOutput(driveClosedLoopOutput) + .withSpeedAt12VoltsMps(kSpeedAt12VoltsMps) + .withSteerInertia(kSteerInertia) + .withDriveInertia(kDriveInertia) + .withSteerFrictionVoltage(kSteerFrictionVoltage) + .withDriveFrictionVoltage(kDriveFrictionVoltage) + .withFeedbackSource(SteerFeedbackType.FusedCANcoder) + .withCouplingGearRatio(kCoupleRatio) + .withDriveMotorInitialConfigs(driveInitialConfigs) + .withSteerMotorInitialConfigs(steerInitialConfigs) + .withCANcoderInitialConfigs(cancoderInitialConfigs); + + + // Front Left + public static final int kFrontLeftDriveMotorId = 2; + public static final int kFrontLeftSteerMotorId = 1; + public static final int kFrontLeftEncoderId = 9; + public static final double kFrontLeftEncoderOffset = -0.181640625; + public static final boolean kFrontLeftSteerInvert = true; + + public static final double kFrontLeftXPosInches = 10; + public static final double kFrontLeftYPosInches = 10; + + // Front Right + public static final int kFrontRightDriveMotorId = 4; + public static final int kFrontRightSteerMotorId = 3; + public static final int kFrontRightEncoderId = 10; + public static final double kFrontRightEncoderOffset = 0.06982421875; + public static final boolean kFrontRightSteerInvert = true; + + public static final double kFrontRightXPosInches = 10; + public static final double kFrontRightYPosInches = -10; + + // Back Left + public static final int kBackLeftDriveMotorId = 8; + public static final int kBackLeftSteerMotorId = 7; + public static final int kBackLeftEncoderId = 12; + public static final double kBackLeftEncoderOffset = -0.0263671875; + public static final boolean kBackLeftSteerInvert = true; + + public static final double kBackLeftXPosInches = -10; + public static final double kBackLeftYPosInches = 10; + + // Back Right + public static final int kBackRightDriveMotorId = 6; + public static final int kBackRightSteerMotorId = 5; + public static final int kBackRightEncoderId = 11; + public static final double kBackRightEncoderOffset = -0.114013671875; + public static final boolean kBackRightSteerInvert = true; + + public static final double kBackRightXPosInches = -10; + public static final double kBackRightYPosInches = -10; + + public static final SwerveModuleConstants FrontLeft = ConstantCreator.createModuleConstants( + kFrontLeftSteerMotorId, kFrontLeftDriveMotorId, kFrontLeftEncoderId, kFrontLeftEncoderOffset, Units.inchesToMeters(kFrontLeftXPosInches), Units.inchesToMeters(kFrontLeftYPosInches), kInvertLeftSide) + .withSteerMotorInverted(kFrontLeftSteerInvert); + public static final SwerveModuleConstants FrontRight = ConstantCreator.createModuleConstants( + kFrontRightSteerMotorId, kFrontRightDriveMotorId, kFrontRightEncoderId, kFrontRightEncoderOffset, Units.inchesToMeters(kFrontRightXPosInches), Units.inchesToMeters(kFrontRightYPosInches), kInvertRightSide) + .withSteerMotorInverted(kFrontRightSteerInvert); + public static final SwerveModuleConstants BackLeft = ConstantCreator.createModuleConstants( + kBackLeftSteerMotorId, kBackLeftDriveMotorId, kBackLeftEncoderId, kBackLeftEncoderOffset, Units.inchesToMeters(kBackLeftXPosInches), Units.inchesToMeters(kBackLeftYPosInches), kInvertLeftSide) + .withSteerMotorInverted(kBackLeftSteerInvert); + public static final SwerveModuleConstants BackRight = ConstantCreator.createModuleConstants( + kBackRightSteerMotorId, kBackRightDriveMotorId, kBackRightEncoderId, kBackRightEncoderOffset, Units.inchesToMeters(kBackRightXPosInches), Units.inchesToMeters(kBackRightYPosInches), kInvertRightSide) + .withSteerMotorInverted(kBackRightSteerInvert); + + // TODO: the last two lines MUST be removed +// public static final CommandSwerveDrivetrain DriveTrain = new CommandSwerveDrivetrain(DrivetrainConstants, FrontLeft, +// FrontRight, BackLeft, BackRight); +} diff --git a/src/main/java/frc/robot/subsystems/MapleSubsystem.java b/src/main/java/frc/robot/subsystems/MapleSubsystem.java index 8284857..aa4f5f0 100644 --- a/src/main/java/frc/robot/subsystems/MapleSubsystem.java +++ b/src/main/java/frc/robot/subsystems/MapleSubsystem.java @@ -4,8 +4,9 @@ import edu.wpi.first.wpilibj.DriverStation; import edu.wpi.first.wpilibj2.command.SubsystemBase; -import frc.robot.Constants; +import frc.robot.constants.Constants; import frc.robot.Robot; +import frc.robot.constants.LogPaths; import frc.robot.utils.MapleTimeUtils; import org.littletonrobotics.junction.Logger; @@ -60,7 +61,7 @@ public void periodic() { final long t0 = System.nanoTime(); periodic(getDt(), DriverStation.isEnabled()); final double cpuTimeMS = (System.nanoTime() - t0) / 1_000_000.0; - Logger.recordOutput(Constants.LogConfigs.SYSTEM_PERFORMANCE_PATH + getName() + "-CPUTimeMS", cpuTimeMS); + Logger.recordOutput(LogPaths.SYSTEM_PERFORMANCE_PATH + getName() + "-CPUTimeMS", cpuTimeMS); } private double getDt() { diff --git a/src/main/java/frc/robot/subsystems/drive/HolonomicDriveSubsystem.java b/src/main/java/frc/robot/subsystems/drive/HolonomicDriveSubsystem.java index d639d4f..f86622b 100644 --- a/src/main/java/frc/robot/subsystems/drive/HolonomicDriveSubsystem.java +++ b/src/main/java/frc/robot/subsystems/drive/HolonomicDriveSubsystem.java @@ -16,11 +16,14 @@ import edu.wpi.first.math.numbers.N1; import edu.wpi.first.math.numbers.N3; import edu.wpi.first.wpilibj2.command.Subsystem; -import frc.robot.Constants; +import frc.robot.constants.FieldConstants; import frc.robot.utils.CompetitionFieldUtils.CompetitionFieldVisualizer; import frc.robot.utils.LocalADStarAK; import org.littletonrobotics.junction.Logger; +import static frc.robot.constants.JoystickConfigs.*; +import static frc.robot.constants.DriveControlLoops.*; + public interface HolonomicDriveSubsystem extends Subsystem { /** * runs a ChassisSpeeds without doing any pre-processing @@ -81,7 +84,7 @@ default PathConstraints getChassisConstrains(double speedMultiplier) { * @param driverStationCentricSpeeds a continuous chassis speeds, driverstation-centric, normally from a gamepad * */ default void runDriverStationCentricChassisSpeeds(ChassisSpeeds driverStationCentricSpeeds) { - final Rotation2d driverStationFacing = Constants.getDriverStationFacing(); + final Rotation2d driverStationFacing = FieldConstants.getDriverStationFacing(); runRobotCentricChassisSpeeds(ChassisSpeeds.fromFieldRelativeSpeeds( driverStationCentricSpeeds, getPose().getRotation().minus(driverStationFacing) @@ -118,13 +121,13 @@ default void configHolonomicPathPlannerAutoBuilder(CompetitionFieldVisualizer fi this::getMeasuredChassisSpeedsRobotRelative, this::runRobotCentricChassisSpeeds, new HolonomicPathFollowerConfig( - Constants.SwerveDriveChassisConfigs.chassisTranslationPIDConfigPathFollowing.toPathPlannerPIDConstants(), - Constants.SwerveDriveChassisConfigs.chassisRotationalPIDConfig.toPathPlannerPIDConstants(), + CHASSIS_TRANSLATION_CLOSE_LOOP.toPathPlannerPIDConstants(), + CHASSIS_ROTATION_CLOSE_LOOP.toPathPlannerPIDConstants(), getChassisMaxLinearVelocityMetersPerSec(), getChassisMaxLinearVelocityMetersPerSec() / getChassisMaxAngularVelocity(), new ReplanningConfig(false, true) ), - Constants::isSidePresentedAsRed, + FieldConstants::isSidePresentedAsRed, this ); Pathfinding.setPathfinder(new LocalADStarAK()); @@ -149,9 +152,9 @@ default ChassisSpeeds constrainAcceleration( double dtSecs) { final double MAX_LINEAR_ACCELERATION_METERS_PER_SEC_SQ = getChassisMaxLinearVelocityMetersPerSec() - / Constants.DriverJoystickConfigs.linearAccelerationSmoothOutSeconds, + / LINEAR_ACCELERATION_SMOOTH_OUT_SECONDS, MAX_ANGULAR_ACCELERATION_RAD_PER_SEC_SQ = getChassisMaxAngularVelocity() - / Constants.DriverJoystickConfigs.angularAccelerationSmoothOutSeconds; + / ANGULAR_ACCELERATION_SMOOTH_OUT_SECONDS; Translation2d currentLinearVelocityMetersPerSec = new Translation2d(currentSpeeds.vxMetersPerSecond, currentSpeeds.vyMetersPerSecond), desiredLinearVelocityMetersPerSec = new Translation2d(desiredSpeeds.vxMetersPerSecond, desiredSpeeds.vyMetersPerSecond), diff --git a/src/main/java/frc/robot/subsystems/drive/IO/GyroIOPigeon2.java b/src/main/java/frc/robot/subsystems/drive/IO/GyroIOPigeon2.java index 974fad0..1a3f414 100644 --- a/src/main/java/frc/robot/subsystems/drive/IO/GyroIOPigeon2.java +++ b/src/main/java/frc/robot/subsystems/drive/IO/GyroIOPigeon2.java @@ -7,9 +7,10 @@ import com.ctre.phoenix6.StatusSignal; import com.ctre.phoenix6.configs.Pigeon2Configuration; import com.ctre.phoenix6.hardware.Pigeon2; +import com.ctre.phoenix6.mechanisms.swerve.SwerveDrivetrainConstants; import edu.wpi.first.math.geometry.Rotation2d; import edu.wpi.first.math.util.Units; -import frc.robot.Constants; +import org.opencv.calib3d.StereoBM; import java.util.Queue; @@ -17,17 +18,31 @@ * IO implementation for Pigeon2 */ public class GyroIOPigeon2 implements GyroIO { - private final Pigeon2 pigeon = new Pigeon2(0, Constants.SwerveDriveChassisConfigs.CHASSIS_CANBUS); - private final StatusSignal yaw = pigeon.getYaw(); + private final Pigeon2 pigeon; + private final StatusSignal yaw; private final Queue yawPositionInput; - private final StatusSignal yawVelocity = pigeon.getAngularVelocityZWorld(); + private final StatusSignal yawVelocity; - public GyroIOPigeon2() { - pigeon.getConfigurator().apply(new Pigeon2Configuration()); + public GyroIOPigeon2(SwerveDrivetrainConstants drivetrainConstants) { + this( + drivetrainConstants.Pigeon2Id, + drivetrainConstants.CANbusName, + drivetrainConstants.Pigeon2Configs + ); + } + + public GyroIOPigeon2(int Pigeon2Id, String CANbusName, Pigeon2Configuration Pigeon2Configs) { + pigeon = new Pigeon2(Pigeon2Id, CANbusName); + pigeon.getConfigurator().apply(Pigeon2Configs); pigeon.getConfigurator().setYaw(0.0); + + yaw = pigeon.getYaw(); + yawVelocity = pigeon.getAngularVelocityZWorld(); + yawVelocity.setUpdateFrequency(100.0); - pigeon.optimizeBusUtilization(); yawPositionInput = OdometryThread.registerSignalInput(pigeon.getYaw()); + + pigeon.optimizeBusUtilization(); } @Override diff --git a/src/main/java/frc/robot/subsystems/drive/IO/GyroIOSim.java b/src/main/java/frc/robot/subsystems/drive/IO/GyroIOSim.java index 4477238..7a7e148 100644 --- a/src/main/java/frc/robot/subsystems/drive/IO/GyroIOSim.java +++ b/src/main/java/frc/robot/subsystems/drive/IO/GyroIOSim.java @@ -1,21 +1,22 @@ package frc.robot.subsystems.drive.IO; import edu.wpi.first.math.geometry.Rotation2d; -import frc.robot.Constants; +import frc.robot.constants.Constants; import frc.robot.Robot; +import frc.robot.constants.LogPaths; import frc.robot.utils.CustomMaths.MapleCommonMath; import frc.robot.utils.MapleTimeUtils; import org.littletonrobotics.junction.Logger; import java.util.Arrays; -import static frc.robot.Constants.DriveTrainPhysicsSimulationConstants.*; +import static frc.robot.constants.DriveTrainConstants.*; public class GyroIOSim implements GyroIO { public final GyroPhysicsSimulationResults gyroPhysicsSimulationResults = new GyroPhysicsSimulationResults(); public double previousAngularVelocityRadPerSec = gyroPhysicsSimulationResults.robotAngularVelocityRadPerSec; public Rotation2d currentGyroDriftAmount = new Rotation2d(); - public static final String GYRO_LOG_PATH = Constants.LogConfigs.PHYSICS_SIMULATION_PATH + "GyroSim/"; + public static final String GYRO_LOG_PATH = LogPaths.PHYSICS_SIMULATION_PATH + "GyroSim/"; @Override public void updateInputs(GyroIOInputs inputs) { @@ -61,7 +62,7 @@ public static class GyroPhysicsSimulationResults { public boolean hasReading; public final Rotation2d[] odometryYawPositions = - new Rotation2d[SIM_ITERATIONS_PER_ROBOT_PERIOD]; + new Rotation2d[SIMULATION_TICKS_IN_1_PERIOD]; public GyroPhysicsSimulationResults() { robotAngularVelocityRadPerSec = 0.0; diff --git a/src/main/java/frc/robot/subsystems/drive/IO/ModuleIO.java b/src/main/java/frc/robot/subsystems/drive/IO/ModuleIO.java index 6f651b0..06dc857 100644 --- a/src/main/java/frc/robot/subsystems/drive/IO/ModuleIO.java +++ b/src/main/java/frc/robot/subsystems/drive/IO/ModuleIO.java @@ -5,6 +5,7 @@ package frc.robot.subsystems.drive.IO; import edu.wpi.first.math.geometry.Rotation2d; +import edu.wpi.first.math.util.Units; import org.littletonrobotics.junction.AutoLog; public interface ModuleIO { diff --git a/src/main/java/frc/robot/subsystems/drive/IO/ModuleIOSim.java b/src/main/java/frc/robot/subsystems/drive/IO/ModuleIOSim.java index 19fdaa8..b7287ec 100644 --- a/src/main/java/frc/robot/subsystems/drive/IO/ModuleIOSim.java +++ b/src/main/java/frc/robot/subsystems/drive/IO/ModuleIOSim.java @@ -7,11 +7,10 @@ import edu.wpi.first.math.geometry.Rotation2d; import edu.wpi.first.math.kinematics.SwerveModuleState; import edu.wpi.first.wpilibj.simulation.DCMotorSim; -import frc.robot.utils.CustomConfigs.MapleConfigFile; import java.util.Arrays; -import static frc.robot.Constants.DriveTrainPhysicsSimulationConstants.*; +import static frc.robot.constants.DriveTrainConstants.*; /** * Physics sim implementation of module IO. @@ -21,14 +20,12 @@ * approximation for the behavior of the module. */ public class ModuleIOSim implements ModuleIO { - private final double GEAR_RATIO; public final SwerveModulePhysicsSimulationResults physicsSimulationResults; private final DCMotorSim driveSim, steerSim; private double driveAppliedVolts = 0.0, steerAppliedVolts = 0.0; - public ModuleIOSim(MapleConfigFile.ConfigBlock generalConfigs) { - this.GEAR_RATIO = generalConfigs.getDoubleConfig("overallGearRatio"); - this.driveSim = new DCMotorSim(DRIVE_MOTOR, GEAR_RATIO, DRIVE_WHEEL_ROTTER_INERTIA); + public ModuleIOSim() { + this.driveSim = new DCMotorSim(DRIVE_MOTOR, DRIVE_GEAR_RATIO, DRIVE_INERTIA); this.steerSim = new DCMotorSim(STEER_MOTOR, STEER_GEAR_RATIO, STEER_INERTIA); this.physicsSimulationResults = new SwerveModulePhysicsSimulationResults(); } @@ -47,11 +44,11 @@ public void updateInputs(ModuleIOInputs inputs) { inputs.odometryDriveWheelRevolutions = Arrays.copyOf( physicsSimulationResults.odometryDriveWheelRevolutions, - SIM_ITERATIONS_PER_ROBOT_PERIOD + SIMULATION_TICKS_IN_1_PERIOD ); inputs.odometrySteerPositions = Arrays.copyOf( physicsSimulationResults.odometrySteerPositions, - SIM_ITERATIONS_PER_ROBOT_PERIOD + SIMULATION_TICKS_IN_1_PERIOD ); inputs.hardwareConnected = true; @@ -79,11 +76,9 @@ public void updateSim(double periodSecs) { * gets the swerve state, assuming that the chassis is allowed to move freely on field (not hitting anything) * @return the swerve state, in percent full speed * */ - public SwerveModuleState getFreeSwerveSpeed(double robotMaximumFloorSpeed) { + public SwerveModuleState getFreeSwerveSpeed() { return new SwerveModuleState( - driveSim.getAngularVelocityRPM() * GEAR_RATIO - / DRIVE_MOTOR_FREE_FINAL_SPEED_RPM - * robotMaximumFloorSpeed, + driveSim.getAngularVelocityRPM() * WHEEL_RADIUS_METERS, Rotation2d.fromRadians(steerSim.getAngularPositionRad()) ); } @@ -97,9 +92,9 @@ public static class SwerveModulePhysicsSimulationResults { driveWheelFinalVelocityRevolutionsPerSec = 0; public final double[] odometryDriveWheelRevolutions = - new double[SIM_ITERATIONS_PER_ROBOT_PERIOD]; + new double[SIMULATION_TICKS_IN_1_PERIOD]; public final Rotation2d[] odometrySteerPositions = - new Rotation2d[SIM_ITERATIONS_PER_ROBOT_PERIOD]; + new Rotation2d[SIMULATION_TICKS_IN_1_PERIOD]; public SwerveModulePhysicsSimulationResults() { Arrays.fill(odometrySteerPositions, new Rotation2d()); diff --git a/src/main/java/frc/robot/subsystems/drive/IO/ModuleIOSparkMax.java b/src/main/java/frc/robot/subsystems/drive/IO/ModuleIOSpark.java similarity index 96% rename from src/main/java/frc/robot/subsystems/drive/IO/ModuleIOSparkMax.java rename to src/main/java/frc/robot/subsystems/drive/IO/ModuleIOSpark.java index 56188f0..a7ed396 100644 --- a/src/main/java/frc/robot/subsystems/drive/IO/ModuleIOSparkMax.java +++ b/src/main/java/frc/robot/subsystems/drive/IO/ModuleIOSpark.java @@ -13,10 +13,11 @@ import edu.wpi.first.math.util.Units; import edu.wpi.first.wpilibj.AnalogInput; import edu.wpi.first.wpilibj.RobotController; -import frc.robot.Constants; import java.util.Queue; +import static frc.robot.constants.DriveTrainConstants.*; + /** * Module IO implementation for SparkMax drive motor controller, SparkMax turn motor controller (NEO * or NEO 550), and analog absolute encoder connected to the RIO @@ -29,7 +30,7 @@ * absolute encoders using AdvantageScope. These values are logged under * "/Drive/ModuleX/TurnAbsolutePositionRad" */ -public class ModuleIOSparkMax implements ModuleIO { +public class ModuleIOSpark implements ModuleIO { // Gear ratios for SDS MK4i L2, adjust as necessary private static final double DRIVE_GEAR_RATIO = (50.0 / 14.0) * (17.0 / 27.0) * (45.0 / 15.0); private static final double STEER_GEAR_RATIO = 150.0 / 7.0; @@ -46,7 +47,7 @@ public class ModuleIOSparkMax implements ModuleIO { private final boolean isTurnMotorInverted = true; private final Rotation2d absoluteEncoderOffset; - public ModuleIOSparkMax(int index) { + public ModuleIOSpark(int index) { switch (index) { case 0 -> { driveSparkMax = new CANSparkMax(1, MotorType.kBrushless); @@ -103,11 +104,11 @@ public ModuleIOSparkMax(int index) { driveSparkMax.setPeriodicFramePeriod( PeriodicFrame.kStatus2, - (int) (1000.0 / Constants.SwerveDriveChassisConfigs.ODOMETRY_FREQUENCY) + (int) (1000.0 / ODOMETRY_FREQUENCY) ); steerSparkMax.setPeriodicFramePeriod( PeriodicFrame.kStatus2, - (int) (1000.0 / Constants.SwerveDriveChassisConfigs.ODOMETRY_FREQUENCY) + (int) (1000.0 / ODOMETRY_FREQUENCY) ); this.drivePositionInput = OdometryThread.registerInput(driveEncoder::getPosition); this.steerRelativeEncoderPositionUngeared = OdometryThread.registerInput(steerRelativeEncoder::getPosition); diff --git a/src/main/java/frc/robot/subsystems/drive/IO/ModuleIOTalonFX.java b/src/main/java/frc/robot/subsystems/drive/IO/ModuleIOTalon.java similarity index 79% rename from src/main/java/frc/robot/subsystems/drive/IO/ModuleIOTalonFX.java rename to src/main/java/frc/robot/subsystems/drive/IO/ModuleIOTalon.java index 708d74f..75cfbb3 100644 --- a/src/main/java/frc/robot/subsystems/drive/IO/ModuleIOTalonFX.java +++ b/src/main/java/frc/robot/subsystems/drive/IO/ModuleIOTalon.java @@ -6,20 +6,21 @@ import com.ctre.phoenix6.BaseStatusSignal; import com.ctre.phoenix6.StatusSignal; -import com.ctre.phoenix6.configs.TalonFXConfiguration; import com.ctre.phoenix6.controls.DutyCycleOut; import com.ctre.phoenix6.controls.VoltageOut; import com.ctre.phoenix6.hardware.CANcoder; import com.ctre.phoenix6.hardware.TalonFX; +import com.ctre.phoenix6.mechanisms.swerve.SwerveDrivetrainConstants; +import com.ctre.phoenix6.mechanisms.swerve.SwerveModuleConstants; import com.ctre.phoenix6.signals.NeutralModeValue; import edu.wpi.first.math.geometry.Rotation2d; import edu.wpi.first.math.util.Units; -import frc.robot.Constants; -import frc.robot.utils.CustomConfigs.MapleConfigFile; import java.util.Queue; -public class ModuleIOTalonFX implements ModuleIO { +import static frc.robot.constants.DriveTrainConstants.*; + +public class ModuleIOTalon implements ModuleIO { private final String name; private final TalonFX driveTalon; private final TalonFX steerTalon; @@ -36,24 +37,24 @@ public class ModuleIOTalonFX implements ModuleIO { private final Rotation2d absoluteEncoderOffset; private final double DRIVE_GEAR_RATIO; - public ModuleIOTalonFX(MapleConfigFile.ConfigBlock moduleConfigs, MapleConfigFile.ConfigBlock generalConfigs) { - this.name = moduleConfigs.getBlockName(); - driveTalon = new TalonFX(moduleConfigs.getIntConfig("drivingMotorID"), Constants.SwerveDriveChassisConfigs.CHASSIS_CANBUS); - steerTalon = new TalonFX(moduleConfigs.getIntConfig("steeringMotorID"), Constants.SwerveDriveChassisConfigs.CHASSIS_CANBUS); - cancoder = new CANcoder(moduleConfigs.getIntConfig("steeringEncoderID"), Constants.SwerveDriveChassisConfigs.CHASSIS_CANBUS); - absoluteEncoderOffset = new Rotation2d(moduleConfigs.getDoubleConfig("steeringEncoderReadingAtOrigin")); // MUST BE CALIBRATED + public ModuleIOTalon(SwerveDrivetrainConstants drivetrainConstants, SwerveModuleConstants moduleConstants, String name) { + this.name = name; + driveTalon = new TalonFX(moduleConstants.DriveMotorId, drivetrainConstants.CANbusName); + steerTalon = new TalonFX(moduleConstants.SteerMotorId, drivetrainConstants.CANbusName); + cancoder = new CANcoder(moduleConstants.CANcoderId, drivetrainConstants.CANbusName); + absoluteEncoderOffset = Rotation2d.fromRotations(moduleConstants.CANcoderOffset); - var driveConfig = new TalonFXConfiguration(); - driveConfig.CurrentLimits.SupplyCurrentLimit = Constants.SwerveModuleConfigs.DRIVING_CURRENT_LIMIT; + var driveConfig = moduleConstants.DriveMotorInitialConfigs; + driveConfig.CurrentLimits.SupplyCurrentLimit = DRIVE_CURRENT_LIMIT; driveConfig.CurrentLimits.SupplyCurrentLimitEnable = true; driveTalon.getConfigurator().apply(driveConfig); setDriveBrake(true); - var steerConfig = new TalonFXConfiguration(); - steerConfig.CurrentLimits.SupplyCurrentLimit = Constants.SwerveModuleConfigs.STEERING_CURRENT_LIMIT; + var steerConfig = moduleConstants.SteerMotorInitialConfigs; + steerConfig.CurrentLimits.SupplyCurrentLimit = STEER_CURRENT_LIMIT; steerConfig.CurrentLimits.SupplyCurrentLimitEnable = true; steerTalon.getConfigurator().apply(steerConfig); - steerTalon.setInverted(moduleConfigs.getIntConfig("steeringMotorInverted") != 0); + steerTalon.setInverted(moduleConstants.SteerMotorInverted); setSteerBrake(true); driveEncoderUngearedRevolutions = OdometryThread.registerSignalInput(driveTalon.getPosition()); @@ -77,7 +78,7 @@ public ModuleIOTalonFX(MapleConfigFile.ConfigBlock moduleConfigs, MapleConfigFil driveTalon.optimizeBusUtilization(); steerTalon.optimizeBusUtilization(); - this.DRIVE_GEAR_RATIO = generalConfigs.getDoubleConfig("overallGearRatio"); + this.DRIVE_GEAR_RATIO = moduleConstants.DriveMotorGearRatio; } @Override diff --git a/src/main/java/frc/robot/subsystems/drive/IO/OdometryThread.java b/src/main/java/frc/robot/subsystems/drive/IO/OdometryThread.java index 2e7ca60..4724968 100644 --- a/src/main/java/frc/robot/subsystems/drive/IO/OdometryThread.java +++ b/src/main/java/frc/robot/subsystems/drive/IO/OdometryThread.java @@ -2,9 +2,9 @@ import com.ctre.phoenix6.BaseStatusSignal; import com.ctre.phoenix6.StatusSignal; -import frc.robot.Constants; import frc.robot.Robot; import frc.robot.subsystems.drive.OdometryThreadReal; +import frc.robot.subsystems.drive.SwerveDrive; import frc.robot.utils.CompetitionFieldUtils.Simulations.SwerveDriveSimulation; import org.littletonrobotics.junction.AutoLog; @@ -14,6 +14,8 @@ import java.util.concurrent.ArrayBlockingQueue; import java.util.function.Supplier; +import static frc.robot.constants.DriveTrainConstants.*; + public interface OdometryThread { final class OdometryDoubleInput { private final Supplier supplier; @@ -21,7 +23,7 @@ final class OdometryDoubleInput { public OdometryDoubleInput(Supplier signal) { this.supplier = signal; - this.queue = new ArrayBlockingQueue<>(Constants.SwerveDriveChassisConfigs.ODOMETRY_CACHE_CAPACITY); + this.queue = new ArrayBlockingQueue<>(ODOMETRY_CACHE_CAPACITY); } public void cacheInputToQueue() { @@ -32,7 +34,7 @@ public void cacheInputToQueue() { List registeredInputs = new ArrayList<>(); List registeredStatusSignals = new ArrayList<>(); static Queue registerSignalInput(StatusSignal signal) { - signal.setUpdateFrequency(Constants.SwerveDriveChassisConfigs.ODOMETRY_FREQUENCY, Constants.SwerveDriveChassisConfigs.ODOMETRY_WAIT_TIMEOUT_SECONDS); + signal.setUpdateFrequency(ODOMETRY_FREQUENCY, ODOMETRY_WAIT_TIMEOUT_SECONDS); registeredStatusSignals.add(signal); return registerInput(signal.asSupplier()); } @@ -42,9 +44,10 @@ static Queue registerInput(Supplier supplier) { return odometryDoubleInput.queue; } - static OdometryThread createInstance() { + static OdometryThread createInstance(SwerveDrive.DriveType type) { return switch (Robot.CURRENT_ROBOT_MODE) { case REAL -> new OdometryThreadReal( + type, registeredInputs.toArray(new OdometryDoubleInput[0]), registeredStatusSignals.toArray(new BaseStatusSignal[0]) ); diff --git a/src/main/java/frc/robot/subsystems/drive/OdometryThreadReal.java b/src/main/java/frc/robot/subsystems/drive/OdometryThreadReal.java index cfd1784..df7ac44 100644 --- a/src/main/java/frc/robot/subsystems/drive/OdometryThreadReal.java +++ b/src/main/java/frc/robot/subsystems/drive/OdometryThreadReal.java @@ -2,7 +2,6 @@ package frc.robot.subsystems.drive; import com.ctre.phoenix6.BaseStatusSignal; -import frc.robot.Constants; import frc.robot.subsystems.drive.IO.OdometryThread; import frc.robot.utils.MapleTimeUtils; @@ -11,13 +10,18 @@ import java.util.concurrent.locks.Lock; import java.util.concurrent.locks.ReentrantLock; +import static frc.robot.constants.DriveTrainConstants.*; + public class OdometryThreadReal extends Thread implements OdometryThread { + private final SwerveDrive.DriveType driveType; + private final OdometryDoubleInput[] odometryDoubleInputs; private final BaseStatusSignal[] statusSignals; private final Queue timeStampsQueue; private final Lock lock = new ReentrantLock(); - public OdometryThreadReal(OdometryDoubleInput[] odometryDoubleInputs, BaseStatusSignal[] statusSignals) { - this.timeStampsQueue = new ArrayBlockingQueue<>(Constants.SwerveDriveChassisConfigs.ODOMETRY_CACHE_CAPACITY); + public OdometryThreadReal(SwerveDrive.DriveType driveType, OdometryDoubleInput[] odometryDoubleInputs, BaseStatusSignal[] statusSignals) { + this.driveType = driveType; + this.timeStampsQueue = new ArrayBlockingQueue<>(ODOMETRY_CACHE_CAPACITY); this.odometryDoubleInputs = odometryDoubleInputs; this.statusSignals = statusSignals; @@ -48,15 +52,15 @@ private void odometryPeriodic() { } private void refreshSignalsAndBlockThread() { - switch (Constants.SwerveDriveChassisConfigs.SWERVE_DRIVE_TYPE) { - case REV -> - MapleTimeUtils.delay(1.0 / Constants.SwerveDriveChassisConfigs.ODOMETRY_FREQUENCY); + switch (driveType) { + case GENERIC -> + MapleTimeUtils.delay(1.0 / ODOMETRY_FREQUENCY); case CTRE_ON_RIO -> { - MapleTimeUtils.delay(1.0 / Constants.SwerveDriveChassisConfigs.ODOMETRY_FREQUENCY); + MapleTimeUtils.delay(1.0 / ODOMETRY_FREQUENCY); BaseStatusSignal.refreshAll(); } case CTRE_ON_CANIVORE -> - BaseStatusSignal.waitForAll(Constants.SwerveDriveChassisConfigs.ODOMETRY_WAIT_TIMEOUT_SECONDS, statusSignals); + BaseStatusSignal.waitForAll(ODOMETRY_WAIT_TIMEOUT_SECONDS, statusSignals); } } diff --git a/src/main/java/frc/robot/subsystems/drive/SwerveDrive.java b/src/main/java/frc/robot/subsystems/drive/SwerveDrive.java index 444731a..c532cf7 100644 --- a/src/main/java/frc/robot/subsystems/drive/SwerveDrive.java +++ b/src/main/java/frc/robot/subsystems/drive/SwerveDrive.java @@ -9,7 +9,6 @@ import edu.wpi.first.math.estimator.SwerveDrivePoseEstimator; 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.geometry.Twist2d; import edu.wpi.first.math.kinematics.ChassisSpeeds; import edu.wpi.first.math.kinematics.SwerveDriveKinematics; @@ -18,26 +17,29 @@ import edu.wpi.first.math.numbers.N1; import edu.wpi.first.math.numbers.N3; import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; -import frc.robot.Constants; +import frc.robot.constants.FieldConstants; import frc.robot.subsystems.MapleSubsystem; import frc.robot.subsystems.drive.IO.*; import frc.robot.utils.Alert; -import frc.robot.utils.CustomConfigs.MapleConfigFile; import frc.robot.utils.MapleTimeUtils; import org.littletonrobotics.junction.AutoLogOutput; import org.littletonrobotics.junction.Logger; -import static frc.robot.Constants.VisionConfigs.*; +import static frc.robot.constants.Constants.VisionConfigs.*; +import static frc.robot.constants.DriveTrainConstants.*; public class SwerveDrive extends MapleSubsystem implements HolonomicDriveSubsystem { - public final double maxModuleVelocityMetersPerSec, maxAngularVelocityRadPerSec, maxLinearAccelerationMetersPerSecSq; + public enum DriveType { + GENERIC, + CTRE_ON_RIO, + CTRE_ON_CANIVORE + } + private final GyroIO gyroIO; private final GyroIOInputsAutoLogged gyroInputs; private final OdometryThreadInputsAutoLogged odometryThreadInputs; private final SwerveModule[] swerveModules; - private final Translation2d[] MODULE_TRANSLATIONS; - private final double DRIVE_BASE_RADIUS; public final SwerveDriveKinematics kinematics; private Rotation2d rawGyroRotation; private final SwerveModulePosition[] lastModulePositions; @@ -45,7 +47,7 @@ public class SwerveDrive extends MapleSubsystem implements HolonomicDriveSubsyst private final OdometryThread odometryThread; private final Alert gyroDisconnectedAlert = new Alert("Gyro Hardware Fault", Alert.AlertType.ERROR), visionNoResultAlert = new Alert("Vision No Result", Alert.AlertType.INFO); - public SwerveDrive(GyroIO gyroIO, ModuleIO frontLeftModuleIO, ModuleIO frontRightModuleIO, ModuleIO backLeftModuleIO, ModuleIO backRightModuleIO, MapleConfigFile.ConfigBlock generalConfigBlock) { + public SwerveDrive(DriveType type, GyroIO gyroIO, ModuleIO frontLeftModuleIO, ModuleIO frontRightModuleIO, ModuleIO backLeftModuleIO, ModuleIO backRightModuleIO) { super("Drive"); this.gyroIO = gyroIO; this.gyroInputs = new GyroIOInputsAutoLogged(); @@ -57,19 +59,6 @@ public SwerveDrive(GyroIO gyroIO, ModuleIO frontLeftModuleIO, ModuleIO frontRigh new SwerveModule(backRightModuleIO, "BackRight"), }; - final double horizontalWheelsMarginMeters = generalConfigBlock.getDoubleConfig("horizontalWheelsMarginMeters"), - verticalWheelsMarginMeters = generalConfigBlock.getDoubleConfig("verticalWheelsMarginMeters"); - DRIVE_BASE_RADIUS = Math.hypot(horizontalWheelsMarginMeters/2, verticalWheelsMarginMeters/2); - - this.maxModuleVelocityMetersPerSec = generalConfigBlock.getDoubleConfig("maxVelocityMetersPerSecond"); - this.maxAngularVelocityRadPerSec = generalConfigBlock.getDoubleConfig("maxAngularVelocityRadiansPerSecond"); - this.maxLinearAccelerationMetersPerSecSq = generalConfigBlock.getDoubleConfig("maxAccelerationMetersPerSecondSquared"); - this.MODULE_TRANSLATIONS = new Translation2d[] { - new Translation2d(horizontalWheelsMarginMeters / 2, verticalWheelsMarginMeters / 2), // FL - new Translation2d(horizontalWheelsMarginMeters / 2, -verticalWheelsMarginMeters / 2), // FR - new Translation2d(-horizontalWheelsMarginMeters / 2, verticalWheelsMarginMeters / 2), // BL - new Translation2d(-horizontalWheelsMarginMeters / 2, -verticalWheelsMarginMeters / 2) // BR - }; kinematics = new SwerveDriveKinematics(MODULE_TRANSLATIONS); lastModulePositions = new SwerveModulePosition[] {new SwerveModulePosition(), new SwerveModulePosition(), new SwerveModulePosition(), new SwerveModulePosition()}; this.poseEstimator = new SwerveDrivePoseEstimator( @@ -78,7 +67,7 @@ kinematics, rawGyroRotation, lastModulePositions, new Pose2d(), VecBuilder.fill(TRANSLATIONAL_STANDARD_ERROR_METERS_FOR_SINGLE_OBSERVATION, TRANSLATIONAL_STANDARD_ERROR_METERS_FOR_SINGLE_OBSERVATION, ROTATIONAL_STANDARD_ERROR_RADIANS_FOR_SINGLE_OBSERVATION) ); - this.odometryThread = OdometryThread.createInstance(); + this.odometryThread = OdometryThread.createInstance(type); this.odometryThreadInputs = new OdometryThreadInputsAutoLogged(); this.odometryThread.start(); @@ -179,7 +168,7 @@ private void updateRobotFacingWithOdometry(SwerveModulePosition[] modulesDelta) @Override public void runRawChassisSpeeds(ChassisSpeeds speeds) { SwerveModuleState[] setpointStates = kinematics.toSwerveModuleStates(speeds); - SwerveDriveKinematics.desaturateWheelSpeeds(setpointStates, maxModuleVelocityMetersPerSec); + SwerveDriveKinematics.desaturateWheelSpeeds(setpointStates, CHASSIS_MAX_VELOCITY); // Send setpoints to modules SwerveModuleState[] optimizedSetpointStates = new SwerveModuleState[4]; @@ -250,10 +239,10 @@ public ChassisSpeeds getMeasuredChassisSpeedsRobotRelative() { return kinematics.toChassisSpeeds(getModuleStates()); } - @Override public double getChassisMaxLinearVelocityMetersPerSec() {return maxModuleVelocityMetersPerSec;} - @Override public double getChassisMaxAccelerationMetersPerSecSq() {return maxLinearAccelerationMetersPerSecSq;} - @Override public double getChassisMaxAngularVelocity() {return maxAngularVelocityRadPerSec;} - @Override public double getChassisMaxAngularAccelerationRadPerSecSq() {return maxAngularVelocityRadPerSec / 0.7;} + @Override public double getChassisMaxLinearVelocityMetersPerSec() {return CHASSIS_MAX_VELOCITY;} + @Override public double getChassisMaxAccelerationMetersPerSecSq() {return CHASSIS_MAX_ACCELERATION_MPS_SQ;} + @Override public double getChassisMaxAngularVelocity() {return CHASSIS_MAX_ANGULAR_VELOCITY_RAD_PER_SEC;} + @Override public double getChassisMaxAngularAccelerationRadPerSecSq() {return CHASSIS_MAX_ANGULAR_ACCELERATION_RAD_PER_SEC_SQ;} @Override public void addVisionMeasurement(Pose2d visionPose, double timestamp, Matrix measurementStdDevs) { @@ -283,7 +272,7 @@ private void startDashboardDisplay() { builder.addDoubleProperty("Back Right Angle", () -> swerveModules[0].getSteerFacing().getRadians(), null); builder.addDoubleProperty("Back Right Velocity", () -> swerveModules[0].getDriveVelocityMetersPerSec(), null); - builder.addDoubleProperty("Robot Angle", () -> getFacing().minus(Constants.getDriverStationFacing()).getRadians(), null); + builder.addDoubleProperty("Robot Angle", () -> getFacing().minus(FieldConstants.getDriverStationFacing()).getRadians(), null); }); } } diff --git a/src/main/java/frc/robot/subsystems/drive/SwerveModule.java b/src/main/java/frc/robot/subsystems/drive/SwerveModule.java index 74d510d..7c2ba00 100644 --- a/src/main/java/frc/robot/subsystems/drive/SwerveModule.java +++ b/src/main/java/frc/robot/subsystems/drive/SwerveModule.java @@ -10,7 +10,7 @@ import edu.wpi.first.math.kinematics.SwerveModuleState; import edu.wpi.first.math.util.Units; import edu.wpi.first.wpilibj2.command.CommandScheduler; -import frc.robot.Constants; +import frc.robot.constants.DriveTrainConstants; import frc.robot.subsystems.MapleSubsystem; import frc.robot.subsystems.drive.IO.ModuleIO; import frc.robot.subsystems.drive.IO.ModuleIOInputsAutoLogged; @@ -18,7 +18,7 @@ import frc.robot.utils.CustomMaths.SwerveStateProjection; import frc.robot.utils.CustomPIDs.MaplePIDController; -import static frc.robot.Constants.SwerveModuleConfigs.*; +import static frc.robot.constants.DriveControlLoops.*; public class SwerveModule extends MapleSubsystem { private final ModuleIO io; @@ -122,7 +122,7 @@ public double getDrivePositionMeters() { } private double driveWheelRevolutionsToMeters(double driveWheelRevolutions) { - return Units.rotationsToRadians(driveWheelRevolutions) * Constants.SwerveModuleConfigs.WHEEL_RADIUS; + return Units.rotationsToRadians(driveWheelRevolutions) * DriveTrainConstants.WHEEL_RADIUS_METERS; } /** diff --git a/src/main/java/frc/robot/subsystems/led/LEDStatusLight.java b/src/main/java/frc/robot/subsystems/led/LEDStatusLight.java index 7ad3bce..983aa8a 100644 --- a/src/main/java/frc/robot/subsystems/led/LEDStatusLight.java +++ b/src/main/java/frc/robot/subsystems/led/LEDStatusLight.java @@ -7,7 +7,7 @@ import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.Commands; -import frc.robot.Constants; +import frc.robot.constants.Constants; import frc.robot.Robot; import frc.robot.subsystems.MapleSubsystem; import frc.robot.utils.LEDAnimation; diff --git a/src/main/java/frc/robot/subsystems/vision/apriltags/AprilTagVision.java b/src/main/java/frc/robot/subsystems/vision/apriltags/AprilTagVision.java index e6f7ee8..3f66359 100644 --- a/src/main/java/frc/robot/subsystems/vision/apriltags/AprilTagVision.java +++ b/src/main/java/frc/robot/subsystems/vision/apriltags/AprilTagVision.java @@ -12,8 +12,8 @@ import java.util.Optional; import java.util.function.Function; -import static frc.robot.Constants.VisionConfigs.*; -import static frc.robot.Constants.LogConfigs.APRIL_TAGS_VISION_PATH; +import static frc.robot.constants.Constants.VisionConfigs.*; +import static frc.robot.constants.LogPaths.*; import static frc.robot.subsystems.vision.apriltags.MapleMultiTagPoseEstimator.RobotPoseEstimationResult; public class AprilTagVision extends MapleSubsystem { diff --git a/src/main/java/frc/robot/subsystems/vision/apriltags/MapleMultiTagPoseEstimator.java b/src/main/java/frc/robot/subsystems/vision/apriltags/MapleMultiTagPoseEstimator.java index 241c443..1b14f57 100644 --- a/src/main/java/frc/robot/subsystems/vision/apriltags/MapleMultiTagPoseEstimator.java +++ b/src/main/java/frc/robot/subsystems/vision/apriltags/MapleMultiTagPoseEstimator.java @@ -6,7 +6,7 @@ import edu.wpi.first.math.geometry.*; import edu.wpi.first.math.numbers.N1; import edu.wpi.first.math.numbers.N3; -import frc.robot.Constants; +import frc.robot.constants.Constants; import frc.robot.Robot; import frc.robot.utils.CustomConfigs.PhotonCameraProperties; import frc.robot.utils.CustomMaths.Statistics; @@ -16,8 +16,8 @@ import java.util.List; import java.util.Optional; -import static frc.robot.Constants.VisionConfigs.*; -import static frc.robot.Constants.LogConfigs.APRIL_TAGS_VISION_PATH; +import static frc.robot.constants.Constants.VisionConfigs.*; +import static frc.robot.constants.LogPaths.APRIL_TAGS_VISION_PATH; public class MapleMultiTagPoseEstimator { public static final class RobotPoseEstimationResult { diff --git a/src/main/java/frc/robot/tests/WheelsCalibrationCTRE.java b/src/main/java/frc/robot/tests/WheelsCalibrationCTRE.java deleted file mode 100644 index b636dfc..0000000 --- a/src/main/java/frc/robot/tests/WheelsCalibrationCTRE.java +++ /dev/null @@ -1,172 +0,0 @@ -package frc.robot.tests; - -import com.ctre.phoenix6.hardware.CANcoder; -import com.ctre.phoenix6.hardware.TalonFX; -import com.ctre.phoenix6.signals.NeutralModeValue; -import edu.wpi.first.wpilibj.XboxController; -import edu.wpi.first.wpilibj.smartdashboard.SendableChooser; -import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; -import edu.wpi.first.wpilibj2.command.Command; -import edu.wpi.first.wpilibj2.command.InstantCommand; -import frc.robot.Constants; -import frc.robot.utils.CustomConfigs.MapleConfigFile; -import frc.robot.utils.CustomMaths.Angles; - -import java.io.IOException; - -public class WheelsCalibrationCTRE extends Command { - public static final class ChassisDefaultConfigs { - public static final int DEFAULT_GYRO_PORT = 0; - public static final double DEFAULT_GEAR_RATIO = 6.12; - public static final double DEFAULT_WHEEL_RADIUS_METERS = 0.051; // 2 inch - public static final double DEFAULT_HORIZONTAL_WHEELS_MARGIN_METERS = 0.53; - public static final double DEFAULT_VERTICAL_WHEELS_MARGIN_METERS = 0.53; - public static final double DEFAULT_MAX_VELOCITY_METERS_PER_SECOND = 4.172; // calculated from Choreo (Kraken x60 motor, 6.12 gear ratio, 55kg robot mass) - public static final double DEFAULT_MAX_ACCELERATION_METERS_PER_SQUARED_SECOND = 10.184; // calculated from Choreo (Kraken x60 motor, 6.12 gear ratio, 55kg robot mass) - public static final double DEFAULT_MAX_ANGULAR_VELOCITY_DEGREES_PER_SECOND = 540; - } - - public static final class WheelToBeCalibrated { - public final String name; - public final int drivingMotorID, steeringMotorID, encoderID, drivingMotorPortOnPDP, steeringMotorPortOnPDP; - public boolean steeringMotorInverted; - - public WheelToBeCalibrated(String name, int drivingMotorID, int steeringMotorID, int encoderID) { - this(name, drivingMotorID, steeringMotorID, encoderID, -1, -1); - } - - private WheelToBeCalibrated(String name, int drivingMotorID, int steeringMotorID, int encoderID, int drivingMotorPortOnPDP, int steeringMotorPortOnPDP) { - this.name = name; - this.drivingMotorID = drivingMotorID; - this.steeringMotorID = steeringMotorID; - this.encoderID = encoderID; - this.drivingMotorPortOnPDP = drivingMotorPortOnPDP; - this.steeringMotorPortOnPDP = steeringMotorPortOnPDP; - this.steeringMotorInverted = false; - } - } - public static final WheelToBeCalibrated[] WHEELS_TO_BE_CALIBRATED = new WheelToBeCalibrated[] { - new WheelToBeCalibrated("FrontLeft", 1, 2, 1), - new WheelToBeCalibrated("FrontRight", 3, 4, 2), - new WheelToBeCalibrated("BackLeft", 5, 6, 3), - new WheelToBeCalibrated("BackRight", 7, 8, 4) - }; - private enum SteerWheelTurningDirection { - NOT_INVERTED, - INVERTED - } - private final SendableChooser wheelTurningDirectionSendableChooser = new SendableChooser<>(); - private final SendableChooser wheelSendableChooser = new SendableChooser<>(); - private final String configName; - private final MapleConfigFile calibrationFile; - public WheelsCalibrationCTRE() { - configName = "5516-2024-OffSeason"; - for (WheelToBeCalibrated wheelToBeCalibrated: WHEELS_TO_BE_CALIBRATED) - wheelSendableChooser.addOption(wheelToBeCalibrated.name, wheelToBeCalibrated); - wheelSendableChooser.setDefaultOption(WHEELS_TO_BE_CALIBRATED[0].name, WHEELS_TO_BE_CALIBRATED[0]); - wheelSendableChooser.onChange(this::initHardware); - - SmartDashboard.putData("Calibration/Select Wheel to Calibrate", wheelSendableChooser); - - this.calibrationFile = new MapleConfigFile("ChassisWheelsCalibration", configName); - - wheelTurningDirectionSendableChooser.setDefaultOption(SteerWheelTurningDirection.NOT_INVERTED.name(), SteerWheelTurningDirection.NOT_INVERTED); - wheelTurningDirectionSendableChooser.addOption(SteerWheelTurningDirection.INVERTED.name(), SteerWheelTurningDirection.INVERTED); - } - - @Override - public void initialize() { - initHardware(wheelSendableChooser.getSelected()); - SmartDashboard.putData("Calibration/Steer Motor Turning Direction (Should be Spinning Counter-Clockwise)", wheelTurningDirectionSendableChooser); - SmartDashboard.putData("Calibration/MoveDrivingWheel", new InstantCommand( - () -> drivingMotor.set(0.3)) - .finallyDo(() -> drivingMotor.set(0)) - ); - SmartDashboard.putData("Calibration/MoveSteeringWheel", new InstantCommand( - () -> steeringMotor.set(wheelSendableChooser.getSelected().steeringMotorInverted ? -0.2:0.2)) - .finallyDo(() -> steeringMotor.set(0)) - ); - - SmartDashboard.putData("Calibration/Finish", new InstantCommand(() -> finished = true)); - finished = false; - } - - - private TalonFX drivingMotor, steeringMotor; - private CANcoder canCoder; - private boolean finished; - - private final XboxController controller = new XboxController(1); - @Override - public void execute() { - wheelSendableChooser.getSelected().steeringMotorInverted = switch (wheelTurningDirectionSendableChooser.getSelected()){ - case NOT_INVERTED -> false; - case INVERTED -> true; - }; - - for (int i = 0; i < WHEELS_TO_BE_CALIBRATED.length; i++) { - final CANcoder canCoder = new CANcoder(WHEELS_TO_BE_CALIBRATED[i].encoderID, Constants.SwerveDriveChassisConfigs.CHASSIS_CANBUS); - SmartDashboard.putNumber("Calibration/" + WHEELS_TO_BE_CALIBRATED[i].name + " encoder (rad): ", getCanCoderReadingRadian(canCoder)); - } - - SmartDashboard.putNumber("Calibration/Can Coder Reading (Rad)", getCanCoderReadingRadian(canCoder)); - } - - private double getCanCoderReadingRadian(CANcoder canCoder) { - return Angles.simplifyAngle(canCoder.getAbsolutePosition().getValue() * Math.PI * 2); - } - - private void saveConfigurationForCurrentWheel(WheelToBeCalibrated wheelToBeCalibrated) { - final MapleConfigFile.ConfigBlock configBlock = calibrationFile.getBlock(wheelToBeCalibrated.name); - configBlock.putIntConfig("drivingMotorID", wheelToBeCalibrated.drivingMotorID); - configBlock.putIntConfig("drivingMotorPortOnPDP", wheelToBeCalibrated.drivingMotorPortOnPDP); - configBlock.putIntConfig("steeringMotorID", wheelToBeCalibrated.steeringMotorID); - configBlock.putIntConfig("steeringMotorPortOnPDP", wheelToBeCalibrated.steeringMotorPortOnPDP); - configBlock.putIntConfig("steeringEncoderID", wheelToBeCalibrated.encoderID); - - configBlock.putIntConfig("steeringMotorInverted", wheelToBeCalibrated.steeringMotorInverted ? 1 : 0); - - final CANcoder canCoder = new CANcoder(wheelToBeCalibrated.encoderID, Constants.SwerveDriveChassisConfigs.CHASSIS_CANBUS); - configBlock.putDoubleConfig("steeringEncoderReadingAtOrigin", getCanCoderReadingRadian(canCoder)); - } - - private void initHardware(WheelToBeCalibrated wheelToBeCalibrated) { - drivingMotor = new TalonFX(wheelToBeCalibrated.drivingMotorID, Constants.SwerveDriveChassisConfigs.CHASSIS_CANBUS); - drivingMotor.setNeutralMode(NeutralModeValue.Coast); - steeringMotor = new TalonFX(wheelToBeCalibrated.steeringMotorID, Constants.SwerveDriveChassisConfigs.CHASSIS_CANBUS); - steeringMotor.setNeutralMode(NeutralModeValue.Coast); - steeringMotor.setInverted(false); - canCoder = new CANcoder(wheelToBeCalibrated.encoderID, Constants.SwerveDriveChassisConfigs.CHASSIS_CANBUS); - } - - @Override public boolean isFinished() { - return finished; - } - - @Override - public void end(boolean interrupted) { - System.out.println("writing configs to usb"); - final MapleConfigFile.ConfigBlock configBlock = calibrationFile.getBlock("GeneralInformation"); - configBlock.putIntConfig("gyroPort", ChassisDefaultConfigs.DEFAULT_GYRO_PORT); - configBlock.putDoubleConfig("overallGearRatio", ChassisDefaultConfigs.DEFAULT_GEAR_RATIO); - configBlock.putDoubleConfig("wheelRadiusMeters", ChassisDefaultConfigs.DEFAULT_WHEEL_RADIUS_METERS); - configBlock.putDoubleConfig("horizontalWheelsMarginMeters", ChassisDefaultConfigs.DEFAULT_HORIZONTAL_WHEELS_MARGIN_METERS); - configBlock.putDoubleConfig("verticalWheelsMarginMeters", ChassisDefaultConfigs.DEFAULT_VERTICAL_WHEELS_MARGIN_METERS); - configBlock.putDoubleConfig("maxVelocityMetersPerSecond", ChassisDefaultConfigs.DEFAULT_MAX_VELOCITY_METERS_PER_SECOND); - configBlock.putDoubleConfig("maxAccelerationMetersPerSecondSquared", ChassisDefaultConfigs.DEFAULT_MAX_ACCELERATION_METERS_PER_SQUARED_SECOND); - configBlock.putDoubleConfig("maxAngularVelocityRadiansPerSecond", Math.toRadians(ChassisDefaultConfigs.DEFAULT_MAX_ANGULAR_VELOCITY_DEGREES_PER_SECOND)); - - configBlock.putDoubleConfig("bumperWidthMeters", Constants.DriveTrainPhysicsSimulationConstants.DEFAULT_BUMPER_WIDTH_METERS); - configBlock.putDoubleConfig("bumperLengthMeters", Constants.DriveTrainPhysicsSimulationConstants.DEFAULT_BUMPER_LENGTH_METERS); - configBlock.putDoubleConfig("robotMassInSimulation", Constants.DriveTrainPhysicsSimulationConstants.DEFAULT_ROBOT_MASS); - - for (WheelToBeCalibrated wheelToBeCalibrated: WHEELS_TO_BE_CALIBRATED) - saveConfigurationForCurrentWheel(wheelToBeCalibrated); - - try { - calibrationFile.saveConfigToUSB(); - } catch (IOException e) { - throw new RuntimeException(e); - } - } -} diff --git a/src/main/java/frc/robot/utils/CompetitionFieldUtils/Objects/Crescendo2024FieldObjects.java b/src/main/java/frc/robot/utils/CompetitionFieldUtils/Objects/Crescendo2024FieldObjects.java index 2192c61..203e4eb 100644 --- a/src/main/java/frc/robot/utils/CompetitionFieldUtils/Objects/Crescendo2024FieldObjects.java +++ b/src/main/java/frc/robot/utils/CompetitionFieldUtils/Objects/Crescendo2024FieldObjects.java @@ -2,7 +2,7 @@ import edu.wpi.first.math.geometry.*; import edu.wpi.first.math.util.Units; -import frc.robot.Constants; +import frc.robot.constants.FieldConstants; import org.dyn4j.geometry.Geometry; /** @@ -71,7 +71,7 @@ public static class NoteOnFly extends GamePieceOnFlyDisplay { public NoteOnFly(Translation3d shooterPosition, double flightTimeSeconds) { super( shooterPosition, - Constants.toCurrentAllianceTranslation(Constants.CrescendoField2024Constants.SPEAKER_POSE_BLUE), + FieldConstants.toCurrentAllianceTranslation(FieldConstants.SPEAKER_POSE_BLUE), flightTimeSeconds ); } diff --git a/src/main/java/frc/robot/utils/CompetitionFieldUtils/Simulations/CompetitionFieldSimulation.java b/src/main/java/frc/robot/utils/CompetitionFieldUtils/Simulations/CompetitionFieldSimulation.java index b79117f..a25d5c5 100644 --- a/src/main/java/frc/robot/utils/CompetitionFieldUtils/Simulations/CompetitionFieldSimulation.java +++ b/src/main/java/frc/robot/utils/CompetitionFieldUtils/Simulations/CompetitionFieldSimulation.java @@ -20,7 +20,7 @@ import java.util.List; import java.util.Set; -import static frc.robot.Constants.DriveTrainPhysicsSimulationConstants.*; +import static frc.robot.constants.DriveTrainConstants.*; /** * this class simulates the physical behavior of all the objects on field @@ -49,9 +49,9 @@ public CompetitionFieldSimulation(HolonomicChassisSimulation mainRobot, FieldObs } public void updateSimulationWorld() { - final double subPeriodSeconds = Robot.defaultPeriodSecs / SIM_ITERATIONS_PER_ROBOT_PERIOD; + final double subPeriodSeconds = Robot.defaultPeriodSecs / SIMULATION_TICKS_IN_1_PERIOD; // move through 5 sub-periods in each update - for (int i = 0; i < SIM_ITERATIONS_PER_ROBOT_PERIOD; i++) { + for (int i = 0; i < SIMULATION_TICKS_IN_1_PERIOD; i++) { this.physicsWorld.step(1, subPeriodSeconds); for (HolonomicChassisSimulation robotSimulation:robotSimulations) robotSimulation.updateSimulationSubPeriod(i, subPeriodSeconds); diff --git a/src/main/java/frc/robot/utils/CompetitionFieldUtils/Simulations/Crescendo2024FieldSimulation.java b/src/main/java/frc/robot/utils/CompetitionFieldUtils/Simulations/Crescendo2024FieldSimulation.java index fde889f..0f90929 100644 --- a/src/main/java/frc/robot/utils/CompetitionFieldUtils/Simulations/Crescendo2024FieldSimulation.java +++ b/src/main/java/frc/robot/utils/CompetitionFieldUtils/Simulations/Crescendo2024FieldSimulation.java @@ -5,7 +5,7 @@ import edu.wpi.first.math.geometry.Translation2d; import frc.robot.utils.CompetitionFieldUtils.Objects.Crescendo2024FieldObjects; -import static frc.robot.Constants.CrescendoField2024Constants.*; +import static frc.robot.constants.FieldConstants.*; /** * field simulation for 2024 competition diff --git a/src/main/java/frc/robot/utils/CompetitionFieldUtils/Simulations/HolonomicChassisSimulation.java b/src/main/java/frc/robot/utils/CompetitionFieldUtils/Simulations/HolonomicChassisSimulation.java index 8b5c0ef..e02ffe5 100644 --- a/src/main/java/frc/robot/utils/CompetitionFieldUtils/Simulations/HolonomicChassisSimulation.java +++ b/src/main/java/frc/robot/utils/CompetitionFieldUtils/Simulations/HolonomicChassisSimulation.java @@ -2,9 +2,9 @@ import edu.wpi.first.math.geometry.Pose2d; import edu.wpi.first.math.kinematics.ChassisSpeeds; -import frc.robot.Constants; +import frc.robot.constants.Constants; +import frc.robot.constants.DriveTrainConstants; import frc.robot.utils.CompetitionFieldUtils.Objects.RobotOnFieldDisplay; -import frc.robot.utils.CustomConfigs.MapleConfigFile; import frc.robot.utils.CustomMaths.GeometryConvertor; import org.dyn4j.dynamics.Body; import org.dyn4j.dynamics.Force; @@ -19,18 +19,18 @@ * with respect to its collision space, friction and motor propelling forces * */ public abstract class HolonomicChassisSimulation extends Body implements RobotOnFieldDisplay { - public final RobotProfile profile; - public HolonomicChassisSimulation(RobotProfile profile, Pose2d startingPose) { + public final RobotSimulationProfile profile; + public HolonomicChassisSimulation(RobotSimulationProfile profile, Pose2d startingPose) { this.profile = profile; - /* the bumper needs to be rotated 90 degrees */ + /* width and height in world reference is flipped */ final double WIDTH_IN_WORLD_REFERENCE = profile.height, HEIGHT_IN_WORLD_REFERENCE = profile.width; super.addFixture( Geometry.createRectangle(WIDTH_IN_WORLD_REFERENCE, HEIGHT_IN_WORLD_REFERENCE), profile.robotMass / (profile.height * profile.width), - Constants.DriveTrainPhysicsSimulationConstants.ROBOT_BUMPER_COEFFICIENT_OF_FRICTION, - Constants.DriveTrainPhysicsSimulationConstants.ROBOT_BUMPER_COEFFICIENT_OF_RESTITUTION + DriveTrainConstants.BUMPER_COEFFICIENT_OF_FRICTION, + DriveTrainConstants.BUMPER_COEFFICIENT_OF_RESTITUTION ); super.setMass(MassType.NORMAL); @@ -127,7 +127,7 @@ public ChassisSpeeds getMeasuredChassisSpeedsFieldRelative() { * */ public abstract void updateSimulationSubPeriod(int iterationNum, double subPeriodSeconds); - public static final class RobotProfile { + public static final class RobotSimulationProfile { public final double robotMaxVelocity, robotMaxAcceleration, @@ -142,41 +142,28 @@ public static final class RobotProfile { width, height; - public RobotProfile(MapleConfigFile.ConfigBlock chassisGeneralInformation) { + public RobotSimulationProfile() { this( - chassisGeneralInformation.getDoubleConfig("maxVelocityMetersPerSecond"), - chassisGeneralInformation.getDoubleConfig("maxAccelerationMetersPerSecondSquared"), - chassisGeneralInformation.getDoubleConfig("maxAngularVelocityRadiansPerSecond"), - chassisGeneralInformation.getDoubleConfig("robotMassInSimulation"), - chassisGeneralInformation.getDoubleConfig("bumperWidthMeters"), - chassisGeneralInformation.getDoubleConfig("bumperLengthMeters") + DriveTrainConstants.CHASSIS_MAX_VELOCITY, + DriveTrainConstants.CHASSIS_MAX_ACCELERATION_MPS_SQ, + DriveTrainConstants.CHASSIS_MAX_ANGULAR_VELOCITY_RAD_PER_SEC, + DriveTrainConstants.ROBOT_MASS_KG, + DriveTrainConstants.BUMPER_WIDTH_METERS, + DriveTrainConstants.BUMPER_LENGTH_METERS ); } - public RobotProfile(double robotMaxVelocity, double robotMaxAcceleration, double maxAngularVelocity, double robotMass, double width, double height) { - this( - robotMaxVelocity, - robotMaxAcceleration, - Constants.DriveTrainPhysicsSimulationConstants.FLOOR_FRICTION_ACCELERATION_METERS_PER_SEC_SQ, - maxAngularVelocity, - Constants.DriveTrainPhysicsSimulationConstants.MAX_ANGULAR_ACCELERATION_RAD_PER_SEC_SQ, - Constants.DriveTrainPhysicsSimulationConstants.TIME_CHASSIS_STOPS_ROTATING_NO_POWER_SEC, - robotMass, - width, height - ); - } - - public RobotProfile(double robotMaxVelocity, double robotMaxAcceleration, double floorFrictionAcceleration, double maxAngularVelocity, double maxAngularAcceleration, double timeChassisStopsRotating, double robotMass, double width, double height) { + public RobotSimulationProfile(double robotMaxVelocity, double robotMaxAcceleration, double maxAngularVelocity, double robotMass, double width, double height) { this.robotMaxVelocity = robotMaxVelocity; this.robotMaxAcceleration = robotMaxAcceleration; this.robotMass = robotMass; this.propellingForce = robotMaxAcceleration * robotMass; - this.frictionForce = floorFrictionAcceleration * robotMass; + this.frictionForce = DriveTrainConstants.MAX_FRICTION_ACCELERATION * robotMass; this.linearVelocityDamping = robotMaxAcceleration / robotMaxVelocity; this.maxAngularVelocity = maxAngularVelocity; - this.maxAngularAcceleration = maxAngularAcceleration; + this.maxAngularAcceleration = robotMaxAcceleration / (Math.hypot(width, height) / 2); this.angularDamping = maxAngularAcceleration / maxAngularVelocity; - this.angularFrictionAcceleration = maxAngularVelocity / timeChassisStopsRotating; + this.angularFrictionAcceleration = DriveTrainConstants.CHASSIS_FRICTIONAL_ANGULAR_ACCELERATION; this.width = width; this.height = height; } diff --git a/src/main/java/frc/robot/utils/CompetitionFieldUtils/Simulations/OpponentRobotSimulation.java b/src/main/java/frc/robot/utils/CompetitionFieldUtils/Simulations/OpponentRobotSimulation.java index 25809c8..8d9176e 100644 --- a/src/main/java/frc/robot/utils/CompetitionFieldUtils/Simulations/OpponentRobotSimulation.java +++ b/src/main/java/frc/robot/utils/CompetitionFieldUtils/Simulations/OpponentRobotSimulation.java @@ -11,8 +11,10 @@ import edu.wpi.first.wpilibj.smartdashboard.SendableChooser; import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import edu.wpi.first.wpilibj2.command.*; -import frc.robot.Constants; +import frc.robot.constants.Constants; import frc.robot.commands.drive.CustomFollowPathOnFly; +import frc.robot.constants.DriveTrainConstants; +import frc.robot.constants.FieldConstants; import frc.robot.subsystems.drive.HolonomicDriveSubsystem; import frc.robot.utils.MapleJoystickDriveInput; import frc.robot.utils.MaplePathPlannerLoader; @@ -36,13 +38,13 @@ public class OpponentRobotSimulation extends HolonomicChassisSimulation implemen new Pose2d(15.2, 6, new Rotation2d()), new Pose2d(15.2, 5.5, new Rotation2d()) }; - public static final HolonomicChassisSimulation.RobotProfile opponentRobotProfile = new RobotProfile( + public static final RobotSimulationProfile opponentRobotProfile = new RobotSimulationProfile( 4, 12, Math.toRadians(360), Units.lbsToKilograms(125), - Constants.DriveTrainPhysicsSimulationConstants.DEFAULT_BUMPER_WIDTH_METERS, - Constants.DriveTrainPhysicsSimulationConstants.DEFAULT_BUMPER_LENGTH_METERS + DriveTrainConstants.BUMPER_WIDTH_METERS, + DriveTrainConstants.BUMPER_LENGTH_METERS ); private final int robotID; @@ -111,7 +113,7 @@ public Command getAutoCyleRepeadtelyCommand() { final Command teleportToStartingPose = Commands.runOnce(() -> setSimulationWorldPose(cycleBackwardPath.getPreviewStartingHolonomicPose()), this), cycleForward = new CustomFollowPathOnFly( this, - () -> Constants.isSidePresentedAsRed() ? cycleForwardPath.flipPath() : cycleForwardPath, + () -> FieldConstants.isSidePresentedAsRed() ? cycleForwardPath.flipPath() : cycleForwardPath, tolerance, 1, false, @@ -119,7 +121,7 @@ public Command getAutoCyleRepeadtelyCommand() { ), cycleBackWards = new CustomFollowPathOnFly( this, - () -> Constants.isSidePresentedAsRed() ? cycleBackwardPath.flipPath() : cycleBackwardPath, + () -> FieldConstants.isSidePresentedAsRed() ? cycleBackwardPath.flipPath() : cycleBackwardPath, tolerance, 1, false, @@ -143,7 +145,7 @@ public Command getAutoCyleRepeadtelyCommand() { } public Command getJoystickDrive(MapleJoystickDriveInput joystickDriveInput) { - final Pose2d startingPose = Constants.toCurrentAlliancePose(RED_ROBOTS_STARTING_POSITIONS[robotID]), + final Pose2d startingPose = FieldConstants.toCurrentAlliancePose(RED_ROBOTS_STARTING_POSITIONS[robotID]), queeningPose = ROBOT_QUEENING_POSITIONS[robotID]; final Command teleportToStartingPose = Commands.runOnce(() -> setSimulationWorldPose(startingPose), this); Runnable end = () -> { diff --git a/src/main/java/frc/robot/utils/CompetitionFieldUtils/Simulations/SwerveDriveSimulation.java b/src/main/java/frc/robot/utils/CompetitionFieldUtils/Simulations/SwerveDriveSimulation.java index 18a3c24..5f8c27d 100644 --- a/src/main/java/frc/robot/utils/CompetitionFieldUtils/Simulations/SwerveDriveSimulation.java +++ b/src/main/java/frc/robot/utils/CompetitionFieldUtils/Simulations/SwerveDriveSimulation.java @@ -7,12 +7,11 @@ import edu.wpi.first.math.kinematics.SwerveDriveKinematics; import edu.wpi.first.math.kinematics.SwerveModuleState; import edu.wpi.first.math.util.Units; -import frc.robot.Constants; import frc.robot.Robot; +import frc.robot.constants.LogPaths; import frc.robot.subsystems.drive.IO.GyroIOSim; import frc.robot.subsystems.drive.IO.ModuleIOSim; import frc.robot.subsystems.drive.IO.OdometryThread; -import frc.robot.utils.CustomConfigs.MapleConfigFile; import frc.robot.utils.CustomMaths.SwerveStateProjection; import frc.robot.utils.MapleTimeUtils; import org.dyn4j.geometry.Vector2; @@ -20,7 +19,7 @@ import java.util.Arrays; import java.util.function.Consumer; -import static frc.robot.Constants.DriveTrainPhysicsSimulationConstants.*; +import static frc.robot.constants.DriveTrainConstants.*; /** * simulates the behavior of our robot @@ -31,27 +30,28 @@ * and feed the result of the physics simulation back to ModuleIOSim, to simulate the odometry encoders' readings * */ public class SwerveDriveSimulation extends HolonomicChassisSimulation { - private static final String LOG_PATH = Constants.LogConfigs.PHYSICS_SIMULATION_PATH + "SwerveDriveSim/"; + private static final String LOG_PATH = LogPaths.PHYSICS_SIMULATION_PATH + "SwerveDriveSim/"; private final GyroIOSim gyroIOSim; private final ModuleIOSim[] modules; private final SwerveDriveKinematics kinematics; private final Consumer resetOdometryCallBack; public SwerveDriveSimulation( - MapleConfigFile.ConfigBlock chassisGeneralInfoBlock, GyroIOSim gyroIOSim, ModuleIOSim frontLeft, ModuleIOSim frontRight, ModuleIOSim backLeft, ModuleIOSim backRight, SwerveDriveKinematics kinematics, Pose2d startingPose, Consumer resetOdometryCallBack ) { - super(new RobotProfile(chassisGeneralInfoBlock), startingPose); + super(new RobotSimulationProfile( + + ), startingPose); this.gyroIOSim = gyroIOSim; this.modules = new ModuleIOSim[] {frontLeft, frontRight, backLeft, backRight}; this.kinematics = kinematics; this.resetOdometryCallBack = resetOdometryCallBack; resetOdometryToActualRobotPose(); - System.out.println("swerve drive sim profile: " + new RobotProfile(chassisGeneralInfoBlock)); + System.out.println("swerve drive sim profile: " + new RobotSimulationProfile()); } public void resetOdometryToActualRobotPose() { @@ -64,7 +64,7 @@ public void updateSimulationSubPeriod(int iterationNum, double subPeriodSeconds) module.updateSim(subPeriodSeconds); final ChassisSpeeds swerveWheelFreeSpeeds = kinematics.toChassisSpeeds( Arrays.stream(modules) - .map(moduleIOSim -> moduleIOSim.getFreeSwerveSpeed(profile.robotMaxVelocity)) + .map(ModuleIOSim::getFreeSwerveSpeed) .toArray(SwerveModuleState[]::new) ); super.simulateChassisBehaviorWithRobotRelativeSpeeds(swerveWheelFreeSpeeds); @@ -82,7 +82,6 @@ public void updateSimulationSubPeriod(int iterationNum, double subPeriodSeconds) updateModuleSimulationResults( modules[moduleIndex], actualModuleFloorSpeeds[moduleIndex], - profile.robotMaxVelocity, iterationNum, subPeriodSeconds ); } @@ -92,8 +91,10 @@ public void updateSimulationSubPeriod(int iterationNum, double subPeriodSeconds) protected void simulateChassisTranslationalBehavior(Vector2 desiredLinearMotionPercent) { super.simulateChassisTranslationalBehavior(desiredLinearMotionPercent); - final double dTheta = previousDesiredLinearMotionPercent.getAngleBetween(desiredLinearMotionPercent), - desiredMotionDirectionChangingRateOmega = dTheta / (Robot.defaultPeriodSecs / SIM_ITERATIONS_PER_ROBOT_PERIOD), + final double + dt = Robot.defaultPeriodSecs / SIMULATION_TICKS_IN_1_PERIOD, + dTheta = previousDesiredLinearMotionPercent.getAngleBetween(desiredLinearMotionPercent), + desiredMotionDirectionChangingRateOmega = dTheta / dt, centripetalForce = desiredMotionDirectionChangingRateOmega * getLinearVelocity().getMagnitude() * profile.robotMass; super.applyForce(Vector2.create( @@ -117,9 +118,8 @@ private static void updateGyroSimulationResults( private static void updateModuleSimulationResults( ModuleIOSim module, SwerveModuleState actualModuleFloorSpeed, - double robotMaxVelocity, int simulationIteration, double periodSeconds) { - final SwerveModuleState moduleFreeSwerveSpeed = module.getFreeSwerveSpeed(robotMaxVelocity); + final SwerveModuleState moduleFreeSwerveSpeed = module.getFreeSwerveSpeed(); final ModuleIOSim.SwerveModulePhysicsSimulationResults results = module.physicsSimulationResults; final double projectedModuleFloorSpeedMetersPerSecond = SwerveStateProjection.project( actualModuleFloorSpeed, @@ -144,17 +144,17 @@ private static double getActualDriveMotorRotterSpeedRevPerSec(double moduleSpeed moduleSpeedProjectedOnSwerveHeadingMPS * FLOOR_SPEED_WEIGHT_IN_ACTUAL_MOTOR_SPEED + moduleFreeSpeedMPS * (1 - FLOOR_SPEED_WEIGHT_IN_ACTUAL_MOTOR_SPEED); - final double rotterSpeedRadPerSec = rotterSpeedMPS / Constants.SwerveModuleConfigs.WHEEL_RADIUS; + final double rotterSpeedRadPerSec = rotterSpeedMPS / WHEEL_RADIUS_METERS; return Units.radiansToRotations(rotterSpeedRadPerSec); } public static final class OdometryThreadSim implements OdometryThread { @Override public void updateInputs(OdometryThreadInputs inputs) { - inputs.measurementTimeStamps = new double[SIM_ITERATIONS_PER_ROBOT_PERIOD]; + inputs.measurementTimeStamps = new double[SIMULATION_TICKS_IN_1_PERIOD]; final double robotStartingTimeStamps = MapleTimeUtils.getLogTimeSeconds(), - iterationPeriodSeconds = Robot.defaultPeriodSecs/SIM_ITERATIONS_PER_ROBOT_PERIOD; - for (int i =0; i < SIM_ITERATIONS_PER_ROBOT_PERIOD; i++) + iterationPeriodSeconds = Robot.defaultPeriodSecs/SIMULATION_TICKS_IN_1_PERIOD; + for (int i =0; i < SIMULATION_TICKS_IN_1_PERIOD; i++) inputs.measurementTimeStamps[i] = robotStartingTimeStamps + i * iterationPeriodSeconds; } } diff --git a/src/main/java/frc/robot/utils/MapleJoystickDriveInput.java b/src/main/java/frc/robot/utils/MapleJoystickDriveInput.java index a39c3c5..7182c1f 100644 --- a/src/main/java/frc/robot/utils/MapleJoystickDriveInput.java +++ b/src/main/java/frc/robot/utils/MapleJoystickDriveInput.java @@ -5,11 +5,15 @@ import edu.wpi.first.math.kinematics.ChassisSpeeds; import edu.wpi.first.wpilibj.XboxController; import edu.wpi.first.wpilibj2.command.button.CommandXboxController; -import frc.robot.Constants; import frc.robot.utils.CustomMaths.MapleCommonMath; import java.util.function.DoubleSupplier; +import static frc.robot.constants.JoystickConfigs.*; + +/** + * Some optimizations to the pilot's input, including + * */ public class MapleJoystickDriveInput { public final DoubleSupplier joystickXSupplier, joystickYSupplier, joystickOmegaSupplier; @@ -46,7 +50,7 @@ public Translation2d getTranslationalSpeedsFromJoystick(double chassisMaxVelocit linearSpeedYComponentDeadBanded = applySmartDeadBand(linearSpeedYComponentRaw, linearSpeedXComponentRaw); final Translation2d originalTranslationalSpeed = new Translation2d(linearSpeedXComponentDeadBanded, linearSpeedYComponentDeadBanded); - final double translationalSpeedMagnitudeScaled = Math.pow(originalTranslationalSpeed.getNorm(), Constants.DriverJoystickConfigs.linearSpeedInputExponent); + final double translationalSpeedMagnitudeScaled = Math.pow(originalTranslationalSpeed.getNorm(), LINEAR_SPEED_INPUT_EXPONENT); return new Translation2d( translationalSpeedMagnitudeScaled * chassisMaxVelocityMetersPerSec, originalTranslationalSpeed.getAngle() @@ -58,7 +62,7 @@ public double getRotationalSpeedFromJoystick(double maxAngularVelocityRadPerSec) rotationSpeedRaw = -joystickOmegaSupplier.getAsDouble(), rotationalSpeedDeadBanded = applySmartDeadBand(rotationSpeedRaw, 0), rotationalSpeedScaledMagnitude = Math.abs(Math.pow( - rotationalSpeedDeadBanded, Constants.DriverJoystickConfigs.rotationSpeedInputExponent + rotationalSpeedDeadBanded, ROTATION_SPEED_INPUT_EXPONENT )) * maxAngularVelocityRadPerSec; return Math.copySign(rotationalSpeedScaledMagnitude, rotationSpeedRaw); } @@ -72,8 +76,8 @@ public double getRotationalSpeedFromJoystick(double maxAngularVelocityRadPerSec) * */ private static double applySmartDeadBand(double axisValue, double otherAxisValue) { final double deadBand = MapleCommonMath.linearInterpretationWithBounding( - 0, Constants.DriverJoystickConfigs.deadBandWhenOtherAxisEmpty, - 1, Constants.DriverJoystickConfigs.deadBandWhenOtherAxisFull, + 0, DEAD_BAND_WHEN_OTHER_AXIS_EMPTY, + 1, DEAD_BAND_WHEN_OTHER_AXIS_FULL, Math.abs(otherAxisValue) ); return MathUtil.applyDeadband(axisValue, deadBand, 1);