From c6b0d29acdb24728ed566b25d6434a45fbd963c7 Mon Sep 17 00:00:00 2001 From: Michael Mikovsky <77305074+Astatin3@users.noreply.github.com> Date: Wed, 25 Mar 2026 11:15:46 -0600 Subject: [PATCH] Add bounce code --- .../robot/constants/BuildConstants.java | 16 +++---- .../robot/subsystems/intake/Intake.java | 47 ++++++++++++++----- .../subsystems/intake/IntakeConstants.java | 12 +++++ .../robot/subsystems/intake/IntakeIO.java | 2 + 4 files changed, 57 insertions(+), 20 deletions(-) diff --git a/src/main/java/frc4388/robot/constants/BuildConstants.java b/src/main/java/frc4388/robot/constants/BuildConstants.java index b2c6e49..57f5e5e 100644 --- a/src/main/java/frc4388/robot/constants/BuildConstants.java +++ b/src/main/java/frc4388/robot/constants/BuildConstants.java @@ -5,15 +5,15 @@ package frc4388.robot.constants; */ public final class BuildConstants { public static final String MAVEN_GROUP = ""; - public static final String MAVEN_NAME = "2026KPopRobotHunters"; + public static final String MAVEN_NAME = "2026KPopRobotHunters-new"; public static final String VERSION = "unspecified"; - public static final int GIT_REVISION = 182; - public static final String GIT_SHA = "75cab187e2225e51259336e5569f15c41f9169d5"; - public static final String GIT_DATE = "2026-03-21 13:38:34 MDT"; - public static final String GIT_BRANCH = "MiraOrg"; - public static final String BUILD_DATE = "2026-03-21 15:45:59 MDT"; - public static final long BUILD_UNIX_TIME = 1774129559544L; - public static final int DIRTY = 1; + public static final int GIT_REVISION = 183; + public static final String GIT_SHA = "d639b3076d10f39eb5bc18c42c3dc5e707e991ff"; + public static final String GIT_DATE = "2026-03-21 15:49:44 MDT"; + public static final String GIT_BRANCH = "New-Intake"; + public static final String BUILD_DATE = "2026-03-23 10:56:08 MDT"; + public static final long BUILD_UNIX_TIME = 1774284968618L; + public static final int DIRTY = 0; 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 d293bd8..21782aa 100644 --- a/src/main/java/frc4388/robot/subsystems/intake/Intake.java +++ b/src/main/java/frc4388/robot/subsystems/intake/Intake.java @@ -1,10 +1,15 @@ package frc4388.robot.subsystems.intake; +import static edu.wpi.first.units.Units.Amps; import static edu.wpi.first.units.Units.Rotations; +import static edu.wpi.first.units.Units.RotationsPerSecond; + import java.util.function.Supplier; import org.littletonrobotics.junction.Logger; +import com.ctre.phoenix6.Utils; + import edu.wpi.first.math.geometry.Pose2d; import edu.wpi.first.wpilibj2.command.SubsystemBase; @@ -25,16 +30,23 @@ public class Intake extends SubsystemBase { public enum IntakeMode { Extended, Retracted, - Extending, - Retracting, Idle, - RollerStop + Bouncing } private IntakeMode mode = IntakeMode.Idle; public void setMode(IntakeMode mode) { this.mode = mode; + + switch (mode) { + case Bouncing: + // When bounce is enabled: set the bounce timer + this.state.currentBounceTime = Utils.getSystemTimeSeconds() + IntakeConstants.BOUNCE_HALF_PERIOD; + break; + default: + break; + } } public IntakeMode getMode() { @@ -78,6 +90,8 @@ public class Intake extends SubsystemBase { io.updateInputs(state); + // getCurrentTime + switch (mode) { case Extended: io.setArmAngle(state, Rotations.of(IntakeConstants.ARM_LIMIT_EXTENDED.get())); @@ -87,18 +101,27 @@ public class Intake extends SubsystemBase { io.setArmAngle(state, Rotations.of(IntakeConstants.ARM_LIMIT_RETRACTED.get())); io.setRollerOutput(state, 0); break; - case Extending: - io.armOutput(IntakeConstants.ARM_EXTEND_PERCENT_OUTPUT.get()); - io.setRollerOutput(state, IntakeConstants.ROLLER_PERCENT_OUTPUT.get()); - break; - case Retracting: - io.armOutput(IntakeConstants.ARM_RETRACT_PERCENT_OUTPUT.get()); + case Bouncing: io.setRollerOutput(state, 0); + + if( + state.armMotorCurrent.in(Amps) < IntakeConstants.INTAKE_BOUNCE_CURRENT_LIMIT.get() && + state.armMotorVelocity.in(RotationsPerSecond) < IntakeConstants.INTAKE_BOUNCE_VELOCITY_LIMIT.get() + ) { + this.state.currentBounceTime = Utils.getSystemTimeSeconds() + IntakeConstants.BOUNCE_HALF_PERIOD; + } + + // Get the time delta from the last bounce time update + double currentTime = Utils.getSystemTimeSeconds() - state.currentBounceTime; + // Get the percentage through the bounce period (0 output means one half period has passed) + double percentOutput = (currentTime / IntakeConstants.BOUNCE_HALF_PERIOD) * IntakeConstants.INTAKE_BOUNCE_OUTPUT; + // Clamp the output of the motor to some value + percentOutput = Math.max(Math.min(percentOutput, IntakeConstants.INTAKE_BOUNCE_MAX_OUTPUT), -IntakeConstants.INTAKE_BOUNCE_MAX_OUTPUT); + + io.armOutput(percentOutput); break; case Idle: - io.stopArm(); - break; - case RollerStop: + // io.setArmAngle(state, Rotations.of(IntakeConstants.ARM_LIMIT_RETRACTED.get())); io.setRollerOutput(state, 0); break; } diff --git a/src/main/java/frc4388/robot/subsystems/intake/IntakeConstants.java b/src/main/java/frc4388/robot/subsystems/intake/IntakeConstants.java index dc82e42..7aa88cc 100644 --- a/src/main/java/frc4388/robot/subsystems/intake/IntakeConstants.java +++ b/src/main/java/frc4388/robot/subsystems/intake/IntakeConstants.java @@ -1,11 +1,16 @@ package frc4388.robot.subsystems.intake; +import static edu.wpi.first.units.Units.Amps; +import static edu.wpi.first.units.Units.RotationsPerSecond; + import com.ctre.phoenix6.configs.CurrentLimitsConfigs; import com.ctre.phoenix6.configs.MotorOutputConfigs; import com.ctre.phoenix6.configs.Slot0Configs; import com.ctre.phoenix6.configs.TalonFXConfiguration; import com.ctre.phoenix6.signals.NeutralModeValue; +import edu.wpi.first.units.measure.AngularVelocity; +import edu.wpi.first.units.measure.Current; import frc4388.utility.configurable.ConfigurableDouble; import frc4388.utility.status.CanDevice; @@ -16,6 +21,13 @@ public class IntakeConstants { public static final double ROLLER_MOTOR_GEAR_RATIO = 3; + public static final double BOUNCE_HALF_PERIOD = 5.; + public static final double INTAKE_BOUNCE_OUTPUT = 0.2; + public static final double INTAKE_BOUNCE_MAX_OUTPUT = 0.5; + public static final ConfigurableDouble INTAKE_BOUNCE_CURRENT_LIMIT = new ConfigurableDouble("Intake Bounce Current Limit", 20); + public static final ConfigurableDouble INTAKE_BOUNCE_VELOCITY_LIMIT = new ConfigurableDouble("Intake Bounce Velocity Limit", 20); + + //IDs diff --git a/src/main/java/frc4388/robot/subsystems/intake/IntakeIO.java b/src/main/java/frc4388/robot/subsystems/intake/IntakeIO.java index 5f99722..8d1e803 100644 --- a/src/main/java/frc4388/robot/subsystems/intake/IntakeIO.java +++ b/src/main/java/frc4388/robot/subsystems/intake/IntakeIO.java @@ -15,6 +15,8 @@ import edu.wpi.first.units.measure.Current; public interface IntakeIO { @AutoLog public class IntakeState { + double currentBounceTime = 0; + boolean retractedLimit = false; Angle armAngle = Rotations.of(0); Angle armTargetAngle = Rotations.of(0);