From 05d21c1354795c7085e5707694f7c5b3b1f7128c Mon Sep 17 00:00:00 2001 From: Wesley28w Date: Tue, 20 Jan 2026 08:59:34 -0800 Subject: [PATCH] I think I put the command in correctly --- src/main/java/frc/robot/RobotContainer.java | 5 ++++- .../controls/PS5ControllerDriverConfig.java | 20 +++++++++++++++++-- .../frc/robot/subsystems/turret/Turret.java | 18 ++++++++--------- .../frc/robot/util/Vision/TurretVision.java | 12 +++++------ 4 files changed, 36 insertions(+), 19 deletions(-) diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 7727ec7..4ad3c92 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -30,6 +30,7 @@ import frc.robot.subsystems.shooter.Shooter; import frc.robot.subsystems.turret.Turret; import frc.robot.util.PathGroupLoader; import frc.robot.util.Vision.DetectedObject; +import frc.robot.util.Vision.TurretVision; import frc.robot.util.Vision.Vision; /** @@ -46,6 +47,7 @@ public class RobotContainer { private Drivetrain drive = null; private Vision vision = null; private Turret turret = null; + private TurretVision turretVision = null; private Shooter shooter = null; private Command auto = new DoNothing(); @@ -61,6 +63,7 @@ public class RobotContainer { public RobotContainer(RobotId robotId) { turret = new Turret(); shooter = new Shooter(); + turretVision = new TurretVision("SOME CAMERA NAME"); // TODO: Get a real camera // dispatch on the robot switch (robotId) { case TestBed1: @@ -80,7 +83,7 @@ public class RobotContainer { case Phil: case Vertigo: drive = new Drivetrain(vision, new GyroIOPigeon2()); - driver = new PS5ControllerDriverConfig(drive); + driver = new PS5ControllerDriverConfig(drive, shooter, turret, turretVision); operator = new Operator(drive); // Detected objects need access to the drivetrain diff --git a/src/main/java/frc/robot/controls/PS5ControllerDriverConfig.java b/src/main/java/frc/robot/controls/PS5ControllerDriverConfig.java index a0d349e..af0afb8 100644 --- a/src/main/java/frc/robot/controls/PS5ControllerDriverConfig.java +++ b/src/main/java/frc/robot/controls/PS5ControllerDriverConfig.java @@ -7,17 +7,20 @@ import edu.wpi.first.math.geometry.Rotation2d; import edu.wpi.first.math.geometry.Translation2d; import edu.wpi.first.math.util.Units; import edu.wpi.first.wpilibj.DriverStation.Alliance; +import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.CommandScheduler; import edu.wpi.first.wpilibj2.command.FunctionalCommand; import edu.wpi.first.wpilibj2.command.InstantCommand; import edu.wpi.first.wpilibj2.command.SequentialCommandGroup; import frc.robot.Robot; +import frc.robot.commands.gpm.TurretAutoShoot; import frc.robot.constants.Constants; import frc.robot.constants.FieldConstants; import frc.robot.subsystems.drivetrain.Drivetrain; import frc.robot.subsystems.shooter.Shooter; import frc.robot.subsystems.shooter.ShooterConstants; import frc.robot.subsystems.turret.Turret; +import frc.robot.util.Vision.TurretVision; import lib.controllers.PS5Controller; import lib.controllers.PS5Controller.PS5Axis; import lib.controllers.PS5Controller.PS5Button; @@ -30,12 +33,16 @@ public class PS5ControllerDriverConfig extends BaseDriverConfig { private final BooleanSupplier slowModeSupplier = ()->false; private Shooter shooter; private Turret turret; + private TurretVision turretVision; + private Pose2d alignmentPose = null; + private Command turretAutoShoot; - public PS5ControllerDriverConfig(Drivetrain drive, Shooter shooter) { + public PS5ControllerDriverConfig(Drivetrain drive, Shooter shooter, Turret turret, TurretVision turretVision) { super(drive); this.shooter = shooter; this.turret = turret; + this.turretVision = turretVision; } public void configureControls() { @@ -61,7 +68,16 @@ public class PS5ControllerDriverConfig extends BaseDriverConfig { driver.get(PS5Button.CIRCLE).onTrue(new InstantCommand(() -> shooter.setFeeder(ShooterConstants.FEEDER_RUN_POWER))).onFalse(new InstantCommand(() -> shooter.setFeeder(0))); driver.get(PS5Button.TRIANGLE).onTrue(new InstantCommand(() -> shooter.setShooter(ShooterConstants.SHOOTER_VELOCITY))).onFalse(new InstantCommand(() -> shooter.setShooter(0))); - driver.get(PS5Button.SQUARE).onTrue(new InstantCommand(() -> turret.turnOnAlignment())).onFalse(new InstantCommand(() -> turret.turnOffAlignment())); + driver.get(PS5Button.SQUARE).onTrue( + new InstantCommand(()->{ + if (turretAutoShoot != null && turretAutoShoot.isScheduled()){ + turretAutoShoot.cancel(); + } else{ + turretAutoShoot = new TurretAutoShoot(turret, getDrivetrain(), turretVision); + CommandScheduler.getInstance().schedule(turretAutoShoot); + } + }) + ); } public void setAlignmentPose(){ diff --git a/src/main/java/frc/robot/subsystems/turret/Turret.java b/src/main/java/frc/robot/subsystems/turret/Turret.java index 5a8ff99..8475f05 100644 --- a/src/main/java/frc/robot/subsystems/turret/Turret.java +++ b/src/main/java/frc/robot/subsystems/turret/Turret.java @@ -25,12 +25,12 @@ import edu.wpi.first.wpilibj2.command.SubsystemBase; import frc.robot.constants.Constants; import frc.robot.constants.FieldConstants; import frc.robot.constants.IdConstants; -import frc.robot.subsystems.drivetrain.Drivetrain; public class Turret extends SubsystemBase { // double D_x = 1; // double D_y = 1; final private TalonFX motor; + // Enable here: BUT PROB wont use it private boolean infiniteRotation = false; private double versaPlanetaryGearRatio = 5.0; private double turretGearRatio = 140.0/10.0; @@ -53,10 +53,7 @@ public class Turret extends SubsystemBase { MechanismRoot2d mechanismRoot = mechanism2d.getRoot("pivot", 50, 50); MechanismLigament2d ligament2d = mechanismRoot.append(new MechanismLigament2d("turret_motor", 25, 0)); - private Drivetrain drivetrain; - - public Turret(Drivetrain drivetrain) { - this.drivetrain = drivetrain; + public Turret() { motor = new TalonFX(IdConstants.TURRET_MOTOR_ID, Constants.CANIVORE_CAN); // switch of course if (RobotBase.isSimulation()) { @@ -166,15 +163,16 @@ public class Turret extends SubsystemBase { setpoint = MathUtil.clamp(setpointDegrees, TurretConstants.MIN_ANGLE, TurretConstants.MAX_ANGLE); if (infiniteRotation) { - // 1. Get current position in degrees + // Get current position in degrees double currentDegrees = (motor.getPosition().getValueAsDouble() / gearRatio) * 360.0; - // 2. Calculate the error + // Calculate the error double error = setpoint - currentDegrees; - // 3. Wrap the error to [-180, 180] + // Wrap the error to [-180, 180] // This finds the "remainder" of the distance relative to a full circle double optimizedError = Math.IEEEremainder(error, 360.0); - // 4. Calculate new target in degrees + // Calculate new target in degrees double optimizedSetpointDegrees = currentDegrees + optimizedError; + double motorTargetRotations = (optimizedSetpointDegrees / 360.0) * gearRatio; motor.setControl(voltageRequest.withPosition(motorTargetRotations)); } else { @@ -212,7 +210,7 @@ public class Turret extends SubsystemBase { double simRotations = Units.radiansToRotations(simAngle); double motorRotations = simRotations * gearRatio; - encoderSim.setRawRotorPosition(motorRotations); // MUST set position + encoderSim.setRawRotorPosition(motorRotations); encoderSim.setRotorVelocity(turretSim.getVelocityRadPerSec() * Units.radiansToRotations(1) * gearRatio); // Gear Ratio } } \ No newline at end of file diff --git a/src/main/java/frc/robot/util/Vision/TurretVision.java b/src/main/java/frc/robot/util/Vision/TurretVision.java index 6ed4648..09f69ee 100644 --- a/src/main/java/frc/robot/util/Vision/TurretVision.java +++ b/src/main/java/frc/robot/util/Vision/TurretVision.java @@ -24,15 +24,15 @@ public class TurretVision { PhotonPipelineResult result = camera.getLatestResult(); if (!result.hasTargets()) { - return Optional.empty(); + return Optional.empty(); } for (PhotonTrackedTarget target : result.getTargets()) { - if (target.getFiducialId() == tagId) { - // PhotonVision yaw is in DEGREES - double yawRad = Units.degreesToRadians(target.getYaw()); - return Optional.of(yawRad); - } + if (target.getFiducialId() == tagId) { + // PhotonVision yaw is in DEGREES + double yawRad = Units.degreesToRadians(target.getYaw()); + return Optional.of(yawRad); + } } return Optional.empty(); -- 2.39.5