From c4e7cabcceee92735cd9914f61826be74725d789 Mon Sep 17 00:00:00 2001 From: Nick Date: Tue, 11 Nov 2025 18:40:53 -0600 Subject: [PATCH] Added New Ramp Motor. Started Work on New Speed Functions. --- .../firstinspires/ftc/teamcode/Constants.java | 3 ++- .../ftc/teamcode/OpModes/BackupOpMode.java | 2 +- .../ftc/teamcode/OpModes/PrimaryOpMode.java | 2 +- .../ftc/teamcode/commands/RunShooter.java | 2 +- .../ftc/teamcode/subsystems/Shooter.java | 24 +++++++++++-------- 5 files changed, 19 insertions(+), 14 deletions(-) diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Constants.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Constants.java index 24ed3c7cddcc..bb7a2a8f22f8 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Constants.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Constants.java @@ -10,7 +10,8 @@ public static class Drivetrain { public static class Shooter { public static final String shooterMotorName = "shooterMotor"; - public static final String rampMotorName = "rampMotor"; + public static final String rampMotorOneName = "rampMotorOne"; + public static final String rampMotorTwoName = "rampMotorTwo"; public static final double shooterSpeed = 1; public static final double rampSpeed = 0.5; } diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/OpModes/BackupOpMode.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/OpModes/BackupOpMode.java index 273850e0ebe6..952819913fca 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/OpModes/BackupOpMode.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/OpModes/BackupOpMode.java @@ -34,7 +34,7 @@ public void initialize() { drivetrain.setDefaultCommand(new UserDrive(drivetrain, driverOp)); register(drivetrain); - shooter = new Shooter(hardwareMap, Constants.Shooter.shooterMotorName, Constants.Shooter.rampMotorName); + shooter = new Shooter(hardwareMap, Constants.Shooter.shooterMotorName, Constants.Shooter.rampMotorOneName, Constants.Shooter.rampMotorTwoName); register(shooter); coOp.getGamepadButton(GamepadKeys.Button.A).whileHeld( new RunShooter(shooter, Constants.Shooter.shooterSpeed, Constants.Shooter.rampSpeed)); diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/OpModes/PrimaryOpMode.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/OpModes/PrimaryOpMode.java index fd994f7cd70a..77b5c3e44524 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/OpModes/PrimaryOpMode.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/OpModes/PrimaryOpMode.java @@ -36,7 +36,7 @@ public void initialize() { ); drivetrain.setDefaultCommand(new UserDrive(drivetrain, driverOp)); register(drivetrain); - shooter = new Shooter(hardwareMap, Constants.Shooter.shooterMotorName, Constants.Shooter.rampMotorName); + shooter = new Shooter(hardwareMap, Constants.Shooter.shooterMotorName, Constants.Shooter.rampMotorOneName, Constants.Shooter.rampMotorTwoName); register(shooter); intake = new Intake(hardwareMap, Constants.Intake.intakeMotorName); register(intake); diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/commands/RunShooter.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/commands/RunShooter.java index 090e6ed381f0..1c5703493957 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/commands/RunShooter.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/commands/RunShooter.java @@ -17,7 +17,7 @@ public RunShooter(Shooter _subsystem, double _shooterSpeed, double _rampSpeed) { @Override public void initialize() { - subsystem.run(shooterSpeed, rampSpeed); + subsystem.runBase(shooterSpeed, rampSpeed); } @Override diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystems/Shooter.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystems/Shooter.java index d474515887d9..4f3e1ec6d192 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystems/Shooter.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystems/Shooter.java @@ -5,25 +5,29 @@ import com.qualcomm.robotcore.hardware.HardwareMap; public class Shooter extends SubsystemBase { - private final Motor shooterMotor, rampMotor; + private final Motor shooterMotor, rampMotorOne, rampMotorTwo; - public Shooter(HardwareMap map, String shooterMotorName, String rampMotorName) { + public Shooter(HardwareMap map, String shooterMotorName, String rampMotorOneName, String rampMotorTwoName) { shooterMotor = new Motor(map, shooterMotorName); shooterMotor.setRunMode(Motor.RunMode.RawPower); - rampMotor = new Motor(map, rampMotorName); - rampMotor.setRunMode(Motor.RunMode.RawPower); + rampMotorOne = new Motor(map, rampMotorOneName); + rampMotorOne.setRunMode(Motor.RunMode.RawPower); + rampMotorTwo = new Motor(map, rampMotorTwoName); + rampMotorTwo.setRunMode(Motor.RunMode.RawPower); } - public void run(double shooterSpeed, double rampSpeed) { + public void runBase(double shooterSpeed, double rampSpeedOne) { // Clamp speed to valid motor range [-1.0, 1.0] - double clampedSpeed = Math.max(-1.0, Math.min(1.0, shooterSpeed)); - double _rampSpeed = Math.max(-1.0, Math.min(1.0, rampSpeed)); - shooterMotor.set(clampedSpeed); - rampMotor.set(_rampSpeed); + double baseSpeed = Math.max(-1.0, Math.min(1.0, shooterSpeed)); + double _rampSpeed = Math.max(-0.5, Math.min(0.5, rampSpeedOne)); + shooterMotor.set(baseSpeed); + rampMotorOne.set(_rampSpeed); + rampMotorTwo.set(_rampSpeed * -1.0); } public void stop() { shooterMotor.stopMotor(); - rampMotor.stopMotor(); + rampMotorOne.stopMotor(); + rampMotorTwo.stopMotor(); } }