I think I put the command in correctly

diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java
index 7727ec76e62810e03c6857a6214914ab0fc94743..4ad3c92553887f0e265fc93eee72111e90c16215 100644 (file)
--- 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 a0d349efd9b5827f47fd9ec2d5e5e48a625b7f87..af0afb82b98c3863cbba18dfcc53b4c3c2f07cd2 100644 (file)
--- 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 5a8ff99b902070325f5c4f6e877049b4ddd66c13..8475f05b564b0a125c07d54ea74133e51d1e8e3c 100644 (file)
--- 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 6ed464805be90abb281abd113322bb058cba70ec..09f69eeec118377a07da2fcc5c454ec9677f5f61 100644 (file)
--- 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();