From aebc64fd6361b852dfa048595995189f84aa9f90 Mon Sep 17 00:00:00 2001 From: Mechaniac Date: Fri, 6 Feb 2026 03:05:25 +0800 Subject: [PATCH] Set intake trigger index power to zero to avoid premature launch --- .../ftc/teamcode/CommandTeleOp.java | 2 +- .../ftc/teamcode/Commands/IntakeCommand.java | 22 +++++- .../Subsystems/BeamBreakSubsystem.java | 73 +++++-------------- 3 files changed, 41 insertions(+), 56 deletions(-) diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/CommandTeleOp.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/CommandTeleOp.java index 4938d86..96bc0a6 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/CommandTeleOp.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/CommandTeleOp.java @@ -136,7 +136,7 @@ public void initialize() { leftTrigger.whileActiveContinuous( - new IntakeCommand(feeder, () -> beamBreak.isBeamBreakOn(), Constants.Feeder.DEFAULT_INTAKE_POWER, 0.6) + new IntakeCommand(feeder, () -> beamBreak.isBeamBreakOn(), Constants.Feeder.DEFAULT_INTAKE_POWER, 0) ); leftBumper.whileActiveContinuous( new IntakeCommand(feeder, () -> false, -Constants.Feeder.DEFAULT_INTAKE_POWER, -Constants.Feeder.DEFAULT_INDEX_POWER) diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Commands/IntakeCommand.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Commands/IntakeCommand.java index 3033455..81853f7 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Commands/IntakeCommand.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Commands/IntakeCommand.java @@ -12,7 +12,9 @@ public class IntakeCommand extends CommandBase { private final FeederSubsystem feeder; private BooleanSupplier breamBreak; private double intakePower, indexPower; - private static final long INDEX_FEED_PULSE_MS = 260L; + private static final long INDEX_FEED_PULSE_MS = 180L; + private static final double INDEX_FEED_PULSE_POWER_INTAKE = 0.35; + private static final double INDEX_FEED_PULSE_POWER_ACTIVE = 0.6; private boolean beamBlockedLatched; private long indexFeedUntilNanos; @@ -45,6 +47,10 @@ public void execute() { if (!beamBlocked) { beamBlockedLatched = false; + indexFeedUntilNanos = 0L; + feeder.setIntakeOpenLoop(intakePower); + feeder.setIndexOpenLoop(indexPower); + return; } if (beamBlocked && !beamBlockedLatched) { @@ -53,9 +59,21 @@ public void execute() { } final boolean shouldFeedIndexer = nowNanos < indexFeedUntilNanos; + final double indexPulsePower = getIndexPulsePower(); feeder.setIntakeOpenLoop(intakePower); - feeder.setIndexOpenLoop(shouldFeedIndexer ? indexPower : 0.0); + feeder.setIndexOpenLoop(shouldFeedIndexer ? indexPulsePower : 0.0); + } + + private double getIndexPulsePower() { + if (Math.abs(indexPower) < 1e-6) { + return INDEX_FEED_PULSE_POWER_INTAKE; + } + + return Math.copySign( + Math.min(Math.abs(indexPower), INDEX_FEED_PULSE_POWER_ACTIVE), + indexPower + ); } diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Subsystems/BeamBreakSubsystem.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Subsystems/BeamBreakSubsystem.java index 87de729..69ced4a 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Subsystems/BeamBreakSubsystem.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Subsystems/BeamBreakSubsystem.java @@ -1,63 +1,30 @@ -//package org.firstinspires.ftc.teamcode.Subsystems; +package org.firstinspires.ftc.teamcode.Subsystems; -//import com.arcrobotics.ftclib.command.SubsystemBase; -//import com.qualcomm.robotcore.hardware.AnalogInput; -import com.qualcomm.robotcore.hardware.HardwareMap; - -//public class BeamBreakSubsystem extends SubsystemBase { - - //private final AnalogInput beamBreak; - //private final double defaultToleranceVoltage; - - //public BeamBreakSubsystem(HardwareMap hardwareMap, String name, double defaultToleranceVoltage) { - //beamBreak = hardwareMap.get(AnalogInput.class, name); - //this.defaultToleranceVoltage = defaultToleranceVoltage; - //} - - - //public boolean isBeamBreakOn() { - //return isBeamBreakOn(defaultToleranceVoltage); - //} - - //public boolean isBeamBreakOn(double toleranceVoltage) { - // return beamBreak.getVoltage() >= toleranceVoltage; - //} - - //public double getVoltage() { - //return beamBreak.getVoltage(); - //} -//} -package org.firstinspires.ftc.teamcode; - -import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode; -import com.qualcomm.robotcore.eventloop.opmode.TeleOp; +import com.arcrobotics.ftclib.command.SubsystemBase; import com.qualcomm.robotcore.hardware.AnalogInput; +import com.qualcomm.robotcore.hardware.HardwareMap; -@TeleOp(name = "Beam Break Debug", group = "Debug") -public class debug_beam_break extends LinearOpMode { - - private AnalogInput beamBreak; - - @Override - public void runOpMode() { - // 初始化光电门 - beamBreak = hardwareMap.get(AnalogInput.class, "shooterBB"); +public class BeamBreakSubsystem extends SubsystemBase { - telemetry.addData("Status", "Initialized"); - telemetry.update(); + private final AnalogInput beamBreak; + private final double defaultToleranceVoltage; - waitForStart(); + public BeamBreakSubsystem(final HardwareMap hardwareMap, + final String name, + final double defaultToleranceVoltage) { + beamBreak = hardwareMap.get(AnalogInput.class, name); + this.defaultToleranceVoltage = defaultToleranceVoltage; + } - while (opModeIsActive()) { - // 读取电压值 - double voltage = beamBreak.getVoltage(); + public boolean isBeamBreakOn() { + return isBeamBreakOn(defaultToleranceVoltage); + } - telemetry.addData("Beam Break Voltage", "%.3f V", voltage); - telemetry.addData("Beam Break Status", voltage >= 1.0 ? "BLOCKED" : "CLEAR"); - telemetry.addData("Expected Range", "0.0V - 3.3V"); - telemetry.update(); + public boolean isBeamBreakOn(final double toleranceVoltage) { + return beamBreak.getVoltage() >= toleranceVoltage; + } - sleep(100); - } + public double getVoltage() { + return beamBreak.getVoltage(); } }