Merge branch 'main' of github.com:FairportRobotics/Castor2026

src/main/java/frc/robot/Constants.java

@@ -9,6 +9,7 @@ import edu.wpi.first.apriltag.AprilTagFieldLayout;
 import edu.wpi.first.apriltag.AprilTagFields;
 import edu.wpi.first.math.geometry.Pose3d;
 import edu.wpi.first.math.geometry.Rotation2d;
+import edu.wpi.first.math.geometry.Rotation3d;
 import edu.wpi.first.math.geometry.Transform2d;
 import edu.wpi.first.math.geometry.Transform3d;
 import edu.wpi.first.units.measure.Angle;
@@ -170,5 +171,50 @@ public final class Constants {
         public static Pose3d BLUE_HUB_POSE = fieldTags.getTagPose(26).get().transformBy(new Transform3d(new Transform2d(-0.5207, 0, Rotation2d.kZero)));
         public static Pose3d RED_HUB_POSE = fieldTags.getTagPose(10).get().transformBy(new Transform3d(new Transform2d(-0.5207, 0, Rotation2d.kZero)));
 
+        // These are the field targets for passing fuel to the Alliance Zone or to the Neutral Zone from the opponent's side.
+        // Left/Right are from driver's perspective. Coordinates assume origin at blue side of field.
+        // Rotation are zero because they don't matter.
+        public static Pose3d BLUE_LEFT_ALLIANCE_ZONE_PASS_TARGET_POSE = new Pose3d(1.99, 6.35, 0, new Rotation3d());
+        public static Pose3d BLUE_RIGHT_ALLIANCE_ZONE_PASS_TARGET_POSE = new Pose3d(1.99, 6.35, 0, new Rotation3d());
+        public static Pose3d BLUE_LEFT_NEUTRAL_ZONE_PASS_TARGET_POSE = new Pose3d(8.27, 6.35, 0, new Rotation3d());
+        public static Pose3d BLUE_RIGHT_NEUTRAL_ZONE_PASS_TARGET_POSE = new Pose3d(8.27, 1.72, 0, new Rotation3d());
+
+        public static Pose3d RED_LEFT_ALLIANCE_ZONE_PASS_TARGET_POSE = new Pose3d(14.55, 1.72, 0, new Rotation3d());
+        public static Pose3d RED_RIGHT_ALLIANCE_ZONE_PASS_TARGET_POSE = new Pose3d(14.55, 6.35, 0, new Rotation3d());
+        public static Pose3d RED_LEFT_NEUTRAL_ZONE_PASS_TARGET_POSE = new Pose3d(8.27, 1.72, 0, new Rotation3d());
+        public static Pose3d RED_RIGHT_NEUTRAL_ZONE_PASS_TARGET_POSE = new Pose3d(8.27, 6.35, 0, new Rotation3d());
+
+
+    }
+
+    // These represent the dividing lines for the regions in our shooting strategy.
+    public static class ShootingRegionDimensions {
+
+        public static double BLUE_ALLIANCE_ZONE_REGION_X_MAX = 3.5;
+        public static double RED_ALLIANCE_ZONE_REGION_X_MIN = 13.0;
+        public static double NEUTRAL_ZONE_REGION_X_MIN = 5.5;
+        public static double NEUTRAL_ZONE_REGION_X_MAX = 11.0;
+        public static double BLUE_RIGHT_REGION_Y_MAX = 3.0;
+        public static double RED_LEFT_REGION_Y_MAX = 3.0;               // same meaning as the previous one, but from red perspective
+        public static double BLUE_LEFT_REGION_Y_MIN = 5.0;
+        public static double RED_RIGHT_REGION_Y_MIN = 5.0;              // same meaning as the previous one, but from red perspective
+
+    }
+
+    // Regions that define the AutoTurret shooting behaviors.
+    // Color ID in the name reflects robot's alliance since region definitions vary by alliance.
+    // Left/Right are from driver's perspective.
+    public enum ShootingRegion {
+        NON_SHOOTING_REGION,                // Any part of the field that's not in one of the regions
+        BLUE_OWN_ALLIANCE_ZONE,
+        BLUE_LEFT_NEUTRAL_ZONE,
+        BLUE_RIGHT_NEUTRAL_ZONE,
+        BLUE_LEFT_OPPONENT_ALLIANCE_ZONE,
+        BLUE_RIGHT_OPPONENT_ALLIANCE_ZONE,
+        RED_OWN_ALLIANCE_ZONE,
+        RED_LEFT_NEUTRAL_ZONE,
+        RED_RIGHT_NEUTRAL_ZONE,
+        RED_LEFT_OPPONENT_ALLIANCE_ZONE,
+        RED_RIGHT_OPPONENT_ALLIANCE_ZONE        
     }
 }

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

@@ -13,9 +13,7 @@ import frc.robot.Constants.OperatorConstants;
 import frc.robot.commands.AutoShootCommandChassisTurret;
 import frc.robot.commands.AutoShootCommandChassisTurretPathPlanner;
 import frc.robot.commands.AutoTurretCommand;
-import frc.robot.commands.RelayCommand;
+import frc.robot.commands.Reverse;
-import frc.robot.commands.SetDeflectorCommand;
-import frc.robot.commands.ZoneCheck;
 import frc.robot.subsystems.DriveSubsystem;
 import frc.robot.subsystems.HopperSubsystem;
 import frc.robot.subsystems.IntakeSubsystem;
@@ -73,21 +71,20 @@ public class RobotContainer {
      *
      */
     private void configureBindings() {
-        m_driverController.povDown().onTrue(new SetDeflectorCommand(turretSubsystem, Constants.ShooterConstants.DEFLECTOR_STORED_ANGLE));
+        /*m_driverController.povDown().onTrue(new SetDeflectorCommand(turretSubsystem, Constants.ShooterConstants.DEFLECTOR_STORED_ANGLE));
         m_driverController.povLeft().onTrue(new SetDeflectorCommand(turretSubsystem, Constants.ShooterConstants.DEFLECTOR_SET_ANGLE1));
         m_driverController.povRight().onTrue(new SetDeflectorCommand(turretSubsystem, Constants.ShooterConstants.DEFLECTOR_SET_ANGLE3));
-        m_driverController.povUp().onTrue(new SetDeflectorCommand(turretSubsystem, Constants.ShooterConstants.DEFLECTOR_SET_ANGLE2));
+        m_driverController.povUp().onTrue(new SetDeflectorCommand(turretSubsystem, Constants.ShooterConstants.DEFLECTOR_SET_ANGLE2));*/
 
         m_driverController.back().whileTrue(new AutoShootCommandChassisTurretPathPlanner(driveSubsystem, hopperSubsystem, turretSubsystem));
-        //m_driverController.a().whileTrue(new AutoShootCommandChassisTurretPathPlanner(driveSubsystem, hopperSubsystem, turretSubsystem));
-        // m_driverController:wa.a().whileTrue(new AutoShootCommandChassisTurret(driveSubsystem, hopperSubsystem, turretSubsystem));
 
         m_driverController.leftBumper().onTrue(Commands.runOnce(() -> turretSubsystem.homeTurret(), turretSubsystem));
         m_driverController.rightBumper().onTrue(Commands.parallel(intakeSubsystem.intake()));
-        m_driverController.y().whileTrue(new RelayCommand(hopperSubsystem, turretSubsystem, intakeSubsystem));
+        //m_driverController.y().whileTrue(new RelayCommand(hopperSubsystem, turretSubsystem, intakeSubsystem));
         m_driverController.start().whileTrue(intakeSubsystem.resetDeploy());
         m_driverController.x().whileTrue(new AutoShootCommandChassisTurret(driveSubsystem, hopperSubsystem, turretSubsystem));
-        m_driverController.b().toggleOnTrue(new ZoneCheck(driveSubsystem));
+        m_driverController.b().onTrue(new Reverse(intakeSubsystem, hopperSubsystem));
+        m_driverController.y().onTrue(intakeSubsystem.deploy());
     }
 
     /**

src/main/java/frc/robot/Utils.java

@@ -1,5 +1,11 @@
 package frc.robot;
 
+import edu.wpi.first.wpilibj.DriverStation.Alliance;
+import frc.robot.Constants;
+import frc.robot.Constants.ShootingRegion;
+import frc.robot.Constants.ShootingRegionDimensions;
+import edu.wpi.first.math.geometry.Pose3d;
+
 public class Utils {
     
     /**
@@ -22,4 +28,130 @@ public class Utils {
         return robotTurretRotations;
     }
 
+
+
+    /**
+     * Determines the region for the robot based on its location and alliance.
+     * Assumes the robot is a 27" square. Thresholds based on observations from simulator.
+     * @param robotPose: the robot's pose
+     * @param alliance: the red/blue alliance of the robot, as a sanity check
+     * @return the robot's shooting region
+     */
+    public static ShootingRegion findRobotShootingRegion(Pose3d robotPose, Alliance alliance){
+        ShootingRegion region = Constants.ShootingRegion.NON_SHOOTING_REGION;   // the default return value if robot isn't in a valid region
+        double robotX = robotPose.getX();
+        double robotY = robotPose.getY();
+
+        // Receommended: Add a sanity check for invalid robot poses.
+
+        if (alliance == Alliance.Blue) {
+            if (robotX < Constants.ShootingRegionDimensions.BLUE_ALLIANCE_ZONE_REGION_X_MAX) {
+                region = Constants.ShootingRegion.BLUE_OWN_ALLIANCE_ZONE;
+                }
+            else if (robotX > Constants.ShootingRegionDimensions.NEUTRAL_ZONE_REGION_X_MIN &&
+                     robotX < Constants.ShootingRegionDimensions.NEUTRAL_ZONE_REGION_X_MAX &&
+                     robotY > Constants.ShootingRegionDimensions.BLUE_LEFT_REGION_Y_MIN) {
+                        region = Constants.ShootingRegion.BLUE_LEFT_NEUTRAL_ZONE;
+                     }
+            else if (robotX > Constants.ShootingRegionDimensions.NEUTRAL_ZONE_REGION_X_MIN &&
+                     robotX < Constants.ShootingRegionDimensions.NEUTRAL_ZONE_REGION_X_MAX &&
+                     robotY < Constants.ShootingRegionDimensions.BLUE_RIGHT_REGION_Y_MAX) {
+                        region = Constants.ShootingRegion.BLUE_RIGHT_NEUTRAL_ZONE;                
+                     }
+            else if (robotX > Constants.ShootingRegionDimensions.RED_ALLIANCE_ZONE_REGION_X_MIN &&
+                     robotY > Constants.ShootingRegionDimensions.BLUE_LEFT_REGION_Y_MIN) {
+                        region = Constants.ShootingRegion.BLUE_LEFT_OPPONENT_ALLIANCE_ZONE;
+                     }
+            else if (robotX > Constants.ShootingRegionDimensions.RED_ALLIANCE_ZONE_REGION_X_MIN &&
+                     robotY < Constants.ShootingRegionDimensions.BLUE_RIGHT_REGION_Y_MAX) {
+                        region = Constants.ShootingRegion.BLUE_RIGHT_OPPONENT_ALLIANCE_ZONE;
+                     }
+        }
+        else if (alliance == Alliance.Red) {
+            if (robotX > Constants.ShootingRegionDimensions.RED_ALLIANCE_ZONE_REGION_X_MIN) {
+                region = Constants.ShootingRegion.RED_OWN_ALLIANCE_ZONE;
+                }
+            else if (robotX > Constants.ShootingRegionDimensions.NEUTRAL_ZONE_REGION_X_MIN &&
+                     robotX < Constants.ShootingRegionDimensions.NEUTRAL_ZONE_REGION_X_MAX &&
+                     robotY < Constants.ShootingRegionDimensions.RED_LEFT_REGION_Y_MAX) {
+                        region = Constants.ShootingRegion.RED_LEFT_NEUTRAL_ZONE;
+                     }
+            else if (robotX > Constants.ShootingRegionDimensions.NEUTRAL_ZONE_REGION_X_MIN &&
+                     robotX < Constants.ShootingRegionDimensions.NEUTRAL_ZONE_REGION_X_MAX &&
+                     robotY > Constants.ShootingRegionDimensions.RED_RIGHT_REGION_Y_MIN) {
+                        region = Constants.ShootingRegion.RED_RIGHT_NEUTRAL_ZONE;
+                     }
+            else if (robotX < Constants.ShootingRegionDimensions.BLUE_ALLIANCE_ZONE_REGION_X_MAX &&
+                     robotY < Constants.ShootingRegionDimensions.RED_LEFT_REGION_Y_MAX) {
+                        region = Constants.ShootingRegion.RED_LEFT_OPPONENT_ALLIANCE_ZONE;
+                     }
+            else if (robotX > Constants.ShootingRegionDimensions.RED_ALLIANCE_ZONE_REGION_X_MIN &&
+                     robotY > Constants.ShootingRegionDimensions.RED_RIGHT_REGION_Y_MIN) {
+                        region = Constants.ShootingRegion.RED_RIGHT_OPPONENT_ALLIANCE_ZONE;
+                     }
+        }
+        return region;
+    }
+
+
+
+    /**
+     * Determines the appropriate shooting target for the robot based on its alliance
+     * and its current region on the field. There are 10 possible targets:
+     * Scoring hub, 2x passing from Neutral Zone, 2x passing from opponent's Alliance Zone,
+     * and these exist for both red and blue alliances.
+     * Right/left are from driver's perspective.
+     * @param region: the identifier of the robot's current field region
+     * @param alliance: the red/blue alliance of the robot, as a sanity check
+     * @return the shooting target for a robot with the provided properties. Returns all-zero
+     * Pose3d object if the region/alliance combination is invalid.
+     */
+    public static Pose3d findShootingTarget(ShootingRegion region, Alliance alliance){
+
+        Pose3d shootingTarget = new Pose3d();
+        if (alliance == Alliance.Blue) {
+            switch (region) {
+                case BLUE_OWN_ALLIANCE_ZONE:
+                    shootingTarget = Constants.FieldPoses.BLUE_HUB_POSE;
+                    break;
+                case BLUE_LEFT_NEUTRAL_ZONE:
+                    shootingTarget = Constants.FieldPoses.BLUE_LEFT_ALLIANCE_ZONE_PASS_TARGET_POSE;
+                    break;
+                case BLUE_RIGHT_NEUTRAL_ZONE:
+                    shootingTarget = Constants.FieldPoses.BLUE_RIGHT_ALLIANCE_ZONE_PASS_TARGET_POSE;
+                    break;
+                case BLUE_LEFT_OPPONENT_ALLIANCE_ZONE:
+                    shootingTarget = Constants.FieldPoses.BLUE_LEFT_NEUTRAL_ZONE_PASS_TARGET_POSE;
+                    break;
+                case BLUE_RIGHT_OPPONENT_ALLIANCE_ZONE:
+                    shootingTarget = Constants.FieldPoses.BLUE_RIGHT_NEUTRAL_ZONE_PASS_TARGET_POSE;
+                    break;
+                default:
+                    break;
+            }
+        }
+        else if (alliance == Alliance.Red) {
+            switch (region) {
+                case RED_OWN_ALLIANCE_ZONE:
+                    shootingTarget = Constants.FieldPoses.RED_HUB_POSE;
+                    break;
+                case RED_LEFT_NEUTRAL_ZONE:
+                    shootingTarget = Constants.FieldPoses.RED_LEFT_ALLIANCE_ZONE_PASS_TARGET_POSE;
+                    break;
+                case RED_RIGHT_NEUTRAL_ZONE:
+                    shootingTarget = Constants.FieldPoses.RED_RIGHT_ALLIANCE_ZONE_PASS_TARGET_POSE;
+                    break;
+                case RED_LEFT_OPPONENT_ALLIANCE_ZONE:
+                    shootingTarget = Constants.FieldPoses.RED_LEFT_NEUTRAL_ZONE_PASS_TARGET_POSE;
+                    break;
+                case RED_RIGHT_OPPONENT_ALLIANCE_ZONE:
+                    shootingTarget = Constants.FieldPoses.RED_RIGHT_NEUTRAL_ZONE_PASS_TARGET_POSE;
+                    break;
+                default:
+                    break;
+            }
+        }
+        return shootingTarget;
+    }
+
 }

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

@@ -0,0 +1,54 @@
+// 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 edu.wpi.first.wpilibj2.command.Command;
+import frc.robot.subsystems.HopperSubsystem;
+import frc.robot.subsystems.IntakeSubsystem;
+
+/**
+ * An example command that uses an example subsystem.
+ */
+public class Reverse extends Command {
+
+    private IntakeSubsystem intakeSubsystem;
+    private HopperSubsystem hopperSubsystem;
+
+    /**
+     * Creates a new ExampleCommand.
+     *
+     * @param subsystem The subsystem used by this command.
+     */
+    public Reverse(IntakeSubsystem intakeSubsystem, HopperSubsystem hopperSubsystem) {
+        this.intakeSubsystem = intakeSubsystem;
+        this.hopperSubsystem = hopperSubsystem;
+    }
+
+    // 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() {
+        intakeSubsystem.reverseIntake();
+        hopperSubsystem.spindexerReverse();
+        hopperSubsystem.reverseKicker();
+    }
+
+    // Called once the command ends or is interrupted.
+    @Override
+    public void end(boolean interrupted) {
+        hopperSubsystem.stopKicker();
+        hopperSubsystem.spindexerOff();
+        intakeSubsystem.stopIntake();
+    }
+
+    // Returns true when the command should end.
+    @Override
+    public boolean isFinished() {
+        return false;
+    }
+}

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

@@ -1,94 +1,100 @@
 // 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.subsystems;
 
 import org.fairportrobotics.frc.posty.TestableSubsystem;
+import static org.fairportrobotics.frc.posty.assertions.Assertions.assertThat;
 import org.fairportrobotics.frc.posty.test.PostTest;
 import org.littletonrobotics.junction.Logger;
 
 import com.ctre.phoenix6.StatusSignal;
-import com.ctre.phoenix6.configs.ClosedLoopGeneralConfigs;
 import com.ctre.phoenix6.configs.MotorOutputConfigs;
 import com.ctre.phoenix6.configs.Slot0Configs;
 import com.ctre.phoenix6.configs.TalonFXConfiguration;
 import com.ctre.phoenix6.controls.VelocityVoltage;
 import com.ctre.phoenix6.hardware.TalonFX;
 
-import edu.wpi.first.units.AngularVelocityUnit;
 import edu.wpi.first.units.Units;
 import edu.wpi.first.units.measure.AngularVelocity;
-import edu.wpi.first.wpilibj2.command.Command;
-
-import static org.fairportrobotics.frc.posty.assertions.Assertions.*;
 import frc.robot.Constants;
 
 public class HopperSubsystem extends TestableSubsystem {
 
     private TalonFX spindexerMotor;
 
-
     private TalonFX kickerMotor;
     private VelocityVoltage kickerControl = new VelocityVoltage(0);
     private StatusSignal<AngularVelocity> kickerVelocity;
 
-  /** Creates a new HopperSubsystem. */
+    /**
-  public HopperSubsystem() {
+     * Creates a new HopperSubsystem.
-    spindexerMotor = new TalonFX(Constants.HopperConstants.SPINDEXER_MOTOR_ID);
+     */
-    spindexerMotor.getConfigurator().apply(
+    public HopperSubsystem() {
-        new TalonFXConfiguration()
+        spindexerMotor = new TalonFX(Constants.HopperConstants.SPINDEXER_MOTOR_ID);
-            .withMotorOutput(
+        spindexerMotor.getConfigurator().apply(
-                new MotorOutputConfigs()
+                new TalonFXConfiguration()
-                    .withInverted(Constants.HopperConstants.SPINDEXER_MOTOR_DIRECTION))
+                        .withMotorOutput(
-            .withSlot0(
+                                new MotorOutputConfigs()
-                new Slot0Configs()
+                                        .withInverted(Constants.HopperConstants.SPINDEXER_MOTOR_DIRECTION))
-                    .withKP(0)
+                        .withSlot0(
-                    .withKI(0)
+                                new Slot0Configs()
-                    .withKD(0)
+                                        .withKP(0)
-                    .withKS(0)));
+                                        .withKI(0)
+                                        .withKD(0)
+                                        .withKS(0)));
 
-    kickerMotor = new TalonFX(Constants.HopperConstants.KICKER_MOTOR_ID);
+        kickerMotor = new TalonFX(Constants.HopperConstants.KICKER_MOTOR_ID);
-    kickerMotor.getConfigurator().apply(
+        kickerMotor.getConfigurator().apply(
-        new TalonFXConfiguration()
+                new TalonFXConfiguration()
-            .withMotorOutput(
+                        .withMotorOutput(
-                new MotorOutputConfigs()
+                                new MotorOutputConfigs()
-                    .withInverted(Constants.HopperConstants.KICKER_MOTOR_DIRECTION)
+                                        .withInverted(Constants.HopperConstants.KICKER_MOTOR_DIRECTION)
-            )
+                        )
-            .withSlot0(new Slot0Configs()
+                        .withSlot0(new Slot0Configs()
-              .withKP(0)
+                                .withKP(0)
-              .withKI(0)
+                                .withKI(0)
-              .withKD(0)
+                                .withKD(0)
-              .withKS(0)
+                                .withKS(0)
-            ));
+                        ));
-    kickerVelocity = kickerMotor.getVelocity();
+        kickerVelocity = kickerMotor.getVelocity();
-  } 
+    }
 
-  public void feedKicker() {
+    public void feedKicker() {
-    //kickerMotor.setControl(kickerControl.withVelocity(1000));
+        //kickerMotor.setControl(kickerControl.withVelocity(1000));
-    kickerMotor.set(0.75);
+        kickerMotor.set(0.75);
-  }
+    }
 
-  public void reverseKicker() {
+    public void reverseKicker() {
-    //kickerMotor.setControl(kickerControl.withVelocity(-500));
+        //kickerMotor.setControl(kickerControl.withVelocity(-500));
-    kickerMotor.set(-0.5);
+        kickerMotor.set(-0.5);
-  } 
+    }
 
- public void stopKicker() {kickerMotor.stopMotor();}
+    public void stopKicker() {
+        kickerMotor.stopMotor();
+    }
 
-  public void spindexerOn() { spindexerMotor.set(0.75);}
+    public void spindexerOn() {
+        spindexerMotor.set(0.75);
+    }
 
-  public void spindexerOff() { spindexerMotor.stopMotor();}
+    public void spindexerReverse() {
+        spindexerMotor.set(-0.75);
+    }
 
-  @Override
+    public void spindexerOff() {
-  public void periodic() {
+        spindexerMotor.stopMotor();
-      Logger.recordOutput("HopperSubsystem-KickerSpeed(Rotations/sec)", kickerVelocity.refresh().getValue().in(Units.RotationsPerSecond));
+    }
-  }
 
-  @PostTest(enabled = true)
+    @Override
-  public void hopperSystem_CANDevicesConnected(){
+    public void periodic() {
-    assertThat(kickerMotor.isConnected()).as("Kicker motor not connected!").isTrue();
+        Logger.recordOutput("HopperSubsystem-KickerSpeed(Rotations/sec)", kickerVelocity.refresh().getValue().in(Units.RotationsPerSecond));
-    assertThat(spindexerMotor.isConnected()).as("Spindexer motor not connected!").isTrue();
+    }
-  }
+
+    @PostTest(enabled = true)
+    public void hopperSystem_CANDevicesConnected() {
+        assertThat(kickerMotor.isConnected()).as("Kicker motor not connected!").isTrue();
+        assertThat(spindexerMotor.isConnected()).as("Spindexer motor not connected!").isTrue();
+    }
 
 }

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

@@ -1,76 +1,88 @@
 // 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.subsystems;
 
 import org.fairportrobotics.frc.posty.TestableSubsystem;
+import static org.fairportrobotics.frc.posty.assertions.Assertions.assertThat;
 import org.fairportrobotics.frc.posty.test.PostTest;
+import static org.fairportrobotics.frc.robolib.motors.Utils.SetMotorDirection;
 
 import com.ctre.phoenix.motorcontrol.LimitSwitchNormal;
 import com.ctre.phoenix.motorcontrol.LimitSwitchSource;
 import com.ctre.phoenix.motorcontrol.can.WPI_TalonSRX;
 import com.ctre.phoenix6.hardware.TalonFX;
-import frc.robot.Constants;
-
-import static org.fairportrobotics.frc.posty.assertions.Assertions.*;
-import static org.fairportrobotics.frc.robolib.motors.Utils.*;
 
 import edu.wpi.first.wpilibj2.command.Command;
 import edu.wpi.first.wpilibj2.command.Commands;
+import frc.robot.Constants;
 
 public class IntakeSubsystem extends TestableSubsystem {
-  /** Creates a new ExampleSubsystem. */
 
-  private TalonFX intakeMotor;
+    /**
-  private WPI_TalonSRX deployMotor;
+     * Creates a new ExampleSubsystem.
+     */
 
-  public IntakeSubsystem() 
+    private TalonFX intakeMotor;
-  {
+    private WPI_TalonSRX deployMotor;
-    intakeMotor = new TalonFX(Constants.IntakeConstants.INTAKE_MOTOR_ID);
-    SetMotorDirection(intakeMotor, Constants.IntakeConstants.INTAKE_MOTOR_DIRECTION);
 
-    deployMotor = new WPI_TalonSRX(Constants.IntakeConstants.DEPLOY_MOTOR_ID);
+    public IntakeSubsystem() {
-    deployMotor.configForwardLimitSwitchSource(LimitSwitchSource.FeedbackConnector, LimitSwitchNormal.NormallyOpen);
+        intakeMotor = new TalonFX(Constants.IntakeConstants.INTAKE_MOTOR_ID);
-    deployMotor.configReverseLimitSwitchSource(LimitSwitchSource.FeedbackConnector, LimitSwitchNormal.NormallyOpen);
+        SetMotorDirection(intakeMotor, Constants.IntakeConstants.INTAKE_MOTOR_DIRECTION);
-    deployMotor.setInverted(Constants.IntakeConstants.DEPLOY_MOTOR_INVERTED);
-  }
 
-  public Command deploy()
+        deployMotor = new WPI_TalonSRX(Constants.IntakeConstants.DEPLOY_MOTOR_ID);
-  {
+        deployMotor.configForwardLimitSwitchSource(LimitSwitchSource.FeedbackConnector, LimitSwitchNormal.NormallyOpen);
-    return Commands.sequence(
+        deployMotor.configReverseLimitSwitchSource(LimitSwitchSource.FeedbackConnector, LimitSwitchNormal.NormallyOpen);
-      Commands.deadline(
+        deployMotor.setInverted(Constants.IntakeConstants.DEPLOY_MOTOR_INVERTED);
-        Commands.waitSeconds(2),
+    }
-        this.runOnce(
-          () -> {deployMotor.set(1);} // Bump this speed up if intake doesn't deploy
-        )
-      ),
-      this.runOnce(() -> deployMotor.stopMotor())
-    );
-  }
 
-  public Command resetDeploy(){
+    public Command deploy() {
-    return this.runEnd(() -> deployMotor.set(-1), () -> deployMotor.stopMotor());
+        return Commands.sequence(
-  }
+                Commands.deadline(
+                        Commands.waitSeconds(2),
+                        this.runOnce(
+                                () -> {
+                                    deployMotor.set(1);
+                                } // Bump this speed up if intake doesn't deploy
+                        )
+                ),
+                this.runOnce(() -> deployMotor.stopMotor())
+        );
+    }
 
-  public void setSpeed(double speed)
+    public Command resetDeploy() {
-  {
+        return this.runEnd(() -> deployMotor.set(-1), () -> deployMotor.stopMotor());
-    intakeMotor.set(speed);
+    }
-  }
 
-  public Command intake()
+    public void setSpeed(double speed) {
-  {
+        intakeMotor.set(speed);
-    return this.runOnce(() -> {if(intakeMotor.get()==0){setSpeed(-.5);}else{intakeMotor.stopMotor();}});
+    }
-  }
 
-  public Command reverseIntake()
+    public Command stopIntake() {
-  {
+        return this.runOnce(() -> {
-    return this.runEnd(() -> setSpeed(.5), () -> { intakeMotor.stopMotor();});
+            intakeMotor.stopMotor();
-  }
+        });
+    }
 
-  @PostTest(enabled = true)
+    public Command intake() {
-  public void IntakeSubsystem_CANDevicesConnected(){
+        return this.runOnce(() -> {
-    assertThat(intakeMotor.isConnected()).as("Intake motor not connected!").isTrue();
+            if (intakeMotor.get() == 0) {
-    assertThat(deployMotor.isAlive()).as("Deploy controller not connected!").isTrue();
+                setSpeed(-.5);
-  }
+            } else {
+                intakeMotor.stopMotor();
+            }
+        });
+    }
+
+    public Command reverseIntake() {
+        return this.runEnd(() -> setSpeed(.5), () -> {
+            intakeMotor.stopMotor();
+        });
+    }
+
+    @PostTest(enabled = true)
+    public void IntakeSubsystem_CANDevicesConnected() {
+        assertThat(intakeMotor.isConnected()).as("Intake motor not connected!").isTrue();
+        assertThat(deployMotor.isAlive()).as("Deploy controller not connected!").isTrue();
+    }
 }