From 40d2b092abe63399e78bdb9db4a04bc9ff8a3df2 Mon Sep 17 00:00:00 2001 From: ultimatehecker <86735991+ultimatehecker@users.noreply.github.com> Date: Thu, 29 Jan 2026 12:27:57 -0500 Subject: [PATCH 1/4] added second intake motor (will clean up later) --- .../org/firstinspires/ftc/teamcode/subsystems/Intake.java | 6 ++++++ 1 file changed, 6 insertions(+) diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystems/Intake.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystems/Intake.java index 5189051..92add58 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystems/Intake.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystems/Intake.java @@ -16,6 +16,7 @@ public class Intake extends SubsystemBase { private DcMotorEx intakeMotor; + private DcMotorEx frontIntakeMotor; private PIDFController velocityPIDFController; private SimpleMotorFeedforward velocityFeedforward; @@ -25,8 +26,11 @@ public class Intake extends SubsystemBase { public Intake(HardwareMap hMap, TelemetryManager telemetryM) { intakeMotor = hMap.get(DcMotorEx.class, IntakeConstants.intakeMotorID); + frontIntakeMotor = hMap.get(DcMotorEx.class, "frontIntakeMotor"); intakeMotor.setDirection(DcMotorSimple.Direction.REVERSE); intakeMotor.setMode(DcMotor.RunMode.RUN_WITHOUT_ENCODER); + frontIntakeMotor.setDirection(DcMotorSimple.Direction.REVERSE); + frontIntakeMotor.setMode(DcMotor.RunMode.RUN_WITHOUT_ENCODER); velocityPIDFController = new PIDFController(IntakeConstants.kP, IntakeConstants.kI, IntakeConstants.kD, IntakeConstants.kF); velocityFeedforward = new SimpleMotorFeedforward(IntakeConstants.kS, IntakeConstants.kV, IntakeConstants.kA); @@ -50,12 +54,14 @@ public void setVelocitySetpoint(double targetRPM) { velocityFeedforward.setCoefficient(IntakeConstants.kS, IntakeConstants.kV, IntakeConstants.kA); } + frontIntakeMotor.setPower(velocityPIDFController.calculate(getVelocity(), targetRPM) + velocityFeedforward.calculate(targetRPM)); intakeMotor.setPower(velocityPIDFController.calculate(getVelocity(), targetRPM) + velocityFeedforward.calculate(targetRPM)); } public void setOpenLoopSetpoint(double speed) { telemetryM.addData(IntakeConstants.kSubsystemName + "Setpoint Open Loop", speed); intakeMotor.setPower(speed); + frontIntakeMotor.setPower(speed); } public double getVelocity() { From fec8779b5b9eb1c8ec39996ce3f147fd76d62a21 Mon Sep 17 00:00:00 2001 From: ultimatehecker <86735991+ultimatehecker@users.noreply.github.com> Date: Thu, 29 Jan 2026 12:30:28 -0500 Subject: [PATCH 2/4] cleaned up useless imports --- .../ftc/teamcode/RobotContoller.java | 18 ------------------ .../ftc/teamcode/TeleopBindings.java | 1 - .../ftc/teamcode/TestingOpMode.java | 10 ++-------- 3 files changed, 2 insertions(+), 27 deletions(-) diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/RobotContoller.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/RobotContoller.java index 488a72a..566e38b 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/RobotContoller.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/RobotContoller.java @@ -10,29 +10,12 @@ import com.pedropathing.geometry.Pose; import com.qualcomm.robotcore.eventloop.opmode.TeleOp; -import org.firstinspires.ftc.library.command.Command; import org.firstinspires.ftc.library.command.CommandOpMode; -import org.firstinspires.ftc.library.command.CommandScheduler; import org.firstinspires.ftc.library.command.RunCommand; -import org.firstinspires.ftc.library.command.button.Trigger; import org.firstinspires.ftc.library.gamepad.GamepadEx; -import org.firstinspires.ftc.library.gamepad.GamepadKeys; -import org.firstinspires.ftc.robotcore.external.Supplier; -import org.firstinspires.ftc.robotcore.external.Telemetry; import org.firstinspires.ftc.teamcode.autonomous.Auto; import org.firstinspires.ftc.teamcode.autonomous.Location; -import org.firstinspires.ftc.teamcode.command_factories.IntakeFactory; -import org.firstinspires.ftc.teamcode.command_factories.ShooterFactory; -import org.firstinspires.ftc.teamcode.command_factories.TransferFactory; -import org.firstinspires.ftc.teamcode.command_factories.TurretFactory; -import org.firstinspires.ftc.teamcode.commands.ConstrainedFlashCommand; -import org.firstinspires.ftc.teamcode.commands.TeleopMecanum; -import org.firstinspires.ftc.teamcode.commands.TurretPositionSetpoint; import org.firstinspires.ftc.teamcode.constants.GlobalConstants; -import org.firstinspires.ftc.teamcode.constants.LEDConstants; -import org.firstinspires.ftc.teamcode.constants.ShooterConstants; -import org.firstinspires.ftc.teamcode.constants.TransferConstants; -import org.firstinspires.ftc.teamcode.constants.TurretConstants; import org.firstinspires.ftc.teamcode.subsystems.Drivetrain; import org.firstinspires.ftc.teamcode.subsystems.Intake; import org.firstinspires.ftc.teamcode.subsystems.LED; @@ -40,7 +23,6 @@ import org.firstinspires.ftc.teamcode.subsystems.Transfer; import org.firstinspires.ftc.teamcode.subsystems.Turret; import org.firstinspires.ftc.teamcode.subsystems.Vision; -import org.firstinspires.ftc.library.math.geometry.Pose2d; import org.firstinspires.ftc.teamcode.utilities.SavedConfiguration; import java.util.Optional; diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/TeleopBindings.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/TeleopBindings.java index 6b6193c..7e7d8ef 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/TeleopBindings.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/TeleopBindings.java @@ -6,7 +6,6 @@ import org.firstinspires.ftc.teamcode.command_factories.IntakeFactory; import org.firstinspires.ftc.teamcode.command_factories.ShooterFactory; import org.firstinspires.ftc.teamcode.command_factories.TransferFactory; -import org.firstinspires.ftc.teamcode.commands.ConstrainedFlashCommand; import org.firstinspires.ftc.teamcode.commands.TeleopMecanum; import org.firstinspires.ftc.teamcode.constants.LEDConstants; import org.firstinspires.ftc.teamcode.constants.ShooterConstants; diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/TestingOpMode.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/TestingOpMode.java index d29a216..951138f 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/TestingOpMode.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/TestingOpMode.java @@ -5,18 +5,14 @@ import com.bylazar.lights.PanelsLights; import com.bylazar.telemetry.PanelsTelemetry; import com.bylazar.telemetry.TelemetryManager; + import com.pedropathing.follower.Follower; import com.pedropathing.geometry.Pose; -import com.qualcomm.robotcore.eventloop.opmode.Disabled; + import com.qualcomm.robotcore.eventloop.opmode.OpMode; import com.qualcomm.robotcore.eventloop.opmode.TeleOp; -import com.qualcomm.robotcore.hardware.DcMotorEx; -import com.qualcomm.robotcore.hardware.Servo; -import org.firstinspires.ftc.robotcore.external.Telemetry; -import org.firstinspires.ftc.teamcode.constants.IntakeConstants; import org.firstinspires.ftc.teamcode.constants.LEDConstants; -import org.firstinspires.ftc.teamcode.constants.TransferConstants; import org.firstinspires.ftc.teamcode.constants.TurretConstants; import org.firstinspires.ftc.teamcode.pedropathing.Constants; import org.firstinspires.ftc.teamcode.subsystems.Intake; @@ -35,8 +31,6 @@ public class TestingOpMode extends OpMode { public Follower follower; - public boolean toogle = false; - @IgnoreConfigurable private TelemetryManager telemetryManager; From 12db8e48048072ef2d3770aba928aea5f07a4dfd Mon Sep 17 00:00:00 2001 From: ultimatehecker <86735991+ultimatehecker@users.noreply.github.com> Date: Thu, 29 Jan 2026 15:05:02 -0500 Subject: [PATCH 3/4] revamped intake subsystem for second motor (cleaned up) --- .../command_factories/IntakeFactory.java | 12 +++++ .../teamcode/constants/IntakeConstants.java | 6 ++- .../ftc/teamcode/subsystems/Intake.java | 53 ++++++++++++++----- .../ftc/teamcode/subsystems/Transfer.java | 22 +++++++- 4 files changed, 76 insertions(+), 17 deletions(-) diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/command_factories/IntakeFactory.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/command_factories/IntakeFactory.java index 5489086..1185c1e 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/command_factories/IntakeFactory.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/command_factories/IntakeFactory.java @@ -20,4 +20,16 @@ public static Command openLoopSetpointCommand(Intake intake, DoubleSupplier setp intake.setOpenLoopSetpoint(setpoint.getAsDouble()); }).withName("Intake Open Loop"); } + + public static Command setFrontIntakeOpenLoopSetpointCommand(Intake intake, DoubleSupplier setpoint) { + return Commands.runOnce(() -> { + intake.setFrontMotorOpenLoop(setpoint.getAsDouble()); + }).withName("Intake Open Loop"); + } + + public static Command setRearIntakeOpenLoopSetpointCommand(Intake intake, DoubleSupplier setpoint) { + return Commands.runOnce(() -> { + intake.setRearMotorOpenLoop(setpoint.getAsDouble()); + }).withName("Intake Open Loop"); + } } diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/constants/IntakeConstants.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/constants/IntakeConstants.java index a8268c0..112e990 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/constants/IntakeConstants.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/constants/IntakeConstants.java @@ -5,11 +5,15 @@ @Configurable public class IntakeConstants { public static final String kSubsystemName = "Intake "; - public static final String intakeMotorID = "intakeMotor"; + public static final String rearIntakeMotorID = "intakeMotor"; + public static final String frontIntakeMotorID = "frontIntakeMotor"; public static final double intakeMaximumRPM = 435; public static final double intakeMotorCPR = 384.5; + public static final double frontIntakeRatio = (96 / 24.00); + public static final double rearIntakeRatio = 1; + public static double kP = 0.001; public static double kI = 0.0; public static double kD = 0.0; diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystems/Intake.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystems/Intake.java index 92add58..c2d905f 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystems/Intake.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystems/Intake.java @@ -15,8 +15,8 @@ import org.firstinspires.ftc.teamcode.constants.IntakeConstants; public class Intake extends SubsystemBase { - private DcMotorEx intakeMotor; private DcMotorEx frontIntakeMotor; + private DcMotorEx rearIntakeMotor; private PIDFController velocityPIDFController; private SimpleMotorFeedforward velocityFeedforward; @@ -25,12 +25,14 @@ public class Intake extends SubsystemBase { static TelemetryManager telemetryM; public Intake(HardwareMap hMap, TelemetryManager telemetryM) { - intakeMotor = hMap.get(DcMotorEx.class, IntakeConstants.intakeMotorID); - frontIntakeMotor = hMap.get(DcMotorEx.class, "frontIntakeMotor"); - intakeMotor.setDirection(DcMotorSimple.Direction.REVERSE); - intakeMotor.setMode(DcMotor.RunMode.RUN_WITHOUT_ENCODER); + frontIntakeMotor = hMap.get(DcMotorEx.class, IntakeConstants.frontIntakeMotorID); + rearIntakeMotor = hMap.get(DcMotorEx.class, IntakeConstants.rearIntakeMotorID); + frontIntakeMotor.setDirection(DcMotorSimple.Direction.REVERSE); + rearIntakeMotor.setDirection(DcMotorSimple.Direction.REVERSE); + frontIntakeMotor.setMode(DcMotor.RunMode.RUN_WITHOUT_ENCODER); + rearIntakeMotor.setMode(DcMotor.RunMode.RUN_WITHOUT_ENCODER); velocityPIDFController = new PIDFController(IntakeConstants.kP, IntakeConstants.kI, IntakeConstants.kD, IntakeConstants.kF); velocityFeedforward = new SimpleMotorFeedforward(IntakeConstants.kS, IntakeConstants.kV, IntakeConstants.kA); @@ -40,8 +42,10 @@ public Intake(HardwareMap hMap, TelemetryManager telemetryM) { @Override public void periodic() { - telemetryM.addData(IntakeConstants.kSubsystemName + "Current Velocity", getVelocity()); - telemetryM.addData(IntakeConstants.kSubsystemName + "Current Open Loop", intakeMotor.getPower()); + telemetryM.addData("Front " + IntakeConstants.kSubsystemName + "Current Velocity", getFrontVelocity()); + telemetryM.addData("Front " + IntakeConstants.kSubsystemName + "Current Open Loop", frontIntakeMotor.getPower()); + telemetryM.addData("Rear " + IntakeConstants.kSubsystemName + "Current Velocity", getRearVelocity()); + telemetryM.addData("Rear " + IntakeConstants.kSubsystemName + "Current Open Loop", rearIntakeMotor.getPower()); } public void setVelocitySetpoint(double targetRPM) { @@ -54,17 +58,38 @@ public void setVelocitySetpoint(double targetRPM) { velocityFeedforward.setCoefficient(IntakeConstants.kS, IntakeConstants.kV, IntakeConstants.kA); } - frontIntakeMotor.setPower(velocityPIDFController.calculate(getVelocity(), targetRPM) + velocityFeedforward.calculate(targetRPM)); - intakeMotor.setPower(velocityPIDFController.calculate(getVelocity(), targetRPM) + velocityFeedforward.calculate(targetRPM)); + frontIntakeMotor.setPower(velocityPIDFController.calculate(getFrontVelocity(), targetRPM) + velocityFeedforward.calculate(targetRPM)); + rearIntakeMotor.setPower(velocityPIDFController.calculate(getRearVelocity(), targetRPM) + velocityFeedforward.calculate(targetRPM)); } - public void setOpenLoopSetpoint(double speed) { - telemetryM.addData(IntakeConstants.kSubsystemName + "Setpoint Open Loop", speed); - intakeMotor.setPower(speed); + public void setFrontMotorOpenLoop(double speed) { + telemetryM.addData("Front" + IntakeConstants.kSubsystemName + "Setpoint Open Loop", speed); frontIntakeMotor.setPower(speed); } - public double getVelocity() { - return (intakeMotor.getVelocity() / IntakeConstants.intakeMotorCPR) * 60; + public void setRearMotorOpenLoop(double speed) { + telemetryM.addData("Rear" + IntakeConstants.kSubsystemName + "Setpoint Open Loop", speed); + rearIntakeMotor.setPower(speed); + } + + + public void setOpenLoopSetpoint(double speed) { + setFrontMotorOpenLoop(speed); + setRearMotorOpenLoop(speed); + } + + public void setEqualOpenLoopSetpoint(double rearSpeed) { + double frontSpeed = rearSpeed; + + setFrontMotorOpenLoop(frontSpeed); + setRearMotorOpenLoop(rearSpeed); + } + + public double getFrontVelocity() { + return ((frontIntakeMotor.getVelocity() / IntakeConstants.intakeMotorCPR) * IntakeConstants.frontIntakeRatio) * 60; + } + + public double getRearVelocity() { + return ((rearIntakeMotor.getVelocity() / IntakeConstants.intakeMotorCPR) * IntakeConstants.rearIntakeRatio) * 60; } } diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystems/Transfer.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystems/Transfer.java index fc36a09..98a5873 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystems/Transfer.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystems/Transfer.java @@ -22,6 +22,9 @@ public class Transfer extends SubsystemBase { @IgnoreConfigurable static TelemetryManager telemetryM; + private boolean isBlockerEngaged = true; + private boolean isKickerEngaged = false; + public Transfer(HardwareMap hMap, TelemetryManager telemetryM) { kickerServo = hMap.get(Servo.class, TransferConstants.kickerServoID); blockerServo = hMap.get(Servo.class, TransferConstants.blockerServoID); @@ -45,8 +48,15 @@ public void periodic() { } public void onInitialization(boolean initKicker, boolean initBlocker) { - if(initKicker) kickerServo.setPosition(TransferConstants.kickerIdlePosition); - if(initBlocker) blockerServo.setPosition(TransferConstants.blockerIdlePosition); + if(initKicker) { + kickerServo.setPosition(TransferConstants.kickerIdlePosition); + isKickerEngaged = false; + } + + if(initBlocker) { + blockerServo.setPosition(TransferConstants.blockerIdlePosition); + isBlockerEngaged = true; + } } public void setKickerPosition(double position) { @@ -68,4 +78,12 @@ public double firstCSDistance() { public double secondCSDistance() { return secondColorSensor.getDistance(DistanceUnit.INCH); } + + public boolean isBlockerEngaged() { + return isBlockerEngaged; + } + + public boolean isKickerEngaged() { + return isKickerEngaged; + } } From 1b99ed002009ed22c892bbd25b0d756c4afa4b50 Mon Sep 17 00:00:00 2001 From: ultimatehecker <86735991+ultimatehecker@users.noreply.github.com> Date: Thu, 29 Jan 2026 15:16:17 -0500 Subject: [PATCH 4/4] led commands --- .../ftc/teamcode/command_factories/LEDFactory.java | 14 +++++++++++++- 1 file changed, 13 insertions(+), 1 deletion(-) diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/command_factories/LEDFactory.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/command_factories/LEDFactory.java index 3e24b0e..8b1492d 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/command_factories/LEDFactory.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/command_factories/LEDFactory.java @@ -14,10 +14,22 @@ import java.util.function.LongSupplier; public class LEDFactory { - public static Command constantColorCommand(LED led, LEDConstants.ColorValue setpoint) { + public static Command setConstantColorCommand(LED led, LEDConstants.ColorValue setpoint) { return Commands.runOnce(() -> { led.setSolid(setpoint); }); } + + public static Command setStandardBlinkingCommand(LED led, LEDConstants.ColorValue setpoint, LongSupplier intervalMs) { + return Commands.runOnce(() -> { + led.setDefaultSimpleBlink(setpoint, intervalMs.getAsLong()); + }); + } + + public static Command setAdvancedStandardBlinkingCommand(LED led, LEDConstants.ColorValue setpointA, LEDConstants.ColorValue setpointB, LongSupplier intervalMs) { + return Commands.runOnce(() -> { + led.setSimpleBlink(setpointA, setpointB, intervalMs.getAsLong()); + }); + } }