From a06b44edf7cea9c68b1988ed70b14510267f2475 Mon Sep 17 00:00:00 2001 From: Tyler Wilcox Date: Sun, 29 Mar 2026 14:30:48 -0400 Subject: [PATCH] Shooting is working --- src/main/java/frc/robot/Constants.java | 2 +- src/main/java/frc/robot/RobotContainer.java | 5 ++++ .../AutoShootCommandChassisTurret.java | 2 +- .../frc/robot/subsystems/TurretSubsystem.java | 30 +++++++++++++++---- 4 files changed, 32 insertions(+), 7 deletions(-) diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java index 0737efd..9d9365f 100644 --- a/src/main/java/frc/robot/Constants.java +++ b/src/main/java/frc/robot/Constants.java @@ -117,7 +117,7 @@ public final class Constants { public static class HopperConstants { public static final int KICKER_MOTOR_ID = 9; - public static final InvertedValue KICKER_MOTOR_DIRECTION = InvertedValue.CounterClockwise_Positive; + public static final InvertedValue KICKER_MOTOR_DIRECTION = InvertedValue.Clockwise_Positive; public static final int SPINDEXER_MOTOR_ID = 8; public static final InvertedValue SPINDEXER_MOTOR_DIRECTION = InvertedValue.CounterClockwise_Positive; diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 1a2646d..e0bd403 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -70,6 +70,11 @@ public class RobotContainer { // m_driverController.x().onTrue(intakeSubsystem.killSpeedCommand()); // m_driverController.a().onTrue(intakeSubsystem.startSpeedCommand()); + + + m_driverController.b().whileTrue(turretSubsystem.setShooter(1500)); + m_driverController.x().whileTrue(turretSubsystem.setShooter(2000)); + m_driverController.y().whileTrue(turretSubsystem.setShooter(3000)); m_driverController.a().whileTrue(new AutoShootCommandChassisTurret(driveSubsystem, hopperSubsystem, turretSubsystem)); } diff --git a/src/main/java/frc/robot/commands/AutoShootCommandChassisTurret.java b/src/main/java/frc/robot/commands/AutoShootCommandChassisTurret.java index fc9a56b..d30d6fc 100644 --- a/src/main/java/frc/robot/commands/AutoShootCommandChassisTurret.java +++ b/src/main/java/frc/robot/commands/AutoShootCommandChassisTurret.java @@ -74,7 +74,7 @@ public class AutoShootCommandChassisTurret extends Command{ } }); - turretSubsystem.setLauncher(2500); + turretSubsystem.setLauncher(5500); hopperSubsystem.spindexerOn(); cameraAutoCenterController.calculate(-100); diff --git a/src/main/java/frc/robot/subsystems/TurretSubsystem.java b/src/main/java/frc/robot/subsystems/TurretSubsystem.java index 6198ada..fae68ca 100644 --- a/src/main/java/frc/robot/subsystems/TurretSubsystem.java +++ b/src/main/java/frc/robot/subsystems/TurretSubsystem.java @@ -20,6 +20,7 @@ import com.ctre.phoenix6.signals.InvertedValue; import com.revrobotics.PersistMode; import com.revrobotics.ResetMode; import com.revrobotics.spark.ClosedLoopSlot; +import com.revrobotics.spark.FeedbackSensor; import com.revrobotics.spark.SparkClosedLoopController; import com.revrobotics.spark.SparkMax; import com.revrobotics.spark.SparkBase.ControlType; @@ -33,6 +34,7 @@ import edu.wpi.first.units.measure.Angle; import edu.wpi.first.wpilibj.DigitalInput; import edu.wpi.first.wpilibj.Servo; import edu.wpi.first.wpilibj2.command.Command; +import edu.wpi.first.wpilibj2.command.Commands; public class TurretSubsystem extends TestableSubsystem { public enum TurretState { @@ -81,8 +83,14 @@ public class TurretSubsystem extends TestableSubsystem { SparkMaxConfig config = new SparkMaxConfig() .apply((SparkMaxConfig) SparkMaxConfig.Presets.REV_NEO); config.inverted(Constants.ShooterConstants.LAUNCHER_MOTOR_INVERTED); - config.closedLoop.p(1).i(0).d(0.1); - config.closedLoop.allowedClosedLoopError(10, ClosedLoopSlot.kSlot0); + config.voltageCompensation(10); + config.closedLoop.p(0.0001).i(0).d(0.0); // I = 0.0000001 + config.closedLoop.feedForward.kS(0.2).kV(0.002).kA(0.00); + // config.closedLoop.allowedClosedLoopError(10, ClosedLoopSlot.kSlot0); + config.closedLoop.feedbackSensor(FeedbackSensor.kPrimaryEncoder); + config.closedLoop.positionWrappingEnabled(false); + // config.closedLoopRampRate(0.5); + // config.smartCurrentLimit(5, 50); launcherMotor.configure(config, ResetMode.kResetSafeParameters, PersistMode.kPersistParameters); launcherControler = launcherMotor.getClosedLoopController(); @@ -94,10 +102,13 @@ public class TurretSubsystem extends TestableSubsystem { turretGoingPos = false; } - public void setLauncher(double speed) {launcherControler.setSetpoint(speed, ControlType.kVelocity);} + public void setLauncher(double speed) { + launcherControler.setSetpoint(speed, ControlType.kVelocity, ClosedLoopSlot.kSlot0); + // launcherMotor.set(speed/1000); + } public boolean isLauncherUpToSpeed() { - return launcherControler.isAtSetpoint(); + return true; } public void turretControl(double position) @@ -117,6 +128,14 @@ public class TurretSubsystem extends TestableSubsystem { return this.runOnce(() -> setLauncher(-.5)); } + public Command setShooter(int speedRPM){ + return Commands.runEnd(() -> { + setLauncher(speedRPM); + }, () -> { + launcherMotor.stopMotor(); + }, this); + } + public void startHoming() { //turretMotor.set(Constants.ShooterConstants.HOMING_SPEED); @@ -164,7 +183,8 @@ public class TurretSubsystem extends TestableSubsystem { if(LOGGING) { - Logger.recordOutput("Launcher Speed (RPM)", launcherMotor.getEncoder().getVelocity()); + Logger.recordOutput("TurretSubsystem-Launcher Speed (RPM)", launcherMotor.getEncoder().getVelocity()); + Logger.recordOutput("TurretSubsystem-Launcher Volatge", launcherMotor.getOutputCurrent()); } }