From 776e3ed9d34629773027fe3798b727feaa963592 Mon Sep 17 00:00:00 2001 From: Scott Jackson Date: Mon, 13 Apr 2026 18:30:45 -0400 Subject: [PATCH 1/3] Helper functions for regions and targets --- src/main/java/frc/robot/Constants.java | 46 +++++++++ src/main/java/frc/robot/Utils.java | 132 +++++++++++++++++++++++++ 2 files changed, 178 insertions(+) diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java index fc73400..218b824 100644 --- a/src/main/java/frc/robot/Constants.java +++ b/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 } } diff --git a/src/main/java/frc/robot/Utils.java b/src/main/java/frc/robot/Utils.java index 64bd85f..9899985 100644 --- a/src/main/java/frc/robot/Utils.java +++ b/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; + } + } From 9c85cd1c8a7a86d8d42eb6c678f6ce473f736f64 Mon Sep 17 00:00:00 2001 From: TheRealGrayden Date: Mon, 13 Apr 2026 19:57:27 -0400 Subject: [PATCH 2/3] command changes --- src/main/java/frc/robot/RobotContainer.java | 14 ++---- src/main/java/frc/robot/commands/Reverse.java | 50 +++++++++++++++++++ 2 files changed, 55 insertions(+), 9 deletions(-) create mode 100644 src/main/java/frc/robot/commands/Reverse.java diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index d6d68cd..3a9f8cf 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -13,9 +13,6 @@ 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.SetDeflectorCommand; -import frc.robot.commands.ZoneCheck; import frc.robot.subsystems.DriveSubsystem; import frc.robot.subsystems.HopperSubsystem; import frc.robot.subsystems.IntakeSubsystem; @@ -72,21 +69,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(Commands.parallel(intakeSubsystem.reverseIntake(), null)); + m_driverController.y().onTrue(intakeSubsystem.deploy()); } /** diff --git a/src/main/java/frc/robot/commands/Reverse.java b/src/main/java/frc/robot/commands/Reverse.java new file mode 100644 index 0000000..cb3bb66 --- /dev/null +++ b/src/main/java/frc/robot/commands/Reverse.java @@ -0,0 +1,50 @@ +// 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 int m_subsystem; + + /** + * Creates a new ExampleCommand. + * + * @param subsystem The subsystem used by this command. + */ + public Reverse() { + m_subsystem = subsystem; + // Use addRequirements() here to declare subsystem dependencies. + addRequirements(subsystem); + } + + // 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() { + } + + // Called once the command ends or is interrupted. + @Override + public void end(boolean interrupted) { + hopperSubsystem.stopKicker(); + hopperSubsystem.spindexerOff(); + } + + // Returns true when the command should end. + @Override + public boolean isFinished() { + return false; + } +} From 343884052f9373061ddee2ad599387d07c1aec31 Mon Sep 17 00:00:00 2001 From: TheRealGrayden Date: Mon, 13 Apr 2026 20:13:46 -0400 Subject: [PATCH 3/3] Reverse command :3 --- src/main/java/frc/robot/RobotContainer.java | 3 +- src/main/java/frc/robot/commands/Reverse.java | 14 +- .../frc/robot/subsystems/HopperSubsystem.java | 120 +++++++++--------- .../frc/robot/subsystems/IntakeSubsystem.java | 110 +++++++++------- 4 files changed, 135 insertions(+), 112 deletions(-) diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index cd61809..517869e 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -13,6 +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.Reverse; import frc.robot.subsystems.DriveSubsystem; import frc.robot.subsystems.HopperSubsystem; import frc.robot.subsystems.IntakeSubsystem; @@ -82,7 +83,7 @@ public class RobotContainer { //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().onTrue(Commands.parallel(intakeSubsystem.reverseIntake(), null)); + m_driverController.b().onTrue(new Reverse(intakeSubsystem, hopperSubsystem)); m_driverController.y().onTrue(intakeSubsystem.deploy()); } diff --git a/src/main/java/frc/robot/commands/Reverse.java b/src/main/java/frc/robot/commands/Reverse.java index cb3bb66..d2a7425 100644 --- a/src/main/java/frc/robot/commands/Reverse.java +++ b/src/main/java/frc/robot/commands/Reverse.java @@ -12,17 +12,17 @@ import frc.robot.subsystems.IntakeSubsystem; */ public class Reverse extends Command { - private int m_subsystem; + private IntakeSubsystem intakeSubsystem; + private HopperSubsystem hopperSubsystem; /** * Creates a new ExampleCommand. * * @param subsystem The subsystem used by this command. */ - public Reverse() { - m_subsystem = subsystem; - // Use addRequirements() here to declare subsystem dependencies. - addRequirements(subsystem); + public Reverse(IntakeSubsystem intakeSubsystem, HopperSubsystem hopperSubsystem) { + this.intakeSubsystem = intakeSubsystem; + this.hopperSubsystem = hopperSubsystem; } // Called when the command is initially scheduled. @@ -33,6 +33,9 @@ public class Reverse extends Command { // 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. @@ -40,6 +43,7 @@ public class Reverse extends Command { public void end(boolean interrupted) { hopperSubsystem.stopKicker(); hopperSubsystem.spindexerOff(); + intakeSubsystem.stopIntake(); } // Returns true when the command should end. diff --git a/src/main/java/frc/robot/subsystems/HopperSubsystem.java b/src/main/java/frc/robot/subsystems/HopperSubsystem.java index 8097181..0bfdfb2 100644 --- a/src/main/java/frc/robot/subsystems/HopperSubsystem.java +++ b/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 kickerVelocity; - /** Creates a new HopperSubsystem. */ - public HopperSubsystem() { - spindexerMotor = new TalonFX(Constants.HopperConstants.SPINDEXER_MOTOR_ID); - spindexerMotor.getConfigurator().apply( - new TalonFXConfiguration() - .withMotorOutput( - new MotorOutputConfigs() - .withInverted(Constants.HopperConstants.SPINDEXER_MOTOR_DIRECTION)) - .withSlot0( - new Slot0Configs() - .withKP(0) - .withKI(0) - .withKD(0) - .withKS(0))); + /** + * Creates a new HopperSubsystem. + */ + public HopperSubsystem() { + spindexerMotor = new TalonFX(Constants.HopperConstants.SPINDEXER_MOTOR_ID); + spindexerMotor.getConfigurator().apply( + new TalonFXConfiguration() + .withMotorOutput( + new MotorOutputConfigs() + .withInverted(Constants.HopperConstants.SPINDEXER_MOTOR_DIRECTION)) + .withSlot0( + new Slot0Configs() + .withKP(0) + .withKI(0) + .withKD(0) + .withKS(0))); - kickerMotor = new TalonFX(Constants.HopperConstants.KICKER_MOTOR_ID); - kickerMotor.getConfigurator().apply( - new TalonFXConfiguration() - .withMotorOutput( - new MotorOutputConfigs() - .withInverted(Constants.HopperConstants.KICKER_MOTOR_DIRECTION) - ) - .withSlot0(new Slot0Configs() - .withKP(0) - .withKI(0) - .withKD(0) - .withKS(0) - )); - kickerVelocity = kickerMotor.getVelocity(); - } + kickerMotor = new TalonFX(Constants.HopperConstants.KICKER_MOTOR_ID); + kickerMotor.getConfigurator().apply( + new TalonFXConfiguration() + .withMotorOutput( + new MotorOutputConfigs() + .withInverted(Constants.HopperConstants.KICKER_MOTOR_DIRECTION) + ) + .withSlot0(new Slot0Configs() + .withKP(0) + .withKI(0) + .withKD(0) + .withKS(0) + )); + kickerVelocity = kickerMotor.getVelocity(); + } - public void feedKicker() { - //kickerMotor.setControl(kickerControl.withVelocity(1000)); - kickerMotor.set(0.75); - } + public void feedKicker() { + //kickerMotor.setControl(kickerControl.withVelocity(1000)); + kickerMotor.set(0.75); + } - public void reverseKicker() { - //kickerMotor.setControl(kickerControl.withVelocity(-500)); - kickerMotor.set(-0.5); - } + public void reverseKicker() { + //kickerMotor.setControl(kickerControl.withVelocity(-500)); + 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 periodic() { - Logger.recordOutput("HopperSubsystem-KickerSpeed(Rotations/sec)", kickerVelocity.refresh().getValue().in(Units.RotationsPerSecond)); - } + public void spindexerOff() { + spindexerMotor.stopMotor(); + } - @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(); - } + @Override + public void periodic() { + Logger.recordOutput("HopperSubsystem-KickerSpeed(Rotations/sec)", kickerVelocity.refresh().getValue().in(Units.RotationsPerSecond)); + } -} \ No newline at end of file + @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(); + } + +} diff --git a/src/main/java/frc/robot/subsystems/IntakeSubsystem.java b/src/main/java/frc/robot/subsystems/IntakeSubsystem.java index 7d0e930..9f7bb3c 100644 --- a/src/main/java/frc/robot/subsystems/IntakeSubsystem.java +++ b/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() - { - intakeMotor = new TalonFX(Constants.IntakeConstants.INTAKE_MOTOR_ID); - SetMotorDirection(intakeMotor, Constants.IntakeConstants.INTAKE_MOTOR_DIRECTION); + private TalonFX intakeMotor; + private WPI_TalonSRX deployMotor; - deployMotor = new WPI_TalonSRX(Constants.IntakeConstants.DEPLOY_MOTOR_ID); - deployMotor.configForwardLimitSwitchSource(LimitSwitchSource.FeedbackConnector, LimitSwitchNormal.NormallyOpen); - deployMotor.configReverseLimitSwitchSource(LimitSwitchSource.FeedbackConnector, LimitSwitchNormal.NormallyOpen); - deployMotor.setInverted(Constants.IntakeConstants.DEPLOY_MOTOR_INVERTED); - } + public IntakeSubsystem() { + intakeMotor = new TalonFX(Constants.IntakeConstants.INTAKE_MOTOR_ID); + SetMotorDirection(intakeMotor, Constants.IntakeConstants.INTAKE_MOTOR_DIRECTION); - public Command deploy() - { - 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()) - ); - } + deployMotor = new WPI_TalonSRX(Constants.IntakeConstants.DEPLOY_MOTOR_ID); + deployMotor.configForwardLimitSwitchSource(LimitSwitchSource.FeedbackConnector, LimitSwitchNormal.NormallyOpen); + deployMotor.configReverseLimitSwitchSource(LimitSwitchSource.FeedbackConnector, LimitSwitchNormal.NormallyOpen); + deployMotor.setInverted(Constants.IntakeConstants.DEPLOY_MOTOR_INVERTED); + } - public Command resetDeploy(){ - return this.runEnd(() -> deployMotor.set(-1), () -> deployMotor.stopMotor()); - } + public Command deploy() { + 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) - { - intakeMotor.set(speed); - } + public Command resetDeploy() { + return this.runEnd(() -> deployMotor.set(-1), () -> deployMotor.stopMotor()); + } - public Command intake() - { - return this.runOnce(() -> {if(intakeMotor.get()==0){setSpeed(-.5);}else{intakeMotor.stopMotor();}}); - } + public void setSpeed(double speed) { + intakeMotor.set(speed); + } - public Command reverseIntake() - { - return this.runEnd(() -> setSpeed(.5), () -> { intakeMotor.stopMotor();}); - } + public Command stopIntake() { + return this.runOnce(() -> { + 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(); - } + public Command intake() { + return this.runOnce(() -> { + if (intakeMotor.get() == 0) { + 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(); + } }