diff --git a/simgui-ds.json b/simgui-ds.json index 73cc713..b9df190 100644 --- a/simgui-ds.json +++ b/simgui-ds.json @@ -18,12 +18,14 @@ } ], "axisCount": 3, - "buttonCount": 4, + "buttonCount": 6, "buttonKeys": [ 90, 88, 67, - 86 + 86, + 66, + 78 ], "povConfig": [ { @@ -88,5 +90,10 @@ "buttonCount": 0, "povCount": 0 } + ], + "robotJoysticks": [ + { + "guid": "Keyboard0" + } ] } diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/GlobalConstants.java similarity index 97% rename from src/main/java/frc/robot/Constants.java rename to src/main/java/frc/robot/GlobalConstants.java index b8e0ebb..e20cb3f 100644 --- a/src/main/java/frc/robot/Constants.java +++ b/src/main/java/frc/robot/GlobalConstants.java @@ -19,14 +19,16 @@ import frc.robot.pioneersLib.misc.VisionUtil.CameraResolution; import frc.robot.subsystems.Drive.Drive.SysIdMode; -public final class Constants { +public final class GlobalConstants { public enum RobotMode { REAL, TESTING, - SIM + SIM, + REPLAY; } public static final RobotMode ROBOT_MODE = "Crash".equals(System.getenv("CI_NAME")) ? RobotMode.SIM : RobotMode.SIM; + public static final double SIM_DELTA_TIME = 0.02; public static class Controllers { public static final XboxController DRIVER_CONTROLLER = new XboxController(0); diff --git a/src/main/java/frc/robot/Robot.java b/src/main/java/frc/robot/Robot.java index 066905b..915b772 100644 --- a/src/main/java/frc/robot/Robot.java +++ b/src/main/java/frc/robot/Robot.java @@ -23,7 +23,7 @@ public class Robot extends LoggedRobot { @Override public void robotInit() { - switch (Constants.ROBOT_MODE) { + switch (GlobalConstants.ROBOT_MODE) { case REAL: Logger.addDataReceiver(new NT4Publisher()); Logger.addDataReceiver(new WPILOGWriter()); @@ -36,6 +36,9 @@ public void robotInit() { case TESTING: Logger.addDataReceiver(new NT4Publisher()); break; + case REPLAY: + Logger.addDataReceiver(new NT4Publisher()); + break; } Logger.start(); diff --git a/src/main/java/frc/robot/subsystems/Drive/Drive.java b/src/main/java/frc/robot/subsystems/Drive/Drive.java index 2457120..2ad9974 100644 --- a/src/main/java/frc/robot/subsystems/Drive/Drive.java +++ b/src/main/java/frc/robot/subsystems/Drive/Drive.java @@ -21,16 +21,16 @@ import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.PrintCommand; import edu.wpi.first.wpilibj2.command.sysid.SysIdRoutine; -import frc.robot.Constants; +import frc.robot.GlobalConstants; import frc.robot.pioneersLib.misc.LocalADStarAK; import frc.robot.pioneersLib.subsystem.Subsystem; import org.littletonrobotics.junction.Logger; import static edu.wpi.first.units.Units.*; -import static frc.robot.Constants.Controllers.*; -import static frc.robot.Constants.*; -import static frc.robot.Constants.Drive.*; +import static frc.robot.GlobalConstants.Controllers.*; +import static frc.robot.GlobalConstants.*; +import static frc.robot.GlobalConstants.Drive.*; import static frc.robot.subsystems.Drive.TunerConstants.kSpeedAt12Volts; public class Drive extends Subsystem { @@ -56,6 +56,7 @@ private Drive() { case REAL -> new DriveIOReal(); case SIM -> new DriveIOSim(); case TESTING -> new DriveIOReal(); + case REPLAY -> new DriveIOSim(); }; // Setup Path Planner @@ -106,8 +107,8 @@ public void runState() { DriverStation.getAlliance().ifPresent(allianceColor -> { driveIO.getDrive().setOperatorPerspectiveForward( allianceColor == Alliance.Red - ? Constants.Drive.RED_ALLIANCE_PERSPECTIVE_ROTATION - : Constants.Drive.BLUE_ALLIANCE_PERSPECTIVE_ROTATION); + ? GlobalConstants.Drive.RED_ALLIANCE_PERSPECTIVE_ROTATION + : GlobalConstants.Drive.BLUE_ALLIANCE_PERSPECTIVE_ROTATION); robotMirrored = true; }); } @@ -262,7 +263,7 @@ public enum SysIdMode { @Override public Command sysIdQuasistatic(SysIdRoutine.Direction direction) { - switch (Constants.Drive.SYS_ID_MODE) { + switch (GlobalConstants.Drive.SYS_ID_MODE) { case ROTATION: return sysIdRoutineRotation.quasistatic(direction); case TRANSLATION: @@ -276,7 +277,7 @@ public Command sysIdQuasistatic(SysIdRoutine.Direction direction) { @Override public Command sysIdDynamic(SysIdRoutine.Direction direction) { - switch (Constants.Drive.SYS_ID_MODE) { + switch (GlobalConstants.Drive.SYS_ID_MODE) { case ROTATION: return sysIdRoutineRotation.dynamic(direction); case TRANSLATION: diff --git a/src/main/java/frc/robot/subsystems/Drive/DriveIOSim.java b/src/main/java/frc/robot/subsystems/Drive/DriveIOSim.java index f5ef188..312ae1e 100644 --- a/src/main/java/frc/robot/subsystems/Drive/DriveIOSim.java +++ b/src/main/java/frc/robot/subsystems/Drive/DriveIOSim.java @@ -4,7 +4,7 @@ import edu.wpi.first.wpilibj.Notifier; import edu.wpi.first.wpilibj.RobotController; -import frc.robot.Constants; +import frc.robot.GlobalConstants; /** * This class represents the simulated input/output for the drive subsystem. @@ -35,6 +35,6 @@ private void startSimThread() { /* use the measured time delta, get battery voltage from WPILib */ getDrive().updateSimState(deltaTime, RobotController.getBatteryVoltage()); }); - simNotifier.startPeriodic(Constants.Drive.SIM_UPDATE_TIME); + simNotifier.startPeriodic(GlobalConstants.Drive.SIM_UPDATE_TIME); } } diff --git a/src/main/java/frc/robot/subsystems/Drive/DriveStates.java b/src/main/java/frc/robot/subsystems/Drive/DriveStates.java index 02fd418..e2a68a7 100644 --- a/src/main/java/frc/robot/subsystems/Drive/DriveStates.java +++ b/src/main/java/frc/robot/subsystems/Drive/DriveStates.java @@ -3,8 +3,8 @@ import frc.robot.pioneersLib.subsystem.SubsystemStates; import static edu.wpi.first.units.Units.*; -import static frc.robot.Constants.Controllers.*; -import static frc.robot.Constants.Drive.*; +import static frc.robot.GlobalConstants.Controllers.*; +import static frc.robot.GlobalConstants.Drive.*; import static frc.robot.subsystems.Drive.TunerConstants.*; /** diff --git a/src/main/java/frc/robot/subsystems/Intake/Intake.java b/src/main/java/frc/robot/subsystems/Intake/Intake.java new file mode 100644 index 0000000..e2df33d --- /dev/null +++ b/src/main/java/frc/robot/subsystems/Intake/Intake.java @@ -0,0 +1,42 @@ +package frc.robot.subsystems.Intake; + +import static frc.robot.GlobalConstants.ROBOT_MODE; + +import org.littletonrobotics.junction.Logger; + +import frc.robot.pioneersLib.subsystem.Subsystem; + +public class Intake extends Subsystem{ + IntakeIO io; + IntakeIOInputsAutoLogged inputs; + private static Intake instance; + + private Intake(IntakeIO io) { + super("Intake", IntakeStates.IDLE); + this.io = io; + inputs = new IntakeIOInputsAutoLogged(); + } + + @Override + protected void runState() { + Logger.recordOutput("Intake State", getState()); + io.setPivotSetpoint(getState().getPivotSetpoint()); + io.setWheelSpeed(getState().getWheelSpeedSetpoint()); + + io.updateInputs(inputs); + Logger.processInputs("Intake", inputs); + } + + public static Intake getInstance() { + if (instance == null) { + IntakeIO intakeIO = switch(ROBOT_MODE) { + case SIM -> new IntakeIOSim(); + case REAL -> new IntakeIOReal(); + case TESTING -> new IntakeIOReal(); + case REPLAY -> new IntakeIOSim(); + }; + instance = new Intake(intakeIO); + } + return instance; + } +} diff --git a/src/main/java/frc/robot/subsystems/Intake/IntakeConstants.java b/src/main/java/frc/robot/subsystems/Intake/IntakeConstants.java new file mode 100644 index 0000000..02fe99f --- /dev/null +++ b/src/main/java/frc/robot/subsystems/Intake/IntakeConstants.java @@ -0,0 +1,49 @@ +package frc.robot.subsystems.Intake; + +import static edu.wpi.first.units.Units.Degrees; +import static edu.wpi.first.units.Units.KilogramSquareMeters; +import static edu.wpi.first.units.Units.Meters; +import static edu.wpi.first.units.Units.RotationsPerSecond; + +import com.pathplanner.lib.config.PIDConstants; + +import edu.wpi.first.units.measure.Angle; +import edu.wpi.first.units.measure.AngularVelocity; +import edu.wpi.first.units.measure.Distance; +import edu.wpi.first.units.measure.MomentOfInertia; + +public final class IntakeConstants { + public final static AngularVelocity INTAKING_SPEED = RotationsPerSecond.of(60); + public final static AngularVelocity OUTTAKING_SPEED = RotationsPerSecond.of(-60); + public final static AngularVelocity PASSING_SPEED = RotationsPerSecond.of(30); //might be negative idk + public final static AngularVelocity IDLE_SPEED = RotationsPerSecond.of(0); + + public final static Angle INTAKING_PIVOT = Degrees.of(70); + public final static Angle OUTTAKING_PIVOT = Degrees.of(70); + public final static Angle PASSING_PIVOT = Degrees.of(0); + public final static Angle IDLE_PIVOT = Degrees.of(0); + + + public final static class Sim { + public final static int NUM_PIVOT_MOTORS = 1; + public final static double PIVOT_GEARING = 5; + public final static MomentOfInertia PIVOT_MOI = KilogramSquareMeters.of(0.31654227); + public final static Distance PIVOT_ARM_LENGTH = Meters.of(.26199558); + public final static Angle MIN_PIVOT_ANGLE = Degrees.of(0); + public final static Angle MAX_PIVOT_ANGLE = Degrees.of(70); + public final static Angle STARTING_PIVOT_ANGLE = Degrees.of(0); + public final static PIDConstants PIVOT_PID_CONSTANTS = new PIDConstants(3, 0, 0); + + public final static int NUM_WHEEL_MOTORS = 1; + public final static MomentOfInertia WHEEL_MOTOR_MOI = KilogramSquareMeters.of(1); // random value + public final static double WHEEL_MOTOR_GEARING = 3; + public final static PIDConstants WHEEL_PID_CONSTANTS = new PIDConstants(0.0012, 0, 0); + } + + public final static class Real { + public final static int WHEEL_MOTOR_CANID = 10; + public final static int PIVOT_MOTOR_CANID = 11; + public final static PIDConstants PIVOT_PID_CONSTANTS = new PIDConstants(3, 0, 0); + public final static PIDConstants WHEEL_PID_CONSTANTS = new PIDConstants(0.0012, 0, 0); + } + } diff --git a/src/main/java/frc/robot/subsystems/Intake/IntakeIO.java b/src/main/java/frc/robot/subsystems/Intake/IntakeIO.java new file mode 100644 index 0000000..532f2df --- /dev/null +++ b/src/main/java/frc/robot/subsystems/Intake/IntakeIO.java @@ -0,0 +1,25 @@ +package frc.robot.subsystems.Intake; + +import org.littletonrobotics.junction.AutoLog; + +public interface IntakeIO { + + @AutoLog + public static class IntakeIOInputs { + // Pivot + public double pivotPosition; + public double pivotSetpoint; + + // Spinners + public double wheelSpeed; + public double wheelSpeedSetpoint; + } + + public void updateInputs(IntakeIOInputs input); + + public void setPivotSetpoint(double pivotSetpoint); + + public void setWheelSpeed(double wheelSpeed); + + +} diff --git a/src/main/java/frc/robot/subsystems/Intake/IntakeIOReal.java b/src/main/java/frc/robot/subsystems/Intake/IntakeIOReal.java new file mode 100644 index 0000000..00a7e8d --- /dev/null +++ b/src/main/java/frc/robot/subsystems/Intake/IntakeIOReal.java @@ -0,0 +1,46 @@ +package frc.robot.subsystems.Intake; + +import com.ctre.phoenix6.hardware.TalonFX; + +import edu.wpi.first.math.controller.PIDController; +import edu.wpi.first.math.util.Units; + +public class IntakeIOReal implements IntakeIO { + private TalonFX wheelMotor; + private TalonFX pivotMotor; + private PIDController pivotController; + private PIDController wheelSpeedController; + private double pivotPositionSetpoint; + private double wheelSpeedSetpoint; + + public IntakeIOReal() { + wheelMotor = new TalonFX(IntakeConstants.Real.WHEEL_MOTOR_CANID); + pivotMotor = new TalonFX(IntakeConstants.Real.PIVOT_MOTOR_CANID); + pivotController = new PIDController(IntakeConstants.Real.PIVOT_PID_CONSTANTS.kP, + IntakeConstants.Real.PIVOT_PID_CONSTANTS.kI, IntakeConstants.Real.PIVOT_PID_CONSTANTS.kD); + wheelSpeedController = new PIDController(IntakeConstants.Real.WHEEL_PID_CONSTANTS.kP, + IntakeConstants.Real.WHEEL_PID_CONSTANTS.kI, IntakeConstants.Real.WHEEL_PID_CONSTANTS.kD); + } + + @Override + public void updateInputs(IntakeIOInputs inputs) { + inputs.pivotPosition = Units.rotationsToDegrees(pivotMotor.getPosition().getValueAsDouble()); + inputs.pivotSetpoint = pivotPositionSetpoint; + inputs.wheelSpeed = wheelMotor.getVelocity().getValueAsDouble(); + inputs.wheelSpeedSetpoint = wheelSpeedSetpoint; + } + + @Override + public void setPivotSetpoint(double pivotSetpoint) { + this.pivotPositionSetpoint = pivotSetpoint; + double voltage = pivotController.calculate(Units.rotationsToDegrees(pivotMotor.getPosition().getValueAsDouble()), pivotSetpoint); + pivotMotor.setVoltage(voltage); + } + + @Override + public void setWheelSpeed(double wheelSpeed) { + this.wheelSpeedSetpoint = wheelSpeed; + double voltage = wheelSpeedController.calculate(Units.rotationsToDegrees(wheelMotor.getVelocity().getValueAsDouble()), wheelSpeed); + wheelMotor.setVoltage(voltage); + } +} diff --git a/src/main/java/frc/robot/subsystems/Intake/IntakeIOSim.java b/src/main/java/frc/robot/subsystems/Intake/IntakeIOSim.java new file mode 100644 index 0000000..bacd109 --- /dev/null +++ b/src/main/java/frc/robot/subsystems/Intake/IntakeIOSim.java @@ -0,0 +1,66 @@ +package frc.robot.subsystems.Intake; + + +import static frc.robot.GlobalConstants.SIM_DELTA_TIME; + +import edu.wpi.first.math.controller.PIDController; +import edu.wpi.first.math.system.plant.DCMotor; +import edu.wpi.first.math.util.Units; +import edu.wpi.first.wpilibj.simulation.DCMotorSim; +import edu.wpi.first.wpilibj.simulation.SingleJointedArmSim; +import edu.wpi.first.math.system.plant.LinearSystemId; + +public class IntakeIOSim implements IntakeIO { + private SingleJointedArmSim pivotSim; + private DCMotorSim wheelMotorSim; + private PIDController pivotController; + private PIDController wheelSpeedController; + + private double wheelSpeedSetpoint; + private double pivotPositionSetpoint; + + IntakeIOSim() { + pivotSim = new SingleJointedArmSim(DCMotor.getKrakenX60(IntakeConstants.Sim.NUM_PIVOT_MOTORS), + IntakeConstants.Sim.PIVOT_GEARING, IntakeConstants.Sim.PIVOT_MOI.magnitude(), IntakeConstants.Sim.PIVOT_ARM_LENGTH.magnitude(), + Units.degreesToRadians(IntakeConstants.Sim.MIN_PIVOT_ANGLE.magnitude()), + Units.degreesToRadians(IntakeConstants.Sim.MAX_PIVOT_ANGLE.magnitude()), + false, Units.degreesToRadians(IntakeConstants.Sim.STARTING_PIVOT_ANGLE.magnitude())); + + wheelMotorSim = new DCMotorSim(LinearSystemId.createDCMotorSystem(DCMotor.getKrakenX60(IntakeConstants.Sim.NUM_WHEEL_MOTORS), + IntakeConstants.Sim.WHEEL_MOTOR_MOI.magnitude(), IntakeConstants.Sim.WHEEL_MOTOR_GEARING), + DCMotor.getKrakenX60(IntakeConstants.Sim.NUM_WHEEL_MOTORS)); + + pivotController = new PIDController(IntakeConstants.Sim.PIVOT_PID_CONSTANTS.kP, + IntakeConstants.Sim.PIVOT_PID_CONSTANTS.kI, IntakeConstants.Sim.PIVOT_PID_CONSTANTS.kD); + wheelSpeedController = new PIDController(IntakeConstants.Sim.WHEEL_PID_CONSTANTS.kP, + IntakeConstants.Sim.WHEEL_PID_CONSTANTS.kI, IntakeConstants.Sim.WHEEL_PID_CONSTANTS.kD); + + wheelSpeedSetpoint = 0; + pivotPositionSetpoint = 0; + } + + @Override + public void updateInputs(IntakeIOInputs input) { + pivotSim.update(SIM_DELTA_TIME); + wheelMotorSim.update(SIM_DELTA_TIME); + + input.wheelSpeed = Units.radiansToDegrees(wheelMotorSim.getAngularVelocityRadPerSec()); + input.wheelSpeedSetpoint = wheelSpeedSetpoint; + input.pivotPosition = Units.radiansToDegrees(pivotSim.getAngleRads()); + input.pivotSetpoint = pivotPositionSetpoint; + } + + @Override + public void setPivotSetpoint(double pivotSetpoint) { + this.pivotPositionSetpoint = pivotSetpoint; + pivotSim.setInputVoltage(pivotController.calculate(Units.radiansToDegrees(pivotSim.getAngleRads()), pivotSetpoint)); + } + + @Override + public void setWheelSpeed(double wheelSpeedSetpoint) { + this.wheelSpeedSetpoint = wheelSpeedSetpoint; + wheelMotorSim.setInputVoltage(wheelSpeedController.calculate(wheelMotorSim.getAngularVelocityRadPerSec(), wheelSpeedSetpoint)); + } + + +} diff --git a/src/main/java/frc/robot/subsystems/Intake/IntakeStates.java b/src/main/java/frc/robot/subsystems/Intake/IntakeStates.java new file mode 100644 index 0000000..9ea6422 --- /dev/null +++ b/src/main/java/frc/robot/subsystems/Intake/IntakeStates.java @@ -0,0 +1,33 @@ +package frc.robot.subsystems.Intake; + +import frc.robot.pioneersLib.subsystem.SubsystemStates; + +public enum IntakeStates implements SubsystemStates { + INTAKING(IntakeConstants.INTAKING_SPEED.magnitude(), IntakeConstants.INTAKING_PIVOT.magnitude(), "INTAKING"), + OUTTAKING(IntakeConstants.OUTTAKING_SPEED.magnitude(), IntakeConstants.OUTTAKING_PIVOT.magnitude(), "OUTTAKING"), + PASSING(IntakeConstants.PASSING_SPEED.magnitude(), IntakeConstants.PASSING_PIVOT.magnitude(), "PASSING"), + IDLE(IntakeConstants.IDLE_SPEED.magnitude(), IntakeConstants.IDLE_PIVOT.magnitude(), "IDLE"); + + IntakeStates(Double wheelSpeed, double pivotSetpoint, String stateString) { + this.wheelSpeed = wheelSpeed; + this.pivotSetpoint = pivotSetpoint; + this.stateString = stateString; + } + + private double wheelSpeed; + private double pivotSetpoint; + private String stateString; + + public double getWheelSpeedSetpoint() { + return wheelSpeed; + } + + public double getPivotSetpoint() { + return pivotSetpoint; + } + + @Override + public String getStateString() { + return stateString; + } +} diff --git a/src/main/java/frc/robot/subsystems/Manager/Manager.java b/src/main/java/frc/robot/subsystems/Manager/Manager.java index fd85e0c..82a7871 100644 --- a/src/main/java/frc/robot/subsystems/Manager/Manager.java +++ b/src/main/java/frc/robot/subsystems/Manager/Manager.java @@ -1,7 +1,7 @@ package frc.robot.subsystems.Manager; -import static frc.robot.Constants.*; -import static frc.robot.Constants.Controllers.TEST_CONTROLLER; +import static frc.robot.GlobalConstants.*; +import static frc.robot.GlobalConstants.Controllers.TEST_CONTROLLER; import org.littletonrobotics.junction.Logger; @@ -9,6 +9,7 @@ import edu.wpi.first.wpilibj2.command.sysid.SysIdRoutine.Direction; import frc.robot.pioneersLib.subsystem.Subsystem; import frc.robot.subsystems.Drive.Drive; +import frc.robot.subsystems.Intake.Intake; import frc.robot.subsystems.Vision.Vision; public class Manager extends Subsystem { @@ -18,6 +19,7 @@ public class Manager extends Subsystem { private final Drive drive = Drive.getInstance(); private final Vision vision = Vision.getInstance(); private final CommandScheduler commandScheduler = CommandScheduler.getInstance(); + private final Intake intake = Intake.getInstance(); // Change to change the subsystem that gets tested (has runnable sysID tests) saftey ish private final Subsystem sysIdSubsystem = drive; @@ -34,6 +36,9 @@ private Manager() { addRunnableTrigger(() -> {commandScheduler.schedule(sysIdSubsystem.sysIdQuasistatic(Direction.kForward));}, () -> TEST_CONTROLLER.getXButtonPressed()); addRunnableTrigger(() -> {commandScheduler.schedule(sysIdSubsystem.sysIdQuasistatic(Direction.kReverse));}, () -> TEST_CONTROLLER.getYButtonPressed()); } + + addTrigger(ManagerStates.IDLE, ManagerStates.INTAKING, () -> Controllers.DRIVER_CONTROLLER.getAButtonPressed()); + addTrigger(ManagerStates.INTAKING, ManagerStates.IDLE, () -> Controllers.DRIVER_CONTROLLER.getAButtonPressed()); } public static Manager getInstance() { @@ -45,12 +50,14 @@ public static Manager getInstance() { @Override public void runState() { - Logger.recordOutput("Manager/State", getState().getStateString()); Logger.recordOutput("Manager/State Time", getStateTime()); + intake.setState(getState().getIntakeState()); + drive.periodic(); vision.periodic(); + intake.periodic(); // Other subsystem periodics go here } diff --git a/src/main/java/frc/robot/subsystems/Manager/ManagerStates.java b/src/main/java/frc/robot/subsystems/Manager/ManagerStates.java index 98c6791..a1dbef1 100644 --- a/src/main/java/frc/robot/subsystems/Manager/ManagerStates.java +++ b/src/main/java/frc/robot/subsystems/Manager/ManagerStates.java @@ -1,18 +1,26 @@ package frc.robot.subsystems.Manager; import frc.robot.pioneersLib.subsystem.SubsystemStates; +import frc.robot.subsystems.Intake.IntakeStates; public enum ManagerStates implements SubsystemStates { - IDLE("Idle"); + IDLE("Idle", IntakeStates.IDLE), + INTAKING("INTAKING", IntakeStates.INTAKING); - ManagerStates(String stateString) { + ManagerStates(String stateString, IntakeStates intakeState) { this.stateString = stateString; + this.intakeState = intakeState; } private String stateString; + private IntakeStates intakeState; @Override public String getStateString() { return stateString; } + + public IntakeStates getIntakeState() { + return intakeState; + } } diff --git a/src/main/java/frc/robot/subsystems/Vision/Vision.java b/src/main/java/frc/robot/subsystems/Vision/Vision.java index fda1a81..a00e36c 100644 --- a/src/main/java/frc/robot/subsystems/Vision/Vision.java +++ b/src/main/java/frc/robot/subsystems/Vision/Vision.java @@ -8,8 +8,8 @@ import frc.robot.pioneersLib.subsystem.Subsystem; import frc.robot.subsystems.Drive.Drive; -import static frc.robot.Constants.Vision.*; -import static frc.robot.Constants.*; +import static frc.robot.GlobalConstants.Vision.*; +import static frc.robot.GlobalConstants.*; public class Vision extends Subsystem { @@ -32,6 +32,7 @@ public static Vision getInstance() { case REAL -> new VisionIOReal(); case SIM -> new VisionIOSim(); case TESTING -> new VisionIO() {}; + case REPLAY -> new VisionIOSim(); }; instance = new Vision(visionIO, Drive.getInstance()); } diff --git a/src/main/java/frc/robot/subsystems/Vision/VisionIOReal.java b/src/main/java/frc/robot/subsystems/Vision/VisionIOReal.java index 9890931..7837204 100644 --- a/src/main/java/frc/robot/subsystems/Vision/VisionIOReal.java +++ b/src/main/java/frc/robot/subsystems/Vision/VisionIOReal.java @@ -10,7 +10,7 @@ import edu.wpi.first.math.filter.Debouncer; import edu.wpi.first.math.filter.Debouncer.DebounceType; import edu.wpi.first.math.geometry.Pose2d; -import frc.robot.Constants; +import frc.robot.GlobalConstants; public class VisionIOReal implements VisionIO { @@ -28,10 +28,10 @@ public VisionIOReal() { frontCamera = new PhotonCamera("Front Camera"); // Pose estimators :/ - frontEstimator = new PhotonPoseEstimator(Constants.Vision.APRIL_TAG_FIELD_LAYOUT, PoseStrategy.MULTI_TAG_PNP_ON_COPROCESSOR, Constants.Vision.ROBOT_TO_FRONT_CAMERA); - sideEstimator = new PhotonPoseEstimator(Constants.Vision.APRIL_TAG_FIELD_LAYOUT, PoseStrategy.MULTI_TAG_PNP_ON_COPROCESSOR, Constants.Vision.ROBOT_TO_SIDE_CAMERA); - sideDebouncer = new Debouncer(Constants.Vision.CAMERA_DEBOUNCE_TIME, DebounceType.kFalling); - frontDebouncer = new Debouncer(Constants.Vision.CAMERA_DEBOUNCE_TIME, DebounceType.kFalling); + frontEstimator = new PhotonPoseEstimator(GlobalConstants.Vision.APRIL_TAG_FIELD_LAYOUT, PoseStrategy.MULTI_TAG_PNP_ON_COPROCESSOR, GlobalConstants.Vision.ROBOT_TO_FRONT_CAMERA); + sideEstimator = new PhotonPoseEstimator(GlobalConstants.Vision.APRIL_TAG_FIELD_LAYOUT, PoseStrategy.MULTI_TAG_PNP_ON_COPROCESSOR, GlobalConstants.Vision.ROBOT_TO_SIDE_CAMERA); + sideDebouncer = new Debouncer(GlobalConstants.Vision.CAMERA_DEBOUNCE_TIME, DebounceType.kFalling); + frontDebouncer = new Debouncer(GlobalConstants.Vision.CAMERA_DEBOUNCE_TIME, DebounceType.kFalling); } @Override diff --git a/src/main/java/frc/robot/subsystems/Vision/VisionIOSim.java b/src/main/java/frc/robot/subsystems/Vision/VisionIOSim.java index 9a49e45..c5874f6 100644 --- a/src/main/java/frc/robot/subsystems/Vision/VisionIOSim.java +++ b/src/main/java/frc/robot/subsystems/Vision/VisionIOSim.java @@ -14,7 +14,7 @@ import edu.wpi.first.math.filter.Debouncer.DebounceType; import edu.wpi.first.math.geometry.Pose2d; import edu.wpi.first.math.geometry.Rotation2d; -import frc.robot.Constants; +import frc.robot.GlobalConstants; public class VisionIOSim implements VisionIO { @@ -53,9 +53,9 @@ public VisionIOSim() { sideCamera = new PhotonCameraSim(new PhotonCamera("Side Camera"), sideCameraProperties); frontCamera = new PhotonCameraSim(new PhotonCamera("Front Camera"), frontCameraProperties); - visionSim.addAprilTags(Constants.Vision.APRIL_TAG_FIELD_LAYOUT); - visionSim.addCamera(sideCamera, Constants.Vision.ROBOT_TO_SIDE_CAMERA); - visionSim.addCamera(frontCamera, Constants.Vision.ROBOT_TO_FRONT_CAMERA); + visionSim.addAprilTags(GlobalConstants.Vision.APRIL_TAG_FIELD_LAYOUT); + visionSim.addCamera(sideCamera, GlobalConstants.Vision.ROBOT_TO_SIDE_CAMERA); + visionSim.addCamera(frontCamera, GlobalConstants.Vision.ROBOT_TO_FRONT_CAMERA); // Puts a camera stream onto nt4 frontCamera.enableRawStream(true); @@ -68,10 +68,10 @@ public VisionIOSim() { robotPose = new Pose2d(); // Pose estimators :/ - frontEstimator = new PhotonPoseEstimator(Constants.Vision.APRIL_TAG_FIELD_LAYOUT, PoseStrategy.MULTI_TAG_PNP_ON_COPROCESSOR, Constants.Vision.ROBOT_TO_FRONT_CAMERA); - sideEstimator = new PhotonPoseEstimator(Constants.Vision.APRIL_TAG_FIELD_LAYOUT, PoseStrategy.MULTI_TAG_PNP_ON_COPROCESSOR, Constants.Vision.ROBOT_TO_SIDE_CAMERA); - sideDebouncer = new Debouncer(Constants.Vision.CAMERA_DEBOUNCE_TIME, DebounceType.kFalling); - frontDebouncer = new Debouncer(Constants.Vision.CAMERA_DEBOUNCE_TIME, DebounceType.kFalling); + frontEstimator = new PhotonPoseEstimator(GlobalConstants.Vision.APRIL_TAG_FIELD_LAYOUT, PoseStrategy.MULTI_TAG_PNP_ON_COPROCESSOR, GlobalConstants.Vision.ROBOT_TO_FRONT_CAMERA); + sideEstimator = new PhotonPoseEstimator(GlobalConstants.Vision.APRIL_TAG_FIELD_LAYOUT, PoseStrategy.MULTI_TAG_PNP_ON_COPROCESSOR, GlobalConstants.Vision.ROBOT_TO_SIDE_CAMERA); + sideDebouncer = new Debouncer(GlobalConstants.Vision.CAMERA_DEBOUNCE_TIME, DebounceType.kFalling); + frontDebouncer = new Debouncer(GlobalConstants.Vision.CAMERA_DEBOUNCE_TIME, DebounceType.kFalling); } @Override