diff --git a/src/main/java/frc/robot/Robot.java b/src/main/java/frc/robot/Robot.java index f69ea44..45ccf80 100644 --- a/src/main/java/frc/robot/Robot.java +++ b/src/main/java/frc/robot/Robot.java @@ -7,6 +7,7 @@ import edu.wpi.first.wpilibj.TimedRobot; import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.CommandScheduler; +import frc.robot.constants.Constants; /** * The VM is configured to automatically run this class, and to call the functions corresponding to diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index d68629c..21054db 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -13,9 +13,9 @@ import edu.wpi.first.wpilibj2.command.RunCommand; import edu.wpi.first.wpilibj2.command.button.JoystickButton; import edu.wpi.first.wpilibj2.command.button.POVButton; -import frc.robot.Constants.DriveConstants; -import frc.robot.Constants.ElevatorConstants; -import frc.robot.Constants.IOConstants; +import frc.robot.constants.Constants.DriveConstants; +import frc.robot.constants.Constants.ElevatorConstants; +import frc.robot.constants.Constants.IOConstants; import frc.robot.subsystems.DriveSubsystem; import frc.robot.subsystems.ElevatorSubsystem; 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..2dee413 --- /dev/null +++ b/src/main/java/frc/robot/constants/Constants.java @@ -0,0 +1,287 @@ +package frc.robot.constants; + +import java.io.BufferedReader; +import java.io.FileNotFoundException; +import java.io.FileReader; +import java.io.IOException; + +import edu.wpi.first.math.Vector; +import edu.wpi.first.math.geometry.Pose3d; +import edu.wpi.first.math.kinematics.SwerveDriveKinematics; +import edu.wpi.first.math.numbers.N3; +import edu.wpi.first.wpilibj.DriverStation; +import frc.robot.Robot; + +public final record Constants() { + private static Robots robot = null; + //private static Robots robot = Robots.COMPETITION; + + private enum Robots { + COMPETITION, + TLJR + } + + private static final class RobotNameNotFound extends ExceptionInInitializerError { + public RobotNameNotFound(String s) { + super(s); + } + } + + private static void setRobot() { + // First detect robot using roborio + if (Robot.isSimulation()) { + robot = Robots.COMPETITION; + } else { + BufferedReader reader = null; + String line; + try { + try { + reader = new BufferedReader(new FileReader("/etc/machine-info")); + while ((line = reader.readLine()) != null) { + if (line.contains("driftwood")) { + robot = Robots.TLJR; + break; + } + } + if (robot == null) { + throw new RobotNameNotFound("Robot name not found in /etc/machine-info"); + } + } catch (FileNotFoundException e) { + throw new RobotNameNotFound("Robot name not found in /etc/machine-info"); + } finally { + if (reader != null) { + reader.close(); + } + } + } catch (IOException e) { + DriverStation.reportError("Error: Something went fatally wrong when detecting robot", e.getStackTrace()); + } + } + } + + static { + if (robot == null) { + setRobot(); + } + + switch (robot) { + default: + case TLJR: + kFastPeriodicPeriod = TLJRConstants.kFastPeriodicPeriod; + break; + } + } + + public static final double kFastPeriodicPeriod; + + /** + * Input/Output constants + */ + public static final record IOConstants() { + static { + if (robot == null) { + setRobot(); + } + + switch (robot) { + default: + case TLJR: + kDriverControllerPort = TLJRConstants.IOConstants.kDriverControllerPort; + kOperatorControllerPort = TLJRConstants.IOConstants.kOperatorControllerPort; + kControllerDeadband = TLJRConstants.IOConstants.kControllerDeadband; + kSlowModeScalar = TLJRConstants.IOConstants.kSlowModeScalar; + kDPadUp = TLJRConstants.IOConstants.kDPadUp; + kDPadRight = TLJRConstants.IOConstants.kDPadRight; + kDPadDown = TLJRConstants.IOConstants.kDPadDown; + kDPadLeft = TLJRConstants.IOConstants.kDPadLeft; + break; + } + } + + public static final int kDriverControllerPort; + public static final int kOperatorControllerPort; + + public static final double kControllerDeadband; + public static final double kSlowModeScalar; + + public static final int kDPadUp; + public static final int kDPadRight; + public static final int kDPadDown; + public static final int kDPadLeft; + } + + public static final record DriveConstants() { + static { + if (robot == null) { + setRobot(); + } + + switch (robot) { + default: + case TLJR: + kFrontLeftDriveMotorPort = TLJRConstants.DriveConstants.kFrontLeftDriveMotorPort; + kRearLeftDriveMotorPort = TLJRConstants.DriveConstants.kRearLeftDriveMotorPort; + kFrontRightDriveMotorPort = TLJRConstants.DriveConstants.kFrontRightDriveMotorPort; + kRearRightDriveMotorPort = TLJRConstants.DriveConstants.kRearRightDriveMotorPort; + + kFrontLeftTurningMotorPort = TLJRConstants.DriveConstants.kFrontLeftTurningMotorPort; + kRearLeftTurningMotorPort = TLJRConstants.DriveConstants.kRearLeftTurningMotorPort; + kFrontRightTurningMotorPort = TLJRConstants.DriveConstants.kFrontRightTurningMotorPort; + kRearRightTurningMotorPort = TLJRConstants.DriveConstants.kRearRightTurningMotorPort; + + kFrontLeftTurningEncoderPort = TLJRConstants.DriveConstants.kFrontLeftTurningEncoderPort; + kRearLeftTurningEncoderPort = TLJRConstants.DriveConstants.kRearLeftTurningEncoderPort; + kFrontRightTurningEncoderPort = TLJRConstants.DriveConstants.kFrontRightTurningEncoderPort; + kRearRightTurningEncoderPort = TLJRConstants.DriveConstants.kRearRightTurningEncoderPort; + + kFrontLeftDriveMotorReversed = TLJRConstants.DriveConstants.kFrontLeftDriveMotorReversed; + kRearLeftDriveMotorReversed = TLJRConstants.DriveConstants.kRearLeftDriveMotorReversed; + kFrontRightDriveMotorReversed = TLJRConstants.DriveConstants.kFrontRightDriveMotorReversed; + kRearRightDriveMotorReversed = TLJRConstants.DriveConstants.kRearRightDriveMotorReversed; + + kTrackWidth = TLJRConstants.DriveConstants.kTrackWidth; + kWheelBase = TLJRConstants.DriveConstants.kWheelBase; + kWheelDiameterMeters = TLJRConstants.DriveConstants.kWheelDiameterMeters; + kDrivingGearRatio = TLJRConstants.DriveConstants.kDrivingGearRatio; + + kPModuleTurningController = TLJRConstants.DriveConstants.kPModuleTurningController; + kDriveKinematics = TLJRConstants.DriveConstants.kDriveKinematics; + + kMaxSpeedMetersPerSecond = TLJRConstants.DriveConstants.kMaxSpeedMetersPerSecond; + kMaxAngularSpeedRadiansPerSecond = TLJRConstants.DriveConstants.kMaxAngularSpeedRadiansPerSecond; + + kHeadingCorrectionTurningStopTime = TLJRConstants.DriveConstants.kHeadingCorrectionTurningStopTime; + kPHeadingCorrectionController = TLJRConstants.DriveConstants.kPHeadingCorrectionController; + break; + } + } + + public static final int kFrontLeftDriveMotorPort; + public static final int kRearLeftDriveMotorPort; + public static final int kFrontRightDriveMotorPort; + public static final int kRearRightDriveMotorPort; + + public static final int kFrontLeftTurningMotorPort; + public static final int kRearLeftTurningMotorPort; + public static final int kFrontRightTurningMotorPort; + public static final int kRearRightTurningMotorPort; + + public static final int kFrontLeftTurningEncoderPort; + public static final int kRearLeftTurningEncoderPort; + public static final int kFrontRightTurningEncoderPort; + public static final int kRearRightTurningEncoderPort; + + public static final boolean kFrontLeftDriveMotorReversed; + public static final boolean kRearLeftDriveMotorReversed; + public static final boolean kFrontRightDriveMotorReversed; + public static final boolean kRearRightDriveMotorReversed; + + /** Distance between centers of right and left wheels on robot (in meters). */ + public static final double kTrackWidth; + + /** Distance between front and back wheels on robot (in meters). */ + public static final double kWheelBase; + + /** Diameter of each wheel in the SDS MK4i swerve module (in meters) */ + public static final double kWheelDiameterMeters; + + /** Gear ratio between the motor and the wheel. */ + public static final double kDrivingGearRatio; + + public static final double kPModuleTurningController; + + public static final SwerveDriveKinematics kDriveKinematics; + + /** For a a SDS Mk4i L1 swerve base with Neos */ + public static final double kMaxSpeedMetersPerSecond; + /** For a a SDS Mk4i L1 swerve base with Neos */ + public static final double kMaxAngularSpeedRadiansPerSecond; + + /** Heading Correction */ + public static final double kHeadingCorrectionTurningStopTime; + public static final double kPHeadingCorrectionController; + } + + public static final record VisionConstants() { + static { + if (robot == null) { + setRobot(); + } + + switch (robot) { + default: + case TLJR: + kCamPos = TLJRConstants.VisionConstants.kCamPos; + kLimelightName = TLJRConstants.VisionConstants.kLimelightName; + kIMUMode = TLJRConstants.VisionConstants.kIMUMode; + + kOdometrySTDDevs = TLJRConstants.VisionConstants.kOdometrySTDDevs; + kVisionSTDDevs = TLJRConstants.VisionConstants.kVisionSTDDevs; + + kUseVision = TLJRConstants.VisionConstants.kUseVision; + break; + } + } + + public static final Pose3d kCamPos; + + public static final String kLimelightName; + + // https://docs.limelightvision.io/docs/docs-limelight/pipeline-apriltag/apriltag-robot-localization-megatag2 + public static final int kIMUMode; + + public static final Vector kOdometrySTDDevs; + public static final Vector kVisionSTDDevs; + + public static final boolean kUseVision; + } + + public static final record ElevatorConstants() { + static { + if (robot == null) { + setRobot(); + } + + switch (robot) { + default: + case TLJR: + kElevatorMotorPort = TLJRConstants.ElevatorConstants.kElevatorMotorPort; + kElevatorCANrangePort = TLJRConstants.ElevatorConstants.kElevatorCANrangePort; + + kPElevator = TLJRConstants.ElevatorConstants.kPElevator; + + kElevatorGearing = TLJRConstants.ElevatorConstants.kElevatorGearing; + kElevatorMaxSpeed = TLJRConstants.ElevatorConstants.kElevatorMaxSpeed; + kElevatorFeedForward = TLJRConstants.ElevatorConstants.kElevatorFeedForward; + kElevatorSpeedScalar = TLJRConstants.ElevatorConstants.kElevatorSpeedScalar; + kElevatorBottom = TLJRConstants.ElevatorConstants.kElevatorBottom; + kElevatorTop = TLJRConstants.ElevatorConstants.kElevatorTop; + kElevatorDistanceThreshold = TLJRConstants.ElevatorConstants.kElevatorDistanceThreshold; + + kL1Height = TLJRConstants.ElevatorConstants.kL1Height; + kL2Height = TLJRConstants.ElevatorConstants.kL2Height; + kL3Height = TLJRConstants.ElevatorConstants.kL3Height; + kL4Height = TLJRConstants.ElevatorConstants.kL4Height; + break; + } + } + + public static final int kElevatorMotorPort; + public static final int kElevatorCANrangePort; + + public static final double kPElevator; + + public static final double kElevatorGearing; + public static final double kElevatorMaxSpeed; + public static final double kElevatorFeedForward; + public static final double kElevatorSpeedScalar; + public static final double kElevatorBottom; + public static final double kElevatorTop; + public static final double kElevatorDistanceThreshold; + + public static final double kL1Height; + public static final double kL2Height; + public static final double kL3Height; + public static final double kL4Height; + } +} diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/constants/TLJRConstants.java similarity index 95% rename from src/main/java/frc/robot/Constants.java rename to src/main/java/frc/robot/constants/TLJRConstants.java index f7041fc..354d748 100644 --- a/src/main/java/frc/robot/Constants.java +++ b/src/main/java/frc/robot/constants/TLJRConstants.java @@ -2,7 +2,7 @@ // Open Source Software; you can modify and/or share it under the terms of // the WPILib BSD license file in the root directory of this project. -package frc.robot; +package frc.robot.constants; import edu.wpi.first.math.VecBuilder; import edu.wpi.first.math.Vector; @@ -25,14 +25,13 @@ * wherever the * constants are needed, to reduce verbosity. */ -public final class Constants { - +public final record TLJRConstants() { public static final double kFastPeriodicPeriod = 0.01; // 100Hz, 10ms /** * Input/Output constants */ - public static final class IOConstants { + public static final record IOConstants() { public static final int kDriverControllerPort = 0; public static final int kOperatorControllerPort = 1; @@ -45,7 +44,7 @@ public static final class IOConstants { public static final int kDPadLeft = 270; } - public static final class DriveConstants { + public static final record DriveConstants() { // TODO: set motor and encoder constants public static final int kFrontLeftDriveMotorPort = 4; public static final int kRearLeftDriveMotorPort = 5; @@ -100,7 +99,7 @@ public static final class DriveConstants { public static final double kPHeadingCorrectionController = 5; } - public static final class VisionConstants { + public static final record VisionConstants() { // TODO: Update cam pose relative to center of bot public static final Pose3d kCamPos = new Pose3d( new Translation3d(0.3048,0.254,0), @@ -119,7 +118,7 @@ public static final class VisionConstants { public static final boolean kUseVision = true; } - public static final class ElevatorConstants { + public static final record ElevatorConstants() { // TODO: Set motor and distance sensor ports public static final int kElevatorMotorPort = 0; public static final int kElevatorCANrangePort = 0; @@ -141,5 +140,4 @@ public static final class ElevatorConstants { public static final double kL3Height = 0.7; public static final double kL4Height = 0.9; } - } diff --git a/src/main/java/frc/robot/subsystems/DriveSubsystem.java b/src/main/java/frc/robot/subsystems/DriveSubsystem.java index 3fccd60..f22b1d7 100644 --- a/src/main/java/frc/robot/subsystems/DriveSubsystem.java +++ b/src/main/java/frc/robot/subsystems/DriveSubsystem.java @@ -21,11 +21,11 @@ import edu.wpi.first.wpilibj.smartdashboard.Field2d; import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import edu.wpi.first.wpilibj2.command.SubsystemBase; -import frc.robot.Constants.DriveConstants; -import frc.robot.Constants.VisionConstants; -import frc.robot.Constants; import frc.robot.utils.LimelightHelpers; import frc.robot.Robot; +import frc.robot.constants.Constants; +import frc.robot.constants.Constants.DriveConstants; +import frc.robot.constants.Constants.VisionConstants; public class DriveSubsystem extends SubsystemBase { private final SwerveModule m_frontLeft = new SwerveModule( diff --git a/src/main/java/frc/robot/subsystems/ElevatorSubsystem.java b/src/main/java/frc/robot/subsystems/ElevatorSubsystem.java index 0b15a76..76836f2 100644 --- a/src/main/java/frc/robot/subsystems/ElevatorSubsystem.java +++ b/src/main/java/frc/robot/subsystems/ElevatorSubsystem.java @@ -15,9 +15,9 @@ import edu.wpi.first.math.controller.PIDController; import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import edu.wpi.first.wpilibj2.command.SubsystemBase; -import frc.robot.Constants; import frc.robot.Robot; -import frc.robot.Constants.ElevatorConstants; +import frc.robot.constants.Constants; +import frc.robot.constants.Constants.ElevatorConstants; public class ElevatorSubsystem extends SubsystemBase { private final SparkFlex m_elevatorMotor; diff --git a/src/main/java/frc/robot/subsystems/SwerveModule.java b/src/main/java/frc/robot/subsystems/SwerveModule.java index cdbc6df..f075718 100644 --- a/src/main/java/frc/robot/subsystems/SwerveModule.java +++ b/src/main/java/frc/robot/subsystems/SwerveModule.java @@ -16,9 +16,9 @@ import edu.wpi.first.math.geometry.Rotation2d; import edu.wpi.first.math.kinematics.SwerveModulePosition; import edu.wpi.first.math.kinematics.SwerveModuleState; -import frc.robot.Constants.DriveConstants; -import frc.robot.Constants; import frc.robot.Robot; +import frc.robot.constants.Constants; +import frc.robot.constants.Constants.DriveConstants; public class SwerveModule { private final SparkMax m_driveMotor;