diff --git a/src/main/java/frc4388/robot/RobotContainer.java b/src/main/java/frc4388/robot/RobotContainer.java index 3a9100e..e27e6b2 100644 --- a/src/main/java/frc4388/robot/RobotContainer.java +++ b/src/main/java/frc4388/robot/RobotContainer.java @@ -13,7 +13,6 @@ import com.pathplanner.lib.commands.PathPlannerAuto; import edu.wpi.first.math.geometry.Rotation2d; import edu.wpi.first.math.geometry.Translation2d; -import edu.wpi.first.wpilibj.DigitalInput; import edu.wpi.first.wpilibj.DriverStation; import edu.wpi.first.wpilibj.GenericHID; import edu.wpi.first.wpilibj.RobotBase; @@ -35,7 +34,6 @@ import frc4388.robot.subsystems.LED; import frc4388.robot.subsystems.intake.Intake; import frc4388.robot.subsystems.intake.Intake.IntakeMode; import frc4388.robot.subsystems.shooter.Shooter; -import frc4388.robot.subsystems.shooter.Shooter.ShooterMode; import frc4388.robot.subsystems.swerve.SwerveDrive; import frc4388.robot.subsystems.vision.Vision; import frc4388.utility.DeferredBlock; @@ -252,12 +250,18 @@ public class RobotContainer { //Operator Controls new Trigger(() -> getDeadbandedOperatorController().getLeftTriggerAxis() >= 0.5) .onTrue(new InstantCommand(() -> { - m_robotIntake.setMode(IntakeMode.Extended); + m_robotIntake.setMode(IntakeMode.Extending); + })) + .onFalse(new InstantCommand(() -> { + m_robotIntake.setMode(IntakeMode.Idle); })); new JoystickButton(getDeadbandedOperatorController(), XboxController.LEFT_BUMPER_BUTTON) + .onTrue(new InstantCommand(() -> { + m_robotIntake.setMode(IntakeMode.Retracting); + })) .onFalse(new InstantCommand(() -> { - m_robotIntake.setMode(IntakeMode.Retracted); + m_robotIntake.setMode(IntakeMode.Idle); })); new Trigger(() -> getDeadbandedOperatorController().getRightTriggerAxis() >= 0.5) diff --git a/src/main/java/frc4388/robot/constants/BuildConstants.java b/src/main/java/frc4388/robot/constants/BuildConstants.java index 469fd5d..ba29324 100644 --- a/src/main/java/frc4388/robot/constants/BuildConstants.java +++ b/src/main/java/frc4388/robot/constants/BuildConstants.java @@ -7,12 +7,12 @@ public final class BuildConstants { public static final String MAVEN_GROUP = ""; public static final String MAVEN_NAME = "2026KPopRobotHunters"; public static final String VERSION = "unspecified"; - public static final int GIT_REVISION = 59; - public static final String GIT_SHA = "7f251857dc4fc6f925d5900e9822f20c8841fa1d"; - public static final String GIT_DATE = "2026-02-14 15:05:25 MST"; + public static final int GIT_REVISION = 60; + public static final String GIT_SHA = "f88619a1da6b3adf64df3c4277deeca5bed6ee21"; + public static final String GIT_DATE = "2026-02-14 15:57:33 MST"; public static final String GIT_BRANCH = "operator-controls"; - public static final String BUILD_DATE = "2026-02-14 15:56:10 MST"; - public static final long BUILD_UNIX_TIME = 1771109770332L; + public static final String BUILD_DATE = "2026-02-16 15:54:00 MST"; + public static final long BUILD_UNIX_TIME = 1771282440915L; public static final int DIRTY = 1; private BuildConstants(){} diff --git a/src/main/java/frc4388/robot/subsystems/intake/Intake.java b/src/main/java/frc4388/robot/subsystems/intake/Intake.java index ed73b44..2d72724 100644 --- a/src/main/java/frc4388/robot/subsystems/intake/Intake.java +++ b/src/main/java/frc4388/robot/subsystems/intake/Intake.java @@ -7,9 +7,7 @@ import java.util.function.Supplier; import org.littletonrobotics.junction.Logger; import edu.wpi.first.math.geometry.Pose2d; -import edu.wpi.first.wpilibj.DigitalInput; import edu.wpi.first.wpilibj2.command.SubsystemBase; -import frc4388.robot.subsystems.intake.Intake.IntakeMode; public class Intake extends SubsystemBase { public IntakeIO io; @@ -29,7 +27,8 @@ public class Intake extends SubsystemBase { Extended, Retracted, Extending, - Retracting + Retracting, + Idle } private IntakeMode mode = IntakeMode.Extended; @@ -73,22 +72,19 @@ public class Intake extends SubsystemBase { io.setRollerVelocity(state, RotationsPerSecond.of(IntakeConstants.ROLLER_ACTIVE.get())); break; case Retracted: - if (!state.retractedLimit){ - io.stopArm(); - } else { - io.setArmAngle(state, Rotations.of(IntakeConstants.ARM_LIMIT_RETRACTED.get())); - } + io.setArmAngle(state, Rotations.of(IntakeConstants.ARM_LIMIT_RETRACTED.get())); io.setRollerVelocity(state, RotationsPerSecond.of(0)); break; case Extending: - io.armExtend(IntakeConstants.ARM_EXTEND_PERCENT_OUTPUT.get()); + io.armOutput(0.1); + io.setRollerVelocity(state, RotationsPerSecond.of(IntakeConstants.ROLLER_ACTIVE.get())); break; case Retracting: - if (!state.retractedLimit){ - io.stopArm(); - } else { - io.armRetract(IntakeConstants.ARM_RETRACT_PERCENT_OUTPUT.get()); - } + io.armOutput(-0.1); + io.setRollerVelocity(state, RotationsPerSecond.of(0)); + break; + case Idle: + io.stopArm(); break; } diff --git a/src/main/java/frc4388/robot/subsystems/intake/IntakeIO.java b/src/main/java/frc4388/robot/subsystems/intake/IntakeIO.java index 42054c7..4babe7f 100644 --- a/src/main/java/frc4388/robot/subsystems/intake/IntakeIO.java +++ b/src/main/java/frc4388/robot/subsystems/intake/IntakeIO.java @@ -36,8 +36,7 @@ public interface IntakeIO { public default void setArmAngle(IntakeState state, Angle angle) {} public default void stopArm() {} public default void setRollerVelocity(IntakeState state, AngularVelocity angularVelocity) {} - public default void armExtend(double percentOutput) {} - public default void armRetract(double percentOutput) {} + public default void armOutput(double percentOutput) {} public default void updateInputs(IntakeState state) {} public default void updateGains() {} } \ No newline at end of file diff --git a/src/main/java/frc4388/robot/subsystems/intake/IntakeReal.java b/src/main/java/frc4388/robot/subsystems/intake/IntakeReal.java index fe7bffb..6cafe03 100644 --- a/src/main/java/frc4388/robot/subsystems/intake/IntakeReal.java +++ b/src/main/java/frc4388/robot/subsystems/intake/IntakeReal.java @@ -1,5 +1,9 @@ package frc4388.robot.subsystems.intake; +import static edu.wpi.first.units.Units.Degrees; +import static edu.wpi.first.units.Units.Rotation; + +import com.ctre.phoenix6.controls.DutyCycleOut; import com.ctre.phoenix6.controls.PositionDutyCycle; import com.ctre.phoenix6.controls.VelocityDutyCycle; import com.ctre.phoenix6.hardware.TalonFX; @@ -69,6 +73,7 @@ public class IntakeReal implements IntakeIO { @Override public void setArmAngle(IntakeState state, Angle angle) { + state.armTargetAngle = angle; // Assume that the angle is always accurate, because I think we will use a shaft encoder // Assume that 0 degrees = forwards. Might need an offset here @@ -79,20 +84,25 @@ public class IntakeReal implements IntakeIO { Angle motorAngle = angle.times(IntakeConstants.ARM_MOTOR_GEAR_RATIO); // PositionDutyCycle posRequest = new PositionDutyCycle(motorTargetAngle); - m_armMotor.setControl(armPosition.withPosition(motorAngle)); + m_armMotor.setControl( + armPosition + .withPosition(motorAngle) + .withLimitReverseMotion(!m_armLimitSwitch.get()) + ); } + @Override public void stopArm(){ m_armMotor.set(0); } + @Override - public void armExtend(double percentOutput){ - m_armMotor.set(percentOutput); - } - - @Override - public void armRetract(double percentOutput){ - m_armMotor.set(percentOutput); + public void armOutput(double percentOutput){ + var d = new DutyCycleOut(0); + m_armMotor.setControl( + d.withOutput(percentOutput) + .withLimitReverseMotion(!m_armLimitSwitch.get()) + ); } @Override