Merge branch 'main' into main

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

@@ -9,7 +9,6 @@
 import edu.wpi.first.wpilibj2.command.InstantCommand;
 import edu.wpi.first.wpilibj2.command.RunCommand;
 import edu.wpi.first.wpilibj2.command.button.JoystickButton;
-
 import frc.robot.subsystems.*;
 import frc.robot.commands.*;
 import frc.robot.constants.*;
@@ -28,6 +27,11 @@ public class RobotContainer {
   public XboxController secondaryController = new XboxController(1);
   public SwerveSubsystem swerveSubsystem = new SwerveSubsystem();
 
+  public IndexerSubsystem indexerSubsystem = new IndexerSubsystem();
+
+  public IntakeSubsystem intakeSubsystem = new IntakeSubsystem();
+
+  public ShooterSubsystem shooterSubsystem = new ShooterSubsystem();
 
 
   /** The container for the robot. Contains subsystems, OI devices, and commands. */
@@ -47,26 +51,37 @@ public void configureButtonBindings(){
             true
     ));
 
+    indexerSubsystem.setDefaultCommand(new BaseCommand(indexerSubsystem));
+
+    // Primary controller
     new JoystickButton(primaryController, XboxController.Button.kY.value).whileTrue(
             new RunCommand(() -> swerveSubsystem.zeroGyro())
     );
 
+    // Secondary controller
+    new JoystickButton(secondaryController, XboxController.Axis.kRightTrigger.value).whileTrue(
+            new SpinUpCommand(shooterSubsystem)
+    );
 
-    new JoystickButton(primaryController, XboxController.Button.kA.value).whileTrue(
-            new PathCommand(swerveSubsystem)
+    new JoystickButton(secondaryController, XboxController.Button.kRightBumper.value).whileTrue(
+            new SpeakerCommand(indexerSubsystem)
     );
 
-    new JoystickButton(primaryController, XboxController.Button.kB.value).whileTrue(
-            new RotateToNoteAndDriveCommand(
-                    swerveSubsystem,
-                    primaryController,
-                    () -> primaryController.getLeftY() * DrivetrainConstants.drivingSpeedScalar / 2.0,
-                    () -> primaryController.getLeftX() * DrivetrainConstants.drivingSpeedScalar / 2.0,
-                    () -> primaryController.getRightX() * DrivetrainConstants.rotationSpeedScalar / 2.0
-            )
+    new JoystickButton(secondaryController, XboxController.Button.kLeftBumper.value).whileTrue(
+            new AmpCommand(indexerSubsystem, secondaryController)
     );
 
+    new JoystickButton(secondaryController, XboxController.Button.kB.value).whileTrue(
+            new AmpCommand2(indexerSubsystem, secondaryController) // This is kinda fucked
+    );
+
+    new JoystickButton(secondaryController, XboxController.Axis.kLeftTrigger.value).whileTrue(
+            new IntakeCommand(intakeSubsystem, indexerSubsystem)
+    );
 
+    new JoystickButton(secondaryController, XboxController.Button.kA.value).whileTrue(
+            new PathCommand(swerveSubsystem)
+    );
 
   }
   /**

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

@@ -0,0 +1,50 @@
+package frc.robot.commands;
+
+import edu.wpi.first.wpilibj.XboxController;
+import edu.wpi.first.wpilibj2.command.Command;
+import frc.robot.subsystems.IndexerSubsystem;
+
+public class AmpCommand extends Command {
+
+    private final IndexerSubsystem indexerSubsystem;
+
+    private final XboxController xboxController;
+
+    private boolean reversed = false;
+
+
+    /**
+     * Moves the indexer to the amp
+     * @param indexerSubsystem The instance of {@link IndexerSubsystem}
+     * @param controller An instance of {@link XboxController}
+     */
+    public AmpCommand(IndexerSubsystem indexerSubsystem, XboxController controller) {
+        this.indexerSubsystem = indexerSubsystem;
+        this.xboxController = controller;
+        addRequirements(indexerSubsystem);
+    }
+
+    @Override
+    public void execute() {
+        if (indexerSubsystem.isCenter() && !reversed) {
+            indexerSubsystem.rotateAllWheelsPercent(0.5);
+        } else {
+            reversed = true;
+        }
+
+        if (reversed && !indexerSubsystem.isTop()) {
+            indexerSubsystem.rotateAllWheelsPercent(-0.5);
+        } else if (reversed && indexerSubsystem.isTop()) {
+            indexerSubsystem.moveIndexerToPos(160);
+        }
+
+        if (xboxController.getRightBumper() && indexerSubsystem.isIndexerRotated()) {
+            indexerSubsystem.rotateAllWheelsPercent(-0.5);
+        }
+    }
+
+    @Override
+    public void end(boolean interrupted) {
+        indexerSubsystem.rotateAllWheelsPercent(0);
+    }
+}

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

@@ -0,0 +1,43 @@
+package frc.robot.commands;
+
+import edu.wpi.first.wpilibj.XboxController;
+import edu.wpi.first.wpilibj2.command.Command;
+import frc.robot.subsystems.IndexerSubsystem;
+
+public class AmpCommand2 extends Command {
+    private final IndexerSubsystem indexerSubsystem;
+
+    private final XboxController xboxController;
+
+    /**
+     * Second Amp command to go straight to amp position from intake, we cannot change to speaker mode if we use this command
+     * @param indexerSubsystem Instance of {@link IndexerSubsystem}
+     */
+    public AmpCommand2(IndexerSubsystem indexerSubsystem, XboxController xboxController) {
+        this.indexerSubsystem = indexerSubsystem;
+        this.xboxController = xboxController;
+
+        addRequirements(indexerSubsystem);
+    }
+
+    @Override
+    public void execute() {
+       if (!indexerSubsystem.isTop()) {
+           indexerSubsystem.rotateMotorPercent(IndexerSubsystem.IndexerMotors.WHEEL_1, 0.4);
+           indexerSubsystem.rotateMotorPercent(IndexerSubsystem.IndexerMotors.WHEEL_2, -0.4);
+       } else {
+           indexerSubsystem.moveIndexerToPos(160);
+
+       }
+        if (xboxController.getRightBumper() && indexerSubsystem.isIndexerRotated()) {
+            indexerSubsystem.rotateAllWheelsPercent(-0.5);
+        }
+    }
+
+    @Override
+    public void end(boolean interrupted) {
+        indexerSubsystem.rotateAllWheelsPercent(0);
+    }
+
+
+}

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

@@ -0,0 +1,30 @@
+package frc.robot.commands;
+
+import edu.wpi.first.wpilibj2.command.Command;
+import frc.robot.subsystems.IndexerSubsystem;
+
+public class BaseCommand extends Command {
+    private final IndexerSubsystem indexerSubsystem;
+
+    /**
+     * Reset indexer to default pos
+     * @param indexerSubsystem Instance of {@link IndexerSubsystem}
+     */
+    public BaseCommand(IndexerSubsystem indexerSubsystem) {
+        this.indexerSubsystem = indexerSubsystem;
+    }
+
+    @Override
+    public void execute() {
+        if (indexerSubsystem.getIndexerAngle() > Math.PI/6) {
+            indexerSubsystem.moveIndexerToPos(10);
+        } else {
+            indexerSubsystem.rotateMotorVolts(IndexerSubsystem.IndexerMotors.INDEXER_ROTATE, 0.0);
+        }
+    }
+
+    @Override
+    public void end(boolean interrupted) {
+        indexerSubsystem.rotateMotorVolts(IndexerSubsystem.IndexerMotors.INDEXER_ROTATE, 0.0);
+    }
+}

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

@@ -0,0 +1,40 @@
+package frc.robot.commands;
+
+import edu.wpi.first.wpilibj2.command.Command;
+import frc.robot.subsystems.IndexerSubsystem;
+import frc.robot.subsystems.IntakeSubsystem;
+
+public class IntakeCommand extends Command {
+    private final IntakeSubsystem intakeSubsystem;
+
+    private final IndexerSubsystem indexerSubsystem;
+
+    /**
+     * Command to run the intake
+     * @param intakeSubsystem The instance of {@link IntakeSubsystem}
+     * @param indexerSubsystem The instance of {@link IndexerSubsystem} (needed for limebreak detection to stop intake motor)
+     */
+    public IntakeCommand(IntakeSubsystem intakeSubsystem, IndexerSubsystem indexerSubsystem) {
+        this.intakeSubsystem = intakeSubsystem;
+        this.indexerSubsystem = indexerSubsystem;
+
+        addRequirements(intakeSubsystem, indexerSubsystem);
+    }
+
+    @Override
+    public void execute() {
+        if (!indexerSubsystem.isCenter()) {
+            intakeSubsystem.setSpeed(.9);
+            indexerSubsystem.rotateAllWheelsPercent(.9);
+        } else {
+            intakeSubsystem.setSpeed(0);
+            indexerSubsystem.rotateAllWheelsPercent(0);
+        }
+    }
+
+    @Override
+    public void end(boolean interrupted) {
+        intakeSubsystem.setSpeed(0);
+        indexerSubsystem.rotateAllWheelsPercent(0);
+    }
+}

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

@@ -5,15 +5,19 @@
 import frc.robot.subsystems.LEDSubsystem;
 
 public class SetLEDCommand extends Command {
-    private static LEDSubsystem subsystem;
-    public SetLEDCommand(LEDSubsystem subsystem) {
+    private final LEDSubsystem subsystem;
+
+    private final LEDConstants.Status status;
+    public SetLEDCommand(LEDSubsystem subsystem, LEDConstants.Status status) {
+        this.status = status;
+
         this.subsystem = subsystem;
         addRequirements(subsystem);
     }
 
-    //@Override
-    public static void run(LEDConstants.Status currentStatus) {
-        switch(currentStatus) {
+    @Override
+    public void execute() {
+        switch(status) {
             case TEST_1:
                 //Confetti
                 subsystem.setLED(-0.87);
@@ -22,4 +26,5 @@ public static void run(LEDConstants.Status currentStatus) {
                 subsystem.setLED(-0.71);
         }
     }
+
 }

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

@@ -0,0 +1,30 @@
+package frc.robot.commands;
+
+import edu.wpi.first.wpilibj2.command.Command;
+import frc.robot.subsystems.IndexerSubsystem;
+
+public class SpeakerCommand extends Command {
+
+    private final IndexerSubsystem indexerSubsystem;
+
+    /**
+     * Command to shoot for the speaker once there is a note in the intake
+     * @param indexerSubsystem Instance of {@link IndexerSubsystem}
+     */
+
+    public SpeakerCommand(IndexerSubsystem indexerSubsystem) {
+        this.indexerSubsystem = indexerSubsystem;
+
+        addRequirements(indexerSubsystem);
+    }
+
+    @Override
+    public void execute() {
+        indexerSubsystem.rotateAllWheelsPercent(1);
+    }
+
+    @Override
+    public void end(boolean interrupted) {
+        indexerSubsystem.rotateAllWheelsPercent(0);
+    }
+}

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

@@ -0,0 +1,28 @@
+package frc.robot.commands;
+
+import edu.wpi.first.wpilibj2.command.Command;
+import frc.robot.subsystems.ShooterSubsystem;
+
+public class SpinUpCommand extends Command {
+    private final ShooterSubsystem shooterSubsystem;
+
+    /**
+     * Command to spin up the shooter
+     * @param shooterSubsystem Instance of {@link ShooterSubsystem}
+     */
+    public SpinUpCommand(ShooterSubsystem shooterSubsystem) {
+        this.shooterSubsystem = shooterSubsystem;
+
+        addRequirements(shooterSubsystem);
+    }
+
+    @Override
+    public void execute() {
+        shooterSubsystem.setSpeed(6000);
+    }
+
+    @Override
+    public void end(boolean interrupted) {
+        shooterSubsystem.setSpeed(0);
+    }
+}

src/main/java/frc/robot/constants/IndexerConstants.java

@@ -0,0 +1,24 @@
+package frc.robot.constants;
+
+import edu.wpi.first.math.controller.ArmFeedforward;
+import edu.wpi.first.math.trajectory.TrapezoidProfile;
+
+public class IndexerConstants {
+    public final static int wheel1ID = 11;
+    public final static int wheel2ID = 12;
+    public final static int wheel3ID = 13;
+    public final static int indexerRotateID = 14;
+
+    public final static int centerLimebreakID = 10;
+    public final static int topLimebreakID = 9;
+
+    public final static double indexerP = 3.0;
+    public final static double indexerI = 0.0;
+    public final static double indexerD = 0.0;
+
+    public final static TrapezoidProfile.Constraints indexerTrapezoidProfile =
+            new TrapezoidProfile.Constraints(Math.PI * 3, Math.PI * 5);
+
+    public final static ArmFeedforward indexerFF = new ArmFeedforward(0.13, 0.5, 0.00);
+
+}

src/main/java/frc/robot/constants/ShooterConstants.java

@@ -0,0 +1,15 @@
+package frc.robot.constants;
+
+import edu.wpi.first.math.controller.ArmFeedforward;
+import edu.wpi.first.math.controller.SimpleMotorFeedforward;
+
+public class ShooterConstants {
+    public static final double shooterP = 0.02;
+    public static final double shooterI = 0.01;
+    public static final double shooterD = 0.0;
+
+    public static final SimpleMotorFeedforward shooterFF = new SimpleMotorFeedforward(0.05, 0.0179, 0.0008);
+
+
+
+}