Added IR Break Beam and autoIntake with Ada, and also synced stuff in Swerve

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

@@ -25,8 +25,6 @@
 import frc.robot.Constants.OperatorConstants;
 import frc.robot.Constants.SwerveConstants;
 import frc.robot.commands.TeleopSwerve;
-import frc.robot.commands.arm.ArmHold;
-import frc.robot.commands.arm.AutoShoot;
 import frc.robot.subsystems.Arm;
 import frc.robot.subsystems.Intake;
 import frc.robot.subsystems.Swerve;
@@ -74,27 +72,15 @@ public RobotContainer() {
     configureBatteryChooser();
     configurePrematchChecklist();
 
-    NamedCommands.registerCommand("Start Intake", Commands.run(intake::intakeNote, intake));
-    NamedCommands.registerCommand("Stop Intake", intake.runOnce(intake::stopIntakeMotor));
-
-    NamedCommands.registerCommand(
-        "Arm to Pickup", arm.moveToPosition(ArmConstants.pickupAngle).withTimeout(3.0));
-    NamedCommands.registerCommand(
-        "Arm to Subwoofer", arm.moveToPosition(ArmConstants.subwooferAngle).withTimeout(3.0));
-    NamedCommands.registerCommand(
-        "Arm to Source Podium",
-        arm.moveToPosition(ArmConstants.autoSourcePodiumAngle).withTimeout(3.0));
-    NamedCommands.registerCommand(
-        "Arm to Amp Podium", arm.moveToPosition(ArmConstants.autoAmpPodiumAngle).withTimeout(3.0));
+    NamedCommands.registerCommand("Note Intake", Commands.run(intake::autoIntake, intake));
+    // NamedCommands.registerCommand("Stop Intake", Commands.run(intake::stopIntakeMotor, intake));
 
     SmartDashboard.putData("Command Scheduler", CommandScheduler.getInstance());
     SmartDashboard.putData("Power Distribution Panel", powerDistribution);
 
     LogUtil.recordMetadata("Battery Number", batteryChooser.getSelectedName());
     LogUtil.recordMetadata("Battery Nickname", batteryChooser.getSelected());
 
-    arm.setDefaultCommand(new ArmHold(arm));
-
     swerve.setDefaultCommand(
         new TeleopSwerve(
             driverController::getLeftY,
@@ -180,31 +166,19 @@ private void configureIntakeBindings() {
   private void configureArmBindings() {
     operatorStick
         .button(OperatorConstants.armToPickup)
-        .whileTrue(arm.moveToPosition(ArmConstants.pickupAngle));
+        .whileTrue(arm.moveToPosition(ArmConstants.pickupHeight));
 
     operatorStick
         .button(OperatorConstants.armToAmp)
-        .whileTrue(arm.moveToPosition(ArmConstants.ampAngle));
+        .whileTrue(arm.moveToPosition(ArmConstants.ampHeight));
 
     operatorStick
         .button(OperatorConstants.armShootSubwoofer)
-        .whileTrue(arm.moveToPosition(ArmConstants.subwooferAngle));
+        .whileTrue(arm.moveToPosition(ArmConstants.subwooferHeight));
 
     operatorStick
         .button(OperatorConstants.armShootPodium)
-        .whileTrue(arm.moveToPosition(ArmConstants.podiumAngle));
-
-    operatorStick
-        .button(OperatorConstants.armManualUp)
-        .whileTrue(arm.run(() -> arm.setSpeed(ArmConstants.manualSpeed)))
-        .onFalse(arm.runOnce(() -> arm.setSpeed(0.0)));
-
-    operatorStick
-        .button(OperatorConstants.armManualDown)
-        .whileTrue(arm.run(() -> arm.setSpeed(-ArmConstants.manualSpeed)))
-        .onFalse(arm.runOnce(() -> arm.setSpeed(0.0)));
-
-    operatorStick.button(OperatorConstants.armAutoShoot).whileTrue(new AutoShoot(arm, swerve));
+        .whileTrue(arm.moveToPosition(ArmConstants.podiumHeight));
   }
 
   private void configureAutoChooser() {

src/main/java/frc/robot/commands/PathFindToPose.java

@@ -0,0 +1,67 @@
+// 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.commands;
+
+
+import com.pathplanner.lib.auto.AutoBuilder;
+import com.pathplanner.lib.path.PathConstraints;
+
+import edu.wpi.first.math.geometry.Pose2d;
+import edu.wpi.first.math.geometry.Rotation2d;
+import edu.wpi.first.math.util.Units;
+import edu.wpi.first.wpilibj2.command.Command;
+import frc.robot.Constants.SwerveConstants;
+import frc.robot.subsystems.Swerve;
+
+public class PathFindToPose extends Command {
+  Swerve swerve;
+  double xCoordinate;
+  double yCoordinate;
+  double rotationSupplier;
+
+  /** Creates a new VisionCommand. */
+  public PathFindToPose(Swerve swerve, double xCoordinate, 
+  double yCoordinate, double rotationSupplier) {
+    // Use addRequirements() here to declare subsystem dependencies.
+    this.xCoordinate = xCoordinate;
+    this.yCoordinate = yCoordinate;
+    this.rotationSupplier = rotationSupplier;
+    this.swerve = swerve;
+
+    addRequirements(swerve);
+  }
+
+  // Called when the command is initially scheduled.
+  @Override
+  public void initialize() {}
+
+  // Called every time the scheduler runs while the command is scheduled.
+  @Override
+  public void execute() {
+
+    Pose2d targetPose = new Pose2d(xCoordinate, yCoordinate, Rotation2d.fromDegrees(rotationSupplier));
+
+    PathConstraints constraints = new PathConstraints(
+        SwerveConstants.maxTranslationalSpeed, SwerveConstants.maxTranslationalAcceleration,
+        Units.degreesToRadians(540), Units.degreesToRadians(720));
+
+    Command pathfindingCommand = AutoBuilder.pathfindToPose(
+          targetPose,
+          constraints,
+          0.0, 
+          0.0 
+    );
+  }
+
+  // Called once the command ends or is interrupted.
+  @Override
+  public void end(boolean interrupted) {}
+
+  // Returns true when the command should end.
+  @Override
+  public boolean isFinished() {
+    return false;
+  }
+}

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

@@ -6,13 +6,19 @@
 
 import com.revrobotics.CANSparkLowLevel.MotorType;
 import com.revrobotics.CANSparkMax;
+
+import edu.wpi.first.wpilibj.DigitalInput;
+import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
+import edu.wpi.first.wpilibj2.command.Command;
 import edu.wpi.first.wpilibj2.command.SubsystemBase;
+import edu.wpi.first.wpilibj2.command.sysid.SysIdRoutine.Direction;
 import frc.robot.Constants.IntakeConstants;
 
 public class Intake extends SubsystemBase {
   /** Creates a new Intake. */
   CANSparkMax intakeMotor1 =
       new CANSparkMax(IntakeConstants.intakeMotorOneID, MotorType.kBrushless);
+  DigitalInput irBreakBeam = new DigitalInput(IntakeConstants.intakeMotorOneID);
 
   public Intake() {}
 
@@ -28,8 +34,16 @@ public void stopIntakeMotor() {
     intakeMotor1.set(0.0);
   }
 
+  public void autoIntake() {
+    intakeNote();
+    if (!irBreakBeam.get()) {
+      stopIntakeMotor();
+    }
+  }
+
   @Override
   public void periodic() {
     // This method will be called once per scheduler run
+    SmartDashboard.putBoolean("Intake Note?", !irBreakBeam.get());
   }
 }