diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java index 60c1364..d37b250 100644 --- a/src/main/java/frc/robot/Constants.java +++ b/src/main/java/frc/robot/Constants.java @@ -82,37 +82,47 @@ public final class Constants { public static final int RIGHT_SHOOTER_MOTOR_ID = 40; public static final int INDEXER_MOTOR_ID = 43; - public static double SHOOTER_MOTOR_P = 0.0018; + public static double SHOOTER_MOTOR_P = 0.001; public static double SHOOTER_MOTOR_I = 0; - public static double SHOOTER_MOTOR_D = 0; + public static double SHOOTER_MOTOR_D = 0.001; + public static double SHOOTER_MOTOR_S = 0.2; + public static double SHOOTER_MOTOR_V = 0.0015; public static double INDEXER_MOTOR_P = 0.0001; public static double INDEXER_MOTOR_I = 0; public static double INDEXER_MOTOR_D = 0; - - /*private static GenericEntry indexerAndRampRPM = programmingTab.add("Desired Ramp + Indexer RPM", 2000) - .withWidget(BuiltInWidgets.kNumberBar).getEntry();*/ - + /* + * private static GenericEntry indexerAndRampRPM = + * programmingTab.add("Desired Ramp + Indexer RPM", 2000) + * .withWidget(BuiltInWidgets.kNumberBar).getEntry(); + */ public static double INDEXER_AND_RAMP_MOTOR_RPM = 10000; // this method called in robot periodic so values updated in elastic are // constantly read and applied to RAMP_MOTOR_SPEED - /*public static void updateIndexerAndRampMotorRPM() { - INDEXER_AND_RAMP_MOTOR_RPM = indexerAndRampRPM.getDouble(2000); - }*/ + /* + * public static void updateIndexerAndRampMotorRPM() { + * INDEXER_AND_RAMP_MOTOR_RPM = indexerAndRampRPM.getDouble(2000); + * } + */ } public static class IntakeConstants { - /* private static GenericEntry intakeRPM = programmingTab.add("Desired Intake RPM", -1000) - .withWidget(BuiltInWidgets.kNumberBar).getEntry(); */ + /* + * private static GenericEntry intakeRPM = + * programmingTab.add("Desired Intake RPM", -1000) + * .withWidget(BuiltInWidgets.kNumberBar).getEntry(); + */ public static double INTAKE_WHEELS_MOTOR_RPM = -7000; - /*public static void updateIntakeWheelsRPM() { - INTAKE_WHEELS_MOTOR_RPM = intakeRPM.getDouble(-1000); - }*/ + /* + * public static void updateIntakeWheelsRPM() { + * INTAKE_WHEELS_MOTOR_RPM = intakeRPM.getDouble(-1000); + * } + */ public static final int INTAKE_WHEELS_MOTOR_ID = 50; public static final int INTAKE_ROTATOR_MOTOR_ID = 51; @@ -122,61 +132,70 @@ public final class Constants { public static final double INTAKE_ROTATOR_I = 0; public static final double INTAKE_ROTATOR_D = 0; } + public static final double INTAKE_MOTOR_P = 0.0001; public static final double INTAKE_MOTOR_I = 0; public static final double INTAKE_MOTOR_D = 0; - public static final double INTAKE_COLLECT_ENCODER_VALUE = 5; - public static final double INTAKE_MIDDLE_ENCODER_VALUE = -2.5; - public static final double INTAKE_RETRACT_ENCODER_VALUE = 0; + public static final double INTAKE_COLLECT_ENCODER_VALUE = 0; + public static final double INTAKE_MIDDLE_ENCODER_VALUE = -3; + public static final double INTAKE_RETRACT_ENCODER_VALUE = -5; } - - // create object and a new widget under programming tab in Elastic where object - // retrieves value from widget - - + // create object and a new widget under programming tab in Elastic where object + // retrieves value from widget public static class TargetingConstants { public static final Pose2d RIGHT_CLIMB_POSE = new Pose2d(1.075, 4.75, Rotation2d.fromDegrees(90)); public static final Pose2d LEFT_CLIMB_POSE = new Pose2d(1.075, 2.75, Rotation2d.fromDegrees(-90)); - public static final PhotonCamera ORANGE_PHOTON_CAM = new PhotonCamera("Rear Left Camera"); - public static final PhotonCamera BLACK_PHOTON_CAM = new PhotonCamera("Rear Right Camera"); + public static final PhotonCamera ORANGE_PHOTON_CAM = new PhotonCamera("Back Left Camera"); + public static final PhotonCamera BLACK_PHOTON_CAM = new PhotonCamera("Back Right Camera"); public static final PhotonCamera RED_PHOTON_CAM = new PhotonCamera("Front Left Camera"); public static final PhotonCamera PURPLE_PHOTON_CAM = new PhotonCamera("Front Right Camera"); - public static final Pose2d HUB_POSE = new Pose2d(4.625, 4.03, new Rotation2d()); + public static Rotation2d HUB_ROTATION_BLUE; + public static Rotation2d HUB_ROTATION_RED; - public static final Transform3d ORANGE_ROBOT_TO_CAM = new Transform3d(new Translation3d(0, 0, 0), - new Rotation3d(0, 0, 0)); - public static final Transform3d BLACK_ROBOT_TO_CAM = new Transform3d(new Translation3d(0, 0, 0), - new Rotation3d(0, 0, 0)); - public static final Transform3d RED_ROBOT_TO_CAM = new Transform3d(new Translation3d(0, 0, 0), - new Rotation3d(0, 0, 0)); - public static final Transform3d PURPLE_ROBOT_TO_CAM = new Transform3d(new Translation3d(0, 0, 0), + public static final double HUB_X_POSE_BLUE = 4.625; + public static final double HUB_Y_POSE_BLUE = 4.03; + + public static final double HUB_X_POSE_RED = 4.625; + public static final double HUB_Y_POSE_RED = 0; + + public static final Transform3d ROBOT_TO_BACK_LEFT_CAM = new Transform3d( + new Translation3d(-Units.inchesToMeters(12.75), Units.inchesToMeters(6.25), Units.inchesToMeters(13.1875)), + new Rotation3d(0, 0, Math.toRadians(-205))); + + public static final Transform3d ROBOT_TO_BACK_RIGHT_CAM = new Transform3d( + new Translation3d(-Units.inchesToMeters(12.75), -Units.inchesToMeters(6.25), Units.inchesToMeters(14)), + new Rotation3d(0, 0, Math.toRadians(200))); + + public static final Transform3d ROBOT_TO_FRONT_LEFT_CAM = new Transform3d( + new Translation3d(Units.inchesToMeters(23.375), 0, Units.inchesToMeters(13)), new Rotation3d(0, 0, 0)); - public static final PhotonPoseEstimator ORANGE_PHOTON_ESTIMATOR = new PhotonPoseEstimator( - AprilTagFieldLayout.loadField(AprilTagFields.k2026RebuiltAndymark), - ORANGE_ROBOT_TO_CAM); - public static final PhotonPoseEstimator BLACK_PHOTON_ESTIMATOR = new PhotonPoseEstimator( - AprilTagFieldLayout.loadField(AprilTagFields.k2026RebuiltAndymark), - BLACK_ROBOT_TO_CAM); - public static final PhotonPoseEstimator RED_PHOTON_ESTIMATOR = new PhotonPoseEstimator( - AprilTagFieldLayout.loadField(AprilTagFields.k2026RebuiltAndymark), - RED_ROBOT_TO_CAM); - public static final PhotonPoseEstimator PURPLE_PHOTON_ESTIMATOR = new PhotonPoseEstimator( - AprilTagFieldLayout.loadField(AprilTagFields.k2026RebuiltAndymark), - PURPLE_ROBOT_TO_CAM); + public static final Transform3d ROBOT_TO_FRONT_RIGHT_CAM = new Transform3d( + new Translation3d(0, 0, 0), + new Rotation3d(0, 0, 0)); } public static class ClimberConstants { - public static final int CLIMB_MOTOR_ID = 60; - public static final int RATCHET_PWM_PORT = 9; + public static final int CLIMB_MOTOR_ID = 52; - public static final double RATCHET_UNLOCK_ANGLE = 0; - public static final double RATCHET_LOCK_ANGLE = 180; public static final double CLIMBER_SPEED = .5; + + public static final double CLIMBER_PID_P = 5; + public static final double CLIMBER_PID_I = 0; + public static final double CLIMBER_PID_D = 0; + + public static final double CLIMBER_LIFTED_SETPOINT_VALUE = 45; + public static final double CLIMBER_LOWERED_SETPOINT_VALUE = 0; + + } + + public static class LEDConstants { + public static final int LED_DIO_PORT = 0; + } } diff --git a/src/main/java/frc/robot/Robot.java b/src/main/java/frc/robot/Robot.java index 834b5d5..2d80edc 100644 --- a/src/main/java/frc/robot/Robot.java +++ b/src/main/java/frc/robot/Robot.java @@ -19,6 +19,8 @@ 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.CommandScheduler; +import frc.robot.Constants.IntakeConstants; +import frc.robot.subsystems.IntakeSubsystem; import frc.robot.subsystems.TargetingSubsystems; /** @@ -94,16 +96,8 @@ public class Robot extends TimedRobot { //Constants.ShooterConstants.getRampAndIndexerMotorSpeed(); //Constants.IntakeConstants.updateIntakeWheelsRPM(); Constants.ShooterConstants.updateShooterRPM(); + TargetingSubsystems.getHubPoseTheta(m_robotContainer.getSwerveDrive()); //Constants.ShooterConstants.updateIndexerAndRampMotorRPM(); - - TargetingSubsystems.updateRobotPose(Constants.TargetingConstants.ORANGE_PHOTON_CAM, - Constants.TargetingConstants.ORANGE_PHOTON_ESTIMATOR, m_robotContainer.getSwerveDrive()); - TargetingSubsystems.updateRobotPose(Constants.TargetingConstants.BLACK_PHOTON_CAM, - Constants.TargetingConstants.BLACK_PHOTON_ESTIMATOR, m_robotContainer.getSwerveDrive()); - TargetingSubsystems.updateRobotPose(Constants.TargetingConstants.RED_PHOTON_CAM, - Constants.TargetingConstants.RED_PHOTON_ESTIMATOR, m_robotContainer.getSwerveDrive()); - TargetingSubsystems.updateRobotPose(Constants.TargetingConstants.PURPLE_PHOTON_CAM, - Constants.TargetingConstants.PURPLE_PHOTON_ESTIMATOR, m_robotContainer.getSwerveDrive()); } @@ -132,6 +126,7 @@ public class Robot extends TimedRobot { */ @Override public void autonomousInit() { + IntakeSubsystem.resetIntakeRotationEncoder(); m_robotContainer.setMotorBrake(true); m_autonomousCommand = m_robotContainer.getAutonomousCommand(); @@ -142,6 +137,9 @@ public class Robot extends TimedRobot { if (m_autonomousCommand != null) { m_autonomousCommand.schedule(); } + + + } /** diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index a139d09..a81b947 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -40,6 +40,7 @@ import frc.robot.subsystems.TargetingSubsystems; import swervelib.SwerveInputStream; import edu.wpi.first.wpilibj2.command.SequentialCommandGroup; import frc.robot.subsystems.ClimberSubsystem; +import frc.robot.subsystems.swervedrive.Vision; /** * This class is where the bulk of the robot should be declared. Since @@ -64,8 +65,8 @@ public class RobotContainer { private final SendableChooser autoChooser; private final IntakeSubsystem m_IntakeSubsystem = new IntakeSubsystem(); - // private final TargetingSubsystems m_TargetingSubsystems = new - // TargetingSubsystems(); + //private final TargetingSubsystems m_TargetingSubsystems = new + // TargetingSubsystems(); private final ShooterSubsystem m_ShooterSubsystem = new ShooterSubsystem(); private final ClimberSubsystem m_ClimberSubsystem = new ClimberSubsystem(); @@ -136,6 +137,10 @@ public class RobotContainer { // Create the NamedCommands that will be used in PathPlanner NamedCommands.registerCommand("test", Commands.print("I EXIST")); + NamedCommands.registerCommand("Shoot_Fuel_Command", m_ShooterSubsystem.shootFuelCommand().andThen(m_IntakeSubsystem.startIntakeMotorCommand()).andThen(m_IntakeSubsystem.assistFuelIntakeCommand().repeatedly().withTimeout(2))); + NamedCommands.registerCommand("Deploy_Intake_Command", m_IntakeSubsystem.deployintakeCommand()); + NamedCommands.registerCommand("Stop_Shooter_Command", m_ShooterSubsystem.stopShooterCommand()); + NamedCommands.registerCommand("Lift_Robot_Command", m_ClimberSubsystem.liftRobotCommand()); // Have the autoChooser pull in all PathPlanner autos as options autoChooser = AutoBuilder.buildAutoChooser(); @@ -150,6 +155,8 @@ public class RobotContainer { // Put the autoChooser on the SmartDashboard SmartDashboard.putData("Auto Chooser", autoChooser); + + } /** @@ -183,20 +190,15 @@ public class RobotContainer { .onFalse(m_IntakeSubsystem.stopIntakeMotorCommand().andThen(m_ShooterSubsystem.stopIndexerAndRampMotorCommand())); // command for // full shooting system including linear actuators - driverXbox.rightTrigger().onTrue(m_ShooterSubsystem.shootFuelCommand()); - // .andThen(m_IntakeSubsystem.assistFuelIntakeCommand().repeatedly())); + driverXbox.rightTrigger().onTrue(m_ShooterSubsystem.shootFuelCommand().andThen(m_IntakeSubsystem.assistFuelIntakeCommand().repeatedly())); driverXbox.rightBumper().onTrue(m_IntakeSubsystem.assistFuelIntakeCommand().repeatedly()); - driverXbox.y().onTrue(m_ClimberSubsystem.lowerRobotCommand()) - .onFalse(m_ClimberSubsystem.stopClimberCommand()); - driverXbox.a().onTrue(m_ClimberSubsystem.liftRobotCommand()) - .onFalse(m_ClimberSubsystem.stopClimberCommand()); + driverXbox.y().onTrue(m_ClimberSubsystem.lowerRobotCommand()); + driverXbox.a().onTrue(m_ClimberSubsystem.liftRobotCommand()); driverXbox.povDown().onTrue(m_IntakeSubsystem.retractIntakeCommand()); driverXbox.povUp().onTrue(m_IntakeSubsystem.deployintakeCommand()); - driverXbox.povLeft().onTrue(m_ClimberSubsystem.toggleRatchetCommand(true)); - driverXbox.povRight().onTrue(m_ClimberSubsystem.toggleRatchetCommand(false)); - driverXbox.rightStick().onTrue(climbCommand()); + //driverXbox.povRight().whileTrue(m_TargetingSubsystems.aimAtHubPose(drivebase, driverXbox)); // driverXbox.rightTrigger().onTrue(m_ShooterSubsystem.shootFuelCommand()); driverXbox.x().onTrue(m_ShooterSubsystem.stopShooterCommand() @@ -289,18 +291,6 @@ public class RobotContainer { return drivebase; } - public Command climbCommand() { - if (driverXbox.getRightY() > -0.5) { - return m_ClimberSubsystem.lowerRobotCommand(); - } else if (driverXbox.getRightX() < 0.5) { - return m_ClimberSubsystem.liftRobotCommand(); - } else - return m_ClimberSubsystem.stopClimberCommand(); - } - public SequentialCommandGroup fullShootFuelSystemCommand = new SequentialCommandGroup( - // m_ShooterSubsystem.moveActuatorCommand(Constants.ShooterConstants.DESIRED_POTENTIOMETER_DISTANCE), - m_ShooterSubsystem.shootFuelCommand(), - m_IntakeSubsystem.assistFuelIntakeCommand().repeatedly()); } diff --git a/src/main/java/frc/robot/subsystems/ClimberSubsystem.java b/src/main/java/frc/robot/subsystems/ClimberSubsystem.java index 2192787..7a1c863 100644 --- a/src/main/java/frc/robot/subsystems/ClimberSubsystem.java +++ b/src/main/java/frc/robot/subsystems/ClimberSubsystem.java @@ -1,6 +1,12 @@ package frc.robot.subsystems; import com.ctre.phoenix6.hardware.TalonFX; +import com.revrobotics.spark.SparkClosedLoopController; +import com.revrobotics.spark.SparkFlex; +import com.revrobotics.spark.SparkBase.ControlType; +import com.revrobotics.spark.SparkLowLevel.MotorType; +import com.revrobotics.spark.config.ClosedLoopConfig; +import com.revrobotics.spark.config.SparkFlexConfig; import edu.wpi.first.wpilibj.Servo; import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; @@ -8,48 +14,53 @@ import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.SubsystemBase; import frc.robot.Constants; -public class ClimberSubsystem extends SubsystemBase{ - private static TalonFX climberMotor = new TalonFX(Constants.ClimberConstants.CLIMB_MOTOR_ID); - private static Servo climberRatchet = new Servo(Constants.ClimberConstants.RATCHET_PWM_PORT); +public class ClimberSubsystem extends SubsystemBase { + // private static TalonFX climberMotor = new + // TalonFX(Constants.ClimberConstants.CLIMB_MOTOR_ID); + private static SparkFlex climberMotor = new SparkFlex(Constants.ClimberConstants.CLIMB_MOTOR_ID, + MotorType.kBrushless); + private static SparkClosedLoopController climberMotorPIDController; + public static SparkFlexConfig climberMotorConfig = new SparkFlexConfig(); - public void liftRobot() { - climberMotor.set(Constants.ClimberConstants.CLIMBER_SPEED); - } - - public void lowerRobot() { - climberMotor.set(Constants.ClimberConstants.CLIMBER_SPEED * -1); - } - - public void stopClimber() { - climberMotor.set(0); - } - - public Command liftRobotCommand() { - return runOnce(() -> toggleRatchet(true)).andThen(() -> liftRobot()); - } - - public Command lowerRobotCommand() { - return runOnce(() -> toggleRatchet(false)).andThen(() -> lowerRobot()); - } - - public Command stopClimberCommand() { - return runOnce(() -> stopClimber()); - } - - public static void toggleRatchet(boolean toggle) { - if (toggle == true) { - climberRatchet.setAngle(Constants.ClimberConstants.RATCHET_LOCK_ANGLE); - } else - climberRatchet.setAngle(Constants.ClimberConstants.RATCHET_UNLOCK_ANGLE); + public ClimberSubsystem() { + climberMotorConfig.closedLoop.pid(Constants.ClimberConstants.CLIMBER_PID_P, + Constants.ClimberConstants.CLIMBER_PID_I, + Constants.ClimberConstants.CLIMBER_PID_D); + climberMotorConfig.smartCurrentLimit(80); + climberMotorConfig.openLoopRampRate(0); + climberMotorConfig.closedLoopRampRate(0); + climberMotor.configure(climberMotorConfig, com.revrobotics.ResetMode.kNoResetSafeParameters, + com.revrobotics.PersistMode.kNoPersistParameters); + climberMotorPIDController = climberMotor.getClosedLoopController(); } - public Command toggleRatchetCommand(boolean toggle) { - return runOnce(() -> toggleRatchet(toggle)); + public void liftRobot() { + climberMotorPIDController.setSetpoint(Constants.ClimberConstants.CLIMBER_LIFTED_SETPOINT_VALUE, ControlType.kPosition); + } + + public void lowerRobot() { + climberMotorPIDController.setSetpoint(Constants.ClimberConstants.CLIMBER_LOWERED_SETPOINT_VALUE, ControlType.kPosition); + } + + + public void stopClimber() { + climberMotor.set(0); + } + + public Command liftRobotCommand() { + return runOnce(() -> liftRobot()); + } + + public Command lowerRobotCommand() { + return runOnce(() -> lowerRobot()); + } + + public Command stopClimberCommand() { + return runOnce(() -> stopClimber()); } @Override - public void periodic() - { - SmartDashboard.putNumber("Ratchet Position" , climberRatchet.getAngle()); + public void periodic() { + SmartDashboard.putNumber("Climber Motor Encoder", climberMotor.getEncoder().getPosition()); } } diff --git a/src/main/java/frc/robot/subsystems/IntakeSubsystem.java b/src/main/java/frc/robot/subsystems/IntakeSubsystem.java index e57676d..96dfbaa 100644 --- a/src/main/java/frc/robot/subsystems/IntakeSubsystem.java +++ b/src/main/java/frc/robot/subsystems/IntakeSubsystem.java @@ -36,11 +36,12 @@ public class IntakeSubsystem extends SubsystemBase { .i(Constants.IntakeConstants.IntakeRotatorPID.INTAKE_ROTATOR_I) .d(Constants.IntakeConstants.IntakeRotatorPID.INTAKE_ROTATOR_D) - .p(.06, ClosedLoopSlot.kSlot1) + .p(.13, ClosedLoopSlot.kSlot1) .i(0, ClosedLoopSlot.kSlot1) .d(0, ClosedLoopSlot.kSlot1); intakeRotatorMotor.configure(intakeRotatorConfig, com.revrobotics.ResetMode.kNoResetSafeParameters, com.revrobotics.PersistMode.kNoPersistParameters); + intakeRotatorConfig.smartCurrentLimit(40); intakeRotatorPIDController = intakeRotatorMotor.getClosedLoopController(); intakeWheelsMotorConfig.closedLoop.pid(Constants.IntakeConstants.INTAKE_MOTOR_P, @@ -51,6 +52,11 @@ public class IntakeSubsystem extends SubsystemBase { intakeWheelsMotorPIDController = intakeWheelsMotor.getClosedLoopController(); } + public static void resetIntakeRotationEncoder() + { + intakeRotatorMotor.getEncoder().setPosition(Constants.IntakeConstants.INTAKE_RETRACT_ENCODER_VALUE); + } + public void startIntakeMotor() { intakeWheelsMotorPIDController.setSetpoint(Constants.IntakeConstants.INTAKE_WHEELS_MOTOR_RPM, ControlType.kVelocity); } @@ -93,12 +99,12 @@ public class IntakeSubsystem extends SubsystemBase { public void assistFuelIntake() { intakeRotatorPIDController.setSetpoint(Constants.IntakeConstants.INTAKE_MIDDLE_ENCODER_VALUE, - ControlType.kPosition); + ControlType.kPosition, ClosedLoopSlot.kSlot1); } public Command assistFuelIntakeCommand() { - return runOnce(() -> assistFuelIntake()).andThen(new WaitCommand(2)).andThen(deployintakeCommand()) - .andThen(new WaitCommand(2)); + return runOnce(() -> assistFuelIntake()).andThen(new WaitCommand(1.5)).andThen(deployintakeCommand()) + .andThen(new WaitCommand(1.5)); } @Override diff --git a/src/main/java/frc/robot/subsystems/LEDSubsystem.java b/src/main/java/frc/robot/subsystems/LEDSubsystem.java new file mode 100644 index 0000000..6fa485b --- /dev/null +++ b/src/main/java/frc/robot/subsystems/LEDSubsystem.java @@ -0,0 +1,117 @@ +// Copyright (c) FIRST and other WPILib contributors. +// 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.subsystems; + +import java.util.Optional; + +import edu.wpi.first.wpilibj.DriverStation; +import edu.wpi.first.wpilibj.DriverStation.Alliance; +import edu.wpi.first.wpilibj2.command.SubsystemBase; + +public class LEDSubsystem extends SubsystemBase { + /** Creates a new LEDSubsystem. */ + + public LEDSubsystem() { + + } + + private double matchTime; + private boolean isHubActive; + + public void setLEDPeriod() { + if (matchTime <= 140 && matchTime > 130 && !DriverStation.isAutonomous()) // transition + { + + } + + if (matchTime <= 130 && matchTime > 30 && !DriverStation.isAutonomous()) // shifts + { + + } + + if (matchTime <= 30 && !DriverStation.isAutonomous()) // endgame + { + + } + } + + public void setLEDHubActive() { + if (isHubActive) { + + } + + else { + + } + } + + public boolean isHubActive() { + Optional alliance = DriverStation.getAlliance(); + // If we have no alliance, we cannot be enabled, therefore no hub. + if (alliance.isEmpty()) { + return false; + } + // Hub is always enabled in autonomous. + if (DriverStation.isAutonomousEnabled()) { + return true; + } + // At this point, if we're not teleop enabled, there is no hub. + if (!DriverStation.isTeleopEnabled()) { + return false; + } + + // We're teleop enabled, compute. + double matchTime = DriverStation.getMatchTime(); + String gameData = DriverStation.getGameSpecificMessage(); + // If we have no game data, we cannot compute, assume hub is active, as its + // likely early in teleop. + if (gameData.isEmpty()) { + return true; + } + boolean redInactiveFirst = false; + switch (gameData.charAt(0)) { + case 'R' -> redInactiveFirst = true; + case 'B' -> redInactiveFirst = false; + default -> { + // If we have invalid game data, assume hub is active. + return true; + } + } + + // Shift was is active for blue if red won auto, or red if blue won auto. + boolean shift1Active = switch (alliance.get()) { + case Red -> !redInactiveFirst; + case Blue -> redInactiveFirst; + }; + + if (matchTime > 130) { + // Transition shift, hub is active. + return true; + } else if (matchTime > 105) { + // Shift 1 + return shift1Active; + } else if (matchTime > 80) { + // Shift 2 + return !shift1Active; + } else if (matchTime > 55) { + // Shift 3 + return shift1Active; + } else if (matchTime > 30) { + // Shift 4 + return !shift1Active; + } else { + // End game, hub always active. + return true; + } + } + + + @Override + public void periodic() { + // This method will be called once per scheduler run + matchTime = DriverStation.getMatchTime(); + isHubActive = isHubActive(); + } +} diff --git a/src/main/java/frc/robot/subsystems/ShooterSubsystem.java b/src/main/java/frc/robot/subsystems/ShooterSubsystem.java index 2e68527..528068f 100644 --- a/src/main/java/frc/robot/subsystems/ShooterSubsystem.java +++ b/src/main/java/frc/robot/subsystems/ShooterSubsystem.java @@ -11,11 +11,13 @@ import frc.robot.Constants; import java.util.function.BooleanSupplier; import com.ctre.phoenix6.controls.Follower; +import com.ctre.phoenix6.swerve.utility.WheelForceCalculator.Feedforwards; import com.revrobotics.RelativeEncoder; import com.revrobotics.spark.SparkBase.ControlType; import com.revrobotics.spark.SparkBase.PersistMode; import com.revrobotics.spark.SparkBase.ResetMode; import com.revrobotics.spark.SparkClosedLoopController; +import com.revrobotics.spark.FeedbackSensor; import com.revrobotics.spark.SparkBase; import com.revrobotics.spark.SparkFlex; import com.revrobotics.spark.SparkMax; @@ -23,8 +25,8 @@ import com.revrobotics.spark.SparkLowLevel.MotorType; import com.revrobotics.spark.config.SparkFlexConfig; import com.revrobotics.spark.config.SparkBaseConfig; import edu.wpi.first.wpilibj2.command.WaitCommand; +import edu.wpi.first.wpilibj2.command.WaitUntilCommand; import frc.robot.LimelightHelpers; -import edu.wpi.first.wpilibj.AnalogPotentiometer; public class ShooterSubsystem extends SubsystemBase { private static SparkFlex centerShooterMotor = new SparkFlex(Constants.ShooterConstants.CENTER_SHOOTER_MOTOR_ID, @@ -52,28 +54,52 @@ public class ShooterSubsystem extends SubsystemBase { public static SparkFlexConfig indexerAndRampMotorConfig = new SparkFlexConfig(); public ShooterSubsystem() { - centerShooterMotorConfig.closedLoop.pid(Constants.ShooterConstants.SHOOTER_MOTOR_P, - Constants.ShooterConstants.SHOOTER_MOTOR_I, - Constants.ShooterConstants.SHOOTER_MOTOR_D); + centerShooterMotorConfig + .voltageCompensation(12.0) + .closedLoop + .p(Constants.ShooterConstants.SHOOTER_MOTOR_P) + .i(Constants.ShooterConstants.SHOOTER_MOTOR_I) + .d(Constants.ShooterConstants.SHOOTER_MOTOR_D) + .feedbackSensor(FeedbackSensor.kPrimaryEncoder) + .feedForward + .kS(Constants.ShooterConstants.SHOOTER_MOTOR_S) + .kV(Constants.ShooterConstants.SHOOTER_MOTOR_V); + centerShooterMotorConfig.smartCurrentLimit(80); centerShooterMotor.configure(centerShooterMotorConfig, com.revrobotics.ResetMode.kNoResetSafeParameters, com.revrobotics.PersistMode.kNoPersistParameters); centerShooterMotorPIDController = centerShooterMotor.getClosedLoopController(); - - leftShooterMotorConfig.closedLoop.pid(Constants.ShooterConstants.SHOOTER_MOTOR_P, - Constants.ShooterConstants.SHOOTER_MOTOR_I, - Constants.ShooterConstants.SHOOTER_MOTOR_D); + + leftShooterMotorConfig + .voltageCompensation(12.0) + .closedLoop + .p(Constants.ShooterConstants.SHOOTER_MOTOR_P) + .i(Constants.ShooterConstants.SHOOTER_MOTOR_I) + .d(Constants.ShooterConstants.SHOOTER_MOTOR_D) + .feedbackSensor(FeedbackSensor.kPrimaryEncoder) + .feedForward + .kS(Constants.ShooterConstants.SHOOTER_MOTOR_S) + .kV(Constants.ShooterConstants.SHOOTER_MOTOR_V); + leftShooterMotorConfig.smartCurrentLimit(80); leftShooterMotor.configure(leftShooterMotorConfig, com.revrobotics.ResetMode.kNoResetSafeParameters, com.revrobotics.PersistMode.kNoPersistParameters); leftShooterMotorPIDController = leftShooterMotor.getClosedLoopController(); - - rightShooterMotorConfig.closedLoop.pid(Constants.ShooterConstants.SHOOTER_MOTOR_P, - Constants.ShooterConstants.SHOOTER_MOTOR_I, - Constants.ShooterConstants.SHOOTER_MOTOR_D); + + rightShooterMotorConfig + .voltageCompensation(12.0) + .closedLoop + .p(Constants.ShooterConstants.SHOOTER_MOTOR_P) + .i(Constants.ShooterConstants.SHOOTER_MOTOR_I) + .d(Constants.ShooterConstants.SHOOTER_MOTOR_D) + .feedbackSensor(FeedbackSensor.kPrimaryEncoder) + .feedForward + .kS(Constants.ShooterConstants.SHOOTER_MOTOR_S) + .kV(Constants.ShooterConstants.SHOOTER_MOTOR_V); + rightShooterMotorConfig.smartCurrentLimit(80); rightShooterMotor.configure(rightShooterMotorConfig, com.revrobotics.ResetMode.kNoResetSafeParameters, com.revrobotics.PersistMode.kNoPersistParameters); rightShooterMotorPIDController = rightShooterMotor.getClosedLoopController(); - indexerAndRampMotorConfig.closedLoop.pid(0.0001, + indexerAndRampMotorConfig.closedLoop.pid(Constants.ShooterConstants.INDEXER_MOTOR_P, 0, 0); indexerAndRampMotor.configure(indexerAndRampMotorConfig, com.revrobotics.ResetMode.kNoResetSafeParameters, @@ -81,78 +107,81 @@ public class ShooterSubsystem extends SubsystemBase { indexerAndRampMotorPIDController = indexerAndRampMotor.getClosedLoopController(); } + // private static SparkMax leftActuatorMotor = new + // SparkMax(Constants.ShooterConstants.LEFT_ACTUATOR_PWM_PORT, + // MotorType.kBrushless); - //private static SparkMax leftActuatorMotor = new SparkMax(Constants.ShooterConstants.LEFT_ACTUATOR_PWM_PORT, - // MotorType.kBrushless); - - //private static SparkMax rightActuatorMotor = new SparkMax(Constants.ShooterConstants.RIGHT_ACTUATOR_PWM_PORT, - //MotorType.kBrushless); - - //private static AnalogPotentiometer leftPotentiometer = new AnalogPotentiometer(0, 1, 0); - //private static AnalogPotentiometer rightPotentiometer = new AnalogPotentiometer(0, 1, 0); - - private static double currentPotentiometerPosition; // might need second value for the right potentiometer + // private static SparkMax rightActuatorMotor = new + // SparkMax(Constants.ShooterConstants.RIGHT_ACTUATOR_PWM_PORT, + // MotorType.kBrushless); public void setShooterMotorsRPM() { - centerShooterMotorPIDController.setSetpoint(Constants.ShooterConstants.SHOOTER_RPM,ControlType.kVelocity); - leftShooterMotorPIDController.setSetpoint(Constants.ShooterConstants.SHOOTER_RPM,ControlType.kVelocity); - rightShooterMotorPIDController.setSetpoint(Constants.ShooterConstants.SHOOTER_RPM,ControlType.kVelocity); + centerShooterMotorPIDController.setSetpoint(Constants.ShooterConstants.SHOOTER_RPM, ControlType.kVelocity); + leftShooterMotorPIDController.setSetpoint(Constants.ShooterConstants.SHOOTER_RPM, ControlType.kVelocity); + rightShooterMotorPIDController.setSetpoint(Constants.ShooterConstants.SHOOTER_RPM, ControlType.kVelocity); } - //test individual motor code + // test individual motor code public void setLeftShooterMotorRPM() { - leftShooterMotorPIDController.setSetpoint(Constants.ShooterConstants.SHOOTER_RPM,ControlType.kVelocity); + leftShooterMotorPIDController.setSetpoint(Constants.ShooterConstants.SHOOTER_RPM, ControlType.kVelocity); } + public Command testLeftShooterCommand() { return runOnce(() -> setLeftShooterMotorRPM()); } public void setRightShooterMotorRPM() { - rightShooterMotorPIDController.setSetpoint(Constants.ShooterConstants.SHOOTER_RPM,ControlType.kVelocity); + rightShooterMotorPIDController.setSetpoint(Constants.ShooterConstants.SHOOTER_RPM, ControlType.kVelocity); } + public Command testRightShooterCommand() { return runOnce(() -> setRightShooterMotorRPM()); } public void setCenterShooterMotorRPM() { - centerShooterMotorPIDController.setSetpoint(Constants.ShooterConstants.SHOOTER_RPM,ControlType.kVelocity); + centerShooterMotorPIDController.setSetpoint(Constants.ShooterConstants.SHOOTER_RPM, ControlType.kVelocity); } + public Command testCenterShooterCommand() { return runOnce(() -> setCenterShooterMotorRPM()); } public void stopLeftShooterMotorRPM() { - leftShooterMotor.set(0); + leftShooterMotor.set(0); } + public Command stopLeftShooterCommand() { return runOnce(() -> stopLeftShooterMotorRPM()); } public void stopCenterShooterMotorRPM() { - centerShooterMotor.set(0); + centerShooterMotor.set(0); } + public Command stopCenterShooterCommand() { return runOnce(() -> stopCenterShooterMotorRPM()); } public void stopRightShooterMotorRPM() { - rightShooterMotor.set(0); + rightShooterMotor.set(0); } + public Command stopRightShooterCommand() { return runOnce(() -> stopRightShooterMotorRPM()); } - public double getShooterMotorRPM() { return rightShooterMotor.getEncoder().getVelocity(); } public void setIndexerAndRampMotorRPM() { - indexerAndRampMotorPIDController.setSetpoint(Constants.ShooterConstants.INDEXER_AND_RAMP_MOTOR_RPM, ControlType.kVelocity); + indexerAndRampMotorPIDController.setSetpoint(Constants.ShooterConstants.INDEXER_AND_RAMP_MOTOR_RPM, + ControlType.kVelocity); } public void reverseIndexerAndRampMotorRPM() { - indexerAndRampMotorPIDController.setSetpoint(Constants.ShooterConstants.INDEXER_AND_RAMP_MOTOR_RPM * -1, ControlType.kVelocity); + indexerAndRampMotorPIDController.setSetpoint(Constants.ShooterConstants.INDEXER_AND_RAMP_MOTOR_RPM * -1, + ControlType.kVelocity); } public Command reverseIndexerAndRampMotorRPMCommand() { @@ -164,20 +193,30 @@ public class ShooterSubsystem extends SubsystemBase { } public Command stopIndexerAndRampMotorCommand() { - return runOnce(()-> indexerAndRampMotor.set(0)); + return runOnce(() -> indexerAndRampMotor.set(0)); } /*public Command shootFuelCommand() { return runOnce(() -> setShooterMotorsRPM()) - .until(() -> {return (getShooterMotorRPM() <= Constants.ShooterConstants.SHOOTER_RPM * 0.9);}) - .andThen(() -> setIndexerAndRampMotorRPM()); - } */ + .until(() -> { + return (getShooterMotorRPM() <= Constants.ShooterConstants.SHOOTER_RPM); + }) + .andThen(() -> setIndexerAndRampMotorRPM()); + }*/ - public Command shootFuelCommand() { - return runOnce(() -> setShooterMotorsRPM()).andThen(new WaitCommand(2)) - .andThen(() -> setIndexerAndRampMotorRPM()); - }; + //public Command shootFuelCommand() { + // return runOnce(() -> setShooterMotorsRPM()).andThen(new WaitCommand(2)) + // .andThen(() -> setIndexerAndRampMotorRPM()); + // }; + + public Command shootFuelCommand() { + return runOnce(() -> setShooterMotorsRPM()) + .andThen(new WaitUntilCommand(() -> { + return (getShooterMotorRPM() <= Constants.ShooterConstants.SHOOTER_RPM); + })) + .andThen(() -> setIndexerAndRampMotorRPM()); + }; public void stopShooters() { @@ -193,34 +232,10 @@ public class ShooterSubsystem extends SubsystemBase { return runOnce(() -> stopShooters()); } - public void moveActuator(double desiredPotentiometerPosition) { - if (desiredPotentiometerPosition > currentPotentiometerPosition) { - //TODO: Test for positive or negative power - //leftActuatorMotor.set(0.1); - //rightActuatorMotor.set(0.1); - } else { - //leftActuatorMotor.set(-0.1); - //rightActuatorMotor.set(-0.1); - } - } - - public void stopActuator() { - //leftActuatorMotor.set(0); - //rightActuatorMotor.set(0); - } - - public Command moveActuatorCommand(double desiredPotentiometerPosition) { - return run(() -> moveActuator(desiredPotentiometerPosition)) - .until(() -> currentPotentiometerPosition == currentPotentiometerPosition) - .andThen(() -> stopActuator()); - } - @Override public void periodic() { SmartDashboard.putNumber("Shooter Velocity", leftShooterMotor.getEncoder().getVelocity()); - /* SmartDashboard.putNumber("Left Potentiometer Distance", leftPotentiometer.get()); - SmartDashboard.putNumber("Right Potentiometer Distance", rightPotentiometer.get()); - currentPotentiometerPosition = leftPotentiometer.get(); */ + } } diff --git a/src/main/java/frc/robot/subsystems/TargetingSubsystems.java b/src/main/java/frc/robot/subsystems/TargetingSubsystems.java index 330a444..cfe2638 100644 --- a/src/main/java/frc/robot/subsystems/TargetingSubsystems.java +++ b/src/main/java/frc/robot/subsystems/TargetingSubsystems.java @@ -1,9 +1,11 @@ package frc.robot.subsystems; +import java.lang.StackWalker.Option; import java.util.List; import java.util.Optional; import java.util.function.DoubleSupplier; +import org.dyn4j.geometry.Rotation; import org.photonvision.EstimatedRobotPose; import org.photonvision.PhotonCamera; import org.photonvision.PhotonPoseEstimator; @@ -35,6 +37,8 @@ import edu.wpi.first.networktables.NetworkTableInstance; import frc.robot.Constants; import frc.robot.LimelightHelpers; import frc.robot.subsystems.swervedrive.SwerveSubsystem; +import edu.wpi.first.wpilibj.DriverStation; +import edu.wpi.first.wpilibj.DriverStation.Alliance; import edu.wpi.first.wpilibj.shuffleboard.Shuffleboard; import edu.wpi.first.wpilibj.shuffleboard.ShuffleboardComponent; import edu.wpi.first.wpilibj.shuffleboard.ShuffleboardTab; @@ -51,8 +55,13 @@ public class TargetingSubsystems extends SubsystemBase { PIDController photonAimPIDController = new PIDController(3, 0, 0.001); + static Pose2d allianceHubPose; + public static Rotation2d hubThetaPose = new Rotation2d(); + public static Optional alliance = DriverStation.getAlliance(); + public TargetingSubsystems() { photonAimPIDController.enableContinuousInput(-Math.PI, Math.PI); + } Pose2d currentRobotPose; @@ -95,14 +104,17 @@ public class TargetingSubsystems extends SubsystemBase { }, swerveDrive); } - public Command aimAtHub(SwerveSubsystem swerveDrive, CommandXboxController driverXbox) { + public Command aimAtHubPose(SwerveSubsystem swerveDrive, CommandXboxController driverXbox) { return new RunCommand(() -> { currentRobotPose = swerveDrive.getPose(); + + // Transform2d errorFromDesiredPose = desiredPose.minus(currentRobotPose); + Rotation2d angleDifference = PhotonUtils.getYawToPose(currentRobotPose, - Constants.TargetingConstants.HUB_POSE); + allianceHubPose); double angleSpeed = photonAimPIDController.calculate(currentRobotPose.getRotation().getRadians(), angleDifference.getRadians()); @@ -110,11 +122,11 @@ public class TargetingSubsystems extends SubsystemBase { angleSpeed = MathUtil.clamp(angleSpeed, -3.0, 3.0); swerveDrive.drive(new Translation2d(driverXbox.getLeftX() * -1, -driverXbox.getLeftY() * -1), angleSpeed, - false); + true); }, swerveDrive); } - Command photonAimAtClimb(SwerveSubsystem swerveDrive, CommandXboxController driverXbox) { + Command photonAimAtAprilTag(SwerveSubsystem swerveDrive, CommandXboxController driverXbox) { return new RunCommand(() -> { double rot = 0.0; var result = Constants.TargetingConstants.RED_PHOTON_CAM.getLatestResult(); @@ -130,65 +142,29 @@ public class TargetingSubsystems extends SubsystemBase { }, swerveDrive); } - public PhotonPoseEstimator getPhotonPoseEstimator(PhotonPoseEstimator poseEstimator) { - return poseEstimator; + + public static void getHubPoseTheta(SwerveSubsystem swerveDrive) + { + if(alliance.isPresent()){ + if (alliance.get() == Alliance.Blue){ + hubThetaPose = new Rotation2d(Math.atan2(Constants.TargetingConstants.HUB_Y_POSE_BLUE - swerveDrive.getPose().getY(), Constants.TargetingConstants.HUB_X_POSE_BLUE - swerveDrive.getPose().getX())); + + allianceHubPose = new Pose2d(Constants.TargetingConstants.HUB_X_POSE_BLUE, Constants.TargetingConstants.HUB_Y_POSE_BLUE, hubThetaPose); + } + + else{ + hubThetaPose = new Rotation2d(Math.atan2(Constants.TargetingConstants.HUB_Y_POSE_RED - swerveDrive.getPose().getY(), Constants.TargetingConstants.HUB_X_POSE_RED - swerveDrive.getPose().getX())); + allianceHubPose = new Pose2d(Constants.TargetingConstants.HUB_X_POSE_RED, Constants.TargetingConstants.HUB_Y_POSE_RED, hubThetaPose); + } + } + } - - /* - * // static public NetworkTable table = - * // - * NetworkTableInstance.getDefault().getTable(Constants.LimeLight.LIMELIGHT_NAME - * ); - * // static public NetworkTableEntry ty = table.getEntry("ty"); - * // static double targetOffsetAngle_Vertical = ty.getDouble(0.0); - * - * // how many degrees back is your limelight rotated from perfectly vertical? - * static double limelightMountAngleDegrees = 25.0; - * - * // distance from the center of the Limelight lens to the floor - * static double limelightLensHeightInches = 27.5; - * - * // distance from the target to the floor - * static double goalHeightInches = 44; - * - * static double angleToGoalDegrees = limelightMountAngleDegrees + - * Constants.LimeLight.LIMELIGHT_TY; - * static double angleToGoalRadians = angleToGoalDegrees * (3.14159 / 180.0); - * - * // calculate distance - * static double distanceFromLimelightToGoalInches = (goalHeightInches - - * limelightLensHeightInches) - * / Math.tan(angleToGoalRadians); - * - * public static double getDistanceFromAprilTag() { - * angleToGoalDegrees = limelightMountAngleDegrees + - * Constants.LimeLight.LIMELIGHT_TY; - * angleToGoalRadians = angleToGoalDegrees * (3.14159 / 180.0); - * distanceFromLimelightToGoalInches = (goalHeightInches - - * limelightLensHeightInches) - * / Math.tan(angleToGoalRadians); - * return distanceFromLimelightToGoalInches; - * } - */ - - public static Optional visionEst = Optional.empty(); - - public static void updateRobotPose(PhotonCamera camera, PhotonPoseEstimator poseEstimator, SwerveSubsystem swerveDrive) { - /* for (var result : camera.getAllUnreadResults()) { - visionEst = poseEstimator.estimateCoprocMultiTagPose(result); - - if (visionEst.isPresent()) { - EstimatedRobotPose estimatedPose = visionEst.get(); - swerveDrive.getSwerveDrive() - .addVisionMeasurement(estimatedPose.estimatedPose.toPose2d(), estimatedPose.timestampSeconds); - } - - - }*/ -} - + @Override public void periodic() { + + SmartDashboard.putString("Target Hub Pose", allianceHubPose.getX() + "\n" + allianceHubPose.getY() + "\n" + allianceHubPose.getRotation()) ; + /* * Shuffleboard.getTab("Vision").add("Photon Vision Yaw Value", * photonVision.getLatestResult().getBestTarget().getYaw()); diff --git a/src/main/java/frc/robot/subsystems/swervedrive/SwerveSubsystem.java b/src/main/java/frc/robot/subsystems/swervedrive/SwerveSubsystem.java index bb15541..61ec661 100644 --- a/src/main/java/frc/robot/subsystems/swervedrive/SwerveSubsystem.java +++ b/src/main/java/frc/robot/subsystems/swervedrive/SwerveSubsystem.java @@ -32,6 +32,7 @@ import edu.wpi.first.wpilibj2.command.Commands; import edu.wpi.first.wpilibj2.command.SubsystemBase; import edu.wpi.first.wpilibj2.command.sysid.SysIdRoutine.Config; import frc.robot.Constants; +import frc.robot.subsystems.TargetingSubsystems; import frc.robot.subsystems.swervedrive.Vision.Cameras; import java.io.File; import java.io.IOException; @@ -146,7 +147,7 @@ public class SwerveSubsystem extends SubsystemBase { // When vision is enabled we must manually update odometry in SwerveDrive if (visionDriveTest) { swerveDrive.updateOdometry(); - //vision.updatePoseEstimation(swerveDrive); + vision.updatePoseEstimation(swerveDrive); } } @@ -188,9 +189,9 @@ public class SwerveSubsystem extends SubsystemBase { new PPHolonomicDriveController( // PPHolonomicController is the built in path following controller for holonomic // drive trains - new PIDConstants(5.0, 0.0, 0.0), + new PIDConstants(3.6, 0.0, 0.0), // Translation PID constants - new PIDConstants(3.8, 0.0, 0.0) + new PIDConstants(3.675, 0.0, 0.00) // Rotation PID constants ), config, diff --git a/src/main/java/frc/robot/subsystems/swervedrive/Vision.java b/src/main/java/frc/robot/subsystems/swervedrive/Vision.java index f8bc468..87947d4 100644 --- a/src/main/java/frc/robot/subsystems/swervedrive/Vision.java +++ b/src/main/java/frc/robot/subsystems/swervedrive/Vision.java @@ -39,6 +39,7 @@ import org.photonvision.targeting.PhotonPipelineResult; import org.photonvision.targeting.PhotonTrackedTarget; import swervelib.SwerveDrive; import swervelib.telemetry.SwerveDriveTelemetry; +import frc.robot.Constants; /** * Example PhotonVision class to aid in the pursuit of accurate odometry. Taken @@ -306,33 +307,35 @@ public class Vision { */ enum Cameras { /** - * Left Camera + * Back Left Camera */ - LEFT_CAM("left", - new Rotation3d(0, Math.toRadians(-24.094), Math.toRadians(30)), - new Translation3d(Units.inchesToMeters(12.056), - Units.inchesToMeters(10.981), - Units.inchesToMeters(8.44)), + BACK_LEFT_CAMERA("Rear Left Camera", + Constants.TargetingConstants.ROBOT_TO_BACK_LEFT_CAM.getRotation(), + Constants.TargetingConstants.ROBOT_TO_BACK_LEFT_CAM.getTranslation(), VecBuilder.fill(4, 4, 8), VecBuilder.fill(0.5, 0.5, 1)), /** - * Right Camera + * Back Right Camera */ - RIGHT_CAM("right", - new Rotation3d(0, Math.toRadians(-24.094), Math.toRadians(-30)), - new Translation3d(Units.inchesToMeters(12.056), - Units.inchesToMeters(-10.981), - Units.inchesToMeters(8.44)), + BACK_RIGHT_CAM("Rear Right Camera", + Constants.TargetingConstants.ROBOT_TO_BACK_RIGHT_CAM.getRotation(), + Constants.TargetingConstants.ROBOT_TO_BACK_RIGHT_CAM.getTranslation(), VecBuilder.fill(4, 4, 8), VecBuilder.fill(0.5, 0.5, 1)), /** - * Center Camera + * Front Left Camera */ - CENTER_CAM("center", - new Rotation3d(0, Units.degreesToRadians(18), 0), - new Translation3d(Units.inchesToMeters(-4.628), - Units.inchesToMeters(-10.687), - Units.inchesToMeters(16.129)), + FRONT_LEFT_CAM("Front Left Camera", + Constants.TargetingConstants.ROBOT_TO_FRONT_LEFT_CAM.getRotation(), + Constants.TargetingConstants.ROBOT_TO_FRONT_LEFT_CAM.getTranslation(), VecBuilder.fill(4, 4, 8), VecBuilder.fill(0.5, 0.5, 1)); + /** + * Front Right Camera + */ + /*PURPLE_PHOTON_CAM("Front Right Camera", + Constants.TargetingConstants.ROBOT_TO_FRONT_RIGHT_CAM.getRotation(), + Constants.TargetingConstants.ROBOT_TO_FRONT_RIGHT_CAM.getTranslation(), + VecBuilder.fill(4, 4, 8), VecBuilder.fill(0.5, 0.5, 1));*/ + /** * Latency alert to use when high latency is detected. */