Merge branch 'main' of https://github.com/Team334/R2024

src/main/deploy/pathplanner/autos/2 Piece Auton Line (Bottom).auto

@@ -24,22 +24,9 @@
           }
         },
         {
-          "type": "race",
+          "type": "named",
           "data": {
-            "commands": [
-              {
-                "type": "wait",
-                "data": {
-                  "waitTime": 2.0
-                }
-              },
-              {
-                "type": "named",
-                "data": {
-                  "name": "actuateIn"
-                }
-              }
-            ]
+            "name": "actuateInFast"
           }
         },
         {

src/main/java/frc/robot/RobotContainer.java

@@ -77,14 +77,18 @@ public class RobotContainer {
    * The container for the robot. Contains subsystems, OI devices, and commands.
    */
   public RobotContainer() {
-    NamedCommands.registerCommand("actuateOut", new SetShooter(_shooterSubsystem, () -> Presets.ACTUATE_SHOOTER_ANGLE).andThen(
-      new FeedActuate(_intakeSubsystem, ActuatorState.OUT, FeedMode.INTAKE))
-    );
+    // brings the actuator out, while intaking in
+    NamedCommands.registerCommand("actuateOut", new FeedActuate(_intakeSubsystem, ActuatorState.OUT, FeedMode.INTAKE));
 
+    // brings the actuator in, while intaking and revving up the shooter for a shot, this will take time and is done while driving
     NamedCommands.registerCommand("actuateIn", new FeedActuate(_intakeSubsystem, ActuatorState.STOWED, FeedMode.INTAKE).alongWith(
       new SpinShooter(_shooterSubsystem, ShooterState.SHOOT, true)
     ));
 
+    // brings the actuator in immediately, this is fast and shooting can't be done immediately 
+    NamedCommands.registerCommand("actuateInFast", new FeedActuate(_intakeSubsystem, ActuatorState.STOWED, FeedMode.NONE, true));
+
+    // revs and intakes if necessary while aiming, then shoots
     NamedCommands.registerCommand("shoot", new AutonShoot(_shooterSubsystem, _elevatorSubsystem, _ledSubsystem, _swerveSubsystem, _intakeSubsystem));
 
     // Drive/Operate default commands
@@ -119,7 +123,7 @@ public RobotContainer() {
 
   // to configure button bindings
   private void configureBindings() {
-    Command feedOut = new FeedActuate(_intakeSubsystem, ActuatorState.OUT).onlyWhile(() -> !_intakeSubsystem.atDesiredActuatorState()).andThen(
+    Command feedOut = new FeedActuate(_intakeSubsystem, ActuatorState.OUT, FeedMode.NONE, true).andThen(
       new FeedActuate(_intakeSubsystem, ActuatorState.OUT, FeedMode.OUTTAKE)
     );
 
@@ -144,6 +148,7 @@ private void configureBindings() {
     _driveController.L1().onTrue(Commands.runOnce(_swerveSubsystem::toggleSpeed, _swerveSubsystem));
     _driveController.R1().onTrue(Commands.runOnce(() -> _swerveSubsystem.fieldOriented = !_swerveSubsystem.fieldOriented, _swerveSubsystem));
     _driveController.cross().whileTrue(new BrakeSwerve(_swerveSubsystem, _ledSubsystem));
+    _driveController.square().onTrue(Commands.runOnce(() -> AutonShoot.canShoot = false));
 
     // TESTING ONLY!!!
     _driveController.triangle().onTrue(Commands.runOnce(() -> _swerveSubsystem.resetGyro(180), _swerveSubsystem));

src/main/java/frc/robot/commands/auto/AutonShoot.java

@@ -13,6 +13,7 @@
 import edu.wpi.first.wpilibj2.command.ParallelRaceGroup;
 import edu.wpi.first.wpilibj2.command.PrintCommand;
 import edu.wpi.first.wpilibj2.command.SequentialCommandGroup;
+import edu.wpi.first.wpilibj2.command.WaitCommand;
 import frc.robot.Constants.Presets;
 import frc.robot.commands.intake.FeedActuate;
 import frc.robot.commands.shooter.SpinShooter;
@@ -30,7 +31,11 @@
 // information, see:
 // https://docs.wpilib.org/en/stable/docs/software/commandbased/convenience-features.html
 public class AutonShoot extends SequentialCommandGroup {
-  private static boolean _preloadShot = false;
+  /** 
+   * Indicates whether the note in the intake can be shot right away, this means that the note is already squished, and
+   * that the shooter is already revved enough.
+   */
+  public static boolean canShoot = false;
 
   /** Creates a new AutonShoot. */
   public AutonShoot(
@@ -43,14 +48,14 @@ public AutonShoot(
     // Add your commands in the addCommands() call, e.g.
     // addCommands(new FooCommand(), new BarCommand());
     addCommands(
-      // new FeedActuate(intake, FeedMode.INTAKE).onlyIf(() -> _preloadShot).withTimeout(1),
-
-      // Parallel command group that aims, revs, and squeezes note. ONLY APPLIES TO PRELOADED NOTE.
       new ParallelCommandGroup(
-        new SpinShooter(shooter, ShooterState.SHOOT, true).alongWith(new FeedActuate(intake, FeedMode.INTAKE).withTimeout(1)).onlyIf(
-          () -> !_preloadShot
-        ).andThen(() -> _preloadShot = true),
-        new AutoAim(swerve, shooter, elevator, leds)//.withTimeout(3)
+        // This command will squeeze the note while revving up the shooter. It will only run if the note can't be shot right away. 
+        new ParallelCommandGroup(
+          new SpinShooter(shooter, ShooterState.SHOOT, true).andThen(new WaitCommand(2)),
+          new FeedActuate(intake, FeedMode.INTAKE).withTimeout(1)
+        ).onlyIf(() -> !canShoot),
+
+        new AutoAim(swerve, shooter, elevator, leds)
       ),
 
       new FeedActuate(intake, FeedMode.OUTTAKE).withTimeout(1),

src/main/java/frc/robot/commands/intake/FeedActuate.java

@@ -2,6 +2,7 @@
 package frc.robot.commands.intake;
 
 import edu.wpi.first.wpilibj2.command.Command;
+import frc.robot.commands.auto.AutonShoot;
 import frc.robot.subsystems.IntakeSubsystem;
 import frc.robot.subsystems.IntakeSubsystem.ActuatorState;
 import frc.robot.subsystems.IntakeSubsystem.FeedMode;
@@ -12,16 +13,25 @@ public class FeedActuate extends Command {
   private final ActuatorState _actuatorState;
   private final FeedMode _feedMode;
 
+  private final boolean _runOnce;
+
   /** Creates a new FeedActuate. */
-  public FeedActuate(IntakeSubsystem intake, ActuatorState actuatorState, FeedMode feedMode) {
+  public FeedActuate(IntakeSubsystem intake, ActuatorState actuatorState, FeedMode feedMode, boolean runOnce) {
     _intake = intake;
 
     _actuatorState = actuatorState;
     _feedMode = feedMode;
 
+    _runOnce = runOnce;
+
     addRequirements(_intake);
   }
 
+  /** Default FeedActuate that runs forever. */
+  public FeedActuate(IntakeSubsystem intake, ActuatorState actuatorState, FeedMode feedMode) {
+    this(intake, actuatorState, feedMode, false);
+  }
+
   /** FeedActuate to only control actuator state. */
   public FeedActuate(IntakeSubsystem intake, ActuatorState actuatorState) {
     this(intake, actuatorState, FeedMode.NONE);
@@ -32,25 +42,21 @@ public FeedActuate(IntakeSubsystem intake, FeedMode feedMode) {
     this(intake, ActuatorState.NONE, feedMode);
   }
 
-  public FeedActuate(IntakeSubsystem intake) {
-    this(intake, FeedMode.NONE);
-  }
-
   // Called when the command is initially scheduled.
   @Override
   public void initialize() {
     _intake.feed(_feedMode);
     _intake.actuate(_actuatorState);
 
-    if (_feedMode == FeedMode.OUTTAKE) _intake.resetHasNote();
+    // if not squished (and revved), can't shoot, else can 
+    if (_runOnce) AutonShoot.canShoot = false;
+    else AutonShoot.canShoot = true;
   }
 
   // Called every time the scheduler runs while the command is scheduled.
   @Override
   public void execute() {
     _intake.actuate(_actuatorState);
-
-    // if (_feedMode == FeedMode.INTAKE && _intake.hasNote()) _intake.feed(FeedMode.NONE);
   }
 
   // Called once the command ends or is interrupted.
@@ -63,6 +69,6 @@ public void end(boolean interrupted) {
   // Returns true when the command should end.
   @Override
   public boolean isFinished() {
-    return false;
+    return _runOnce && _intake.atDesiredActuatorState();
   }
 }

src/main/java/frc/robot/subsystems/IntakeSubsystem.java

@@ -2,22 +2,15 @@
 package frc.robot.subsystems;
 
 import com.revrobotics.CANSparkLowLevel.MotorType;
-import com.revrobotics.AbsoluteEncoder;
 import com.revrobotics.CANSparkMax;
 import com.revrobotics.RelativeEncoder;
-import com.revrobotics.SparkAbsoluteEncoder;
-import com.revrobotics.SparkMaxAlternateEncoder;
 import com.revrobotics.CANSparkBase.SoftLimitDirection;
 
 import edu.wpi.first.math.MathUtil;
 import edu.wpi.first.math.controller.PIDController;
-import edu.wpi.first.math.controller.ProfiledPIDController;
 import edu.wpi.first.math.filter.Debouncer;
 import edu.wpi.first.math.filter.Debouncer.DebounceType;
-import edu.wpi.first.math.trajectory.TrapezoidProfile;
-import edu.wpi.first.wpilibj.Encoder;
 import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
-import edu.wpi.first.wpilibj2.command.ProfiledPIDCommand;
 import edu.wpi.first.wpilibj2.command.SubsystemBase;
 import frc.robot.Constants;
 import frc.robot.Constants.PID;
@@ -30,17 +23,8 @@
 public class IntakeSubsystem extends SubsystemBase {
   private final CANSparkMax _feedMotor, _actuatorMotor;
   private final PIDController _actuatorController = new PIDController(PID.INTAKE_ACTUATE_KP, 0, 0);
-  // private final ProfiledPIDController _actuatorController = new ProfiledPIDController(
-  //   0.05,
-  //   0,
-  //   0,
-  //   new TrapezoidProfile.Constraints(22, 50)
-  // );
 
   private final RelativeEncoder _actuatorEncoder;
-  private final RelativeEncoder _feedEncoder;
-
-  private final Debouncer _feedDebouncer = new Debouncer(0.3, DebounceType.kRising);
 
   /** How to feed (in or out). */
   public enum FeedMode {
@@ -52,7 +36,6 @@ public enum ActuatorState {
     STOWED, OUT, NONE
   }
 
-  private boolean _hasNote = false;
 
   /** Creates a new IntakeSubsystem. */
   public IntakeSubsystem() {
@@ -62,8 +45,6 @@ public IntakeSubsystem() {
     _actuatorEncoder = _actuatorMotor.getEncoder();
     _actuatorEncoder.setPosition(0);
 
-    _feedEncoder = _feedMotor.getEncoder();
-
     _actuatorController.setTolerance(0.5);
 
     NeoConfig.configureNeo(_feedMotor, false);
@@ -92,20 +73,6 @@ public double getActuator() {
     return _actuatorEncoder.getPosition();
   }
 
-  /**
-   * Resets the feed's knowledge of the note.
-   */
-  public void resetHasNote() {
-    _hasNote = false;
-  }
-
-  /**
-   * Boolean for whether the intake is has the note.
-   */
-  public boolean hasNote() {
-    return _hasNote;
-  }
-
   /**
    * Actuate the intake at a given speed. Speed gets clamped.
    */
@@ -142,17 +109,6 @@ public void actuate(ActuatorState actuatorState) {
     }
   }
 
-  // public void setAngle(double angle){
-  //   double out = 0;
-
-  //    out = MathUtil.clamp(
-  //         _actuatorController.calculate(getActuator(), angle),
-  //         -Constants.Speeds.INTAKE_ACTUATE_MAX_SPEED,
-  //         Constants.Speeds.INTAKE_ACTUATE_MAX_SPEED);
-
-  //   _actuatorMotor.set(out);
-  // }
-
   /**
    * Feed in/out of the intake.
    *
@@ -180,13 +136,9 @@ public void feed(FeedMode feedMode) {
 
   @Override
   public void periodic() {
-    boolean stalling = _feedDebouncer.calculate(_feedMotor.getOutputCurrent() > 50);
-    _hasNote = stalling ? true : _hasNote;
-
     // This method will be called once per scheduler run
     SmartDashboard.putNumber("ACTUATOR ENCODER", getActuator());
     SmartDashboard.putNumber("ACTUATOR PERCENT OUTPUT", _actuatorMotor.get());
     SmartDashboard.putNumber("FEED CURRENT OUTPUT", _feedMotor.getOutputCurrent());
-    SmartDashboard.putBoolean("HAS NOTE", hasNote());
   }
 }

src/main/java/frc/robot/subsystems/SwerveDriveSubsystem.java

@@ -66,10 +66,10 @@ public class SwerveDriveSubsystem extends SubsystemBase {
       new SwerveModuleState(_backRight.getDriveVelocity(), Rotation2d.fromDegrees(_frontLeft.getAngle())),
       new SwerveModuleState(_backLeft.getDriveVelocity(), Rotation2d.fromDegrees(_frontLeft.getAngle()))};
 
-  private final BNO055 _gyro = BNO055.getInstance(BNO055.opmode_t.OPERATION_MODE_IMUPLUS,
-      BNO055.vector_type_t.VECTOR_EULER);
+  // private final BNO055 _gyro = BNO055.getInstance(BNO055.opmode_t.OPERATION_MODE_IMUPLUS,
+  //     BNO055.vector_type_t.VECTOR_EULER);
 
-  // private final AHRS _gyro = new AHRS();
+  private final AHRS _gyro = new AHRS();
 
   private VisionSubsystem _visionSubsystem;
 
@@ -319,8 +319,8 @@ public Rotation2d getHeading() {
 
   /** Get heading DIRECTLY from the BNO055 gyro as a Rotation2d. */
   public Rotation2d getHeadingRaw() {
-    return Rotation2d.fromDegrees(-Math.IEEEremainder(_gyro.getHeading(), 360));
-    // return Rotation2d.fromDegrees(-Math.IEEEremainder(_gyro.getAngle(), 360));
+    // return Rotation2d.fromDegrees(-Math.IEEEremainder(_gyro.getHeading(), 360));
+    return Rotation2d.fromDegrees(-Math.IEEEremainder(_gyro.getAngle(), 360));
   }
 
   /**

src/main/java/frc/robot/subsystems/VisionSubsystem.java

@@ -1,6 +1,8 @@
 /* Copyright (C) 2024 Team 334. All Rights Reserved.*/
 package frc.robot.subsystems;
 
+import java.util.Collections;
+import java.util.Comparator;
 import java.util.Optional;
 
 import javax.swing.text.html.Option;
@@ -183,8 +185,38 @@ public double[] tagAngleOffsets(int ID) {
   }
 
   public double[] getNoteAngles() {
-    JsonNode target = _intake.getNeuralTarget();
-    double[] angles = {target.get("tx").asDouble(), target.get("ty").asDouble()};
+    ArrayNode targets = _intake.getNeuralTargets();
+
+    // Collections.sort(
+    //   targets, new Comparator() {
+    //     @Override
+    //     public int compare(JsonNode noteA, JsonNode noteB) {
+    //       double noteA_area = noteA.get("ta").asDouble();
+    //       double noteB_area = noteB.get("ta").asDouble();
+
+    //       if (noteA_area > noteB_area) {
+    //         return 1;
+    //       }
+
+    //       if (noteB_area > noteA_area) {
+    //         return -1;
+    //       }
+
+    //       else {
+    //         return 0;
+    //       }
+    //     }
+    //   }
+    // );
+
+    // return {0, 0};
+
+    JsonNode target = targets.get(0);
+
+    double[] angles = {
+      target.get("tx").asDouble(),
+      target.get("ty").asDouble()
+    };
 
     return angles;
   }

src/main/java/frc/robot/utils/helpers/LimelightHelper.java

@@ -41,13 +41,13 @@ public JsonNode getJson() {
   }
 
   /** 
-   * Returns the neural target from the limelight.
+   * Returns the neural targets from the limelight.
    * 
    */
-  public JsonNode getNeuralTarget() {
+  public ArrayNode getNeuralTargets() {
     ArrayNode targets = (ArrayNode) getJson().get("Results").get("Detector");
 
-    return targets.get(0);
+    return targets;
   }
 
   /**