From df5b432267d3298c34af48f8dd4be8fe72bfd5e7 Mon Sep 17 00:00:00 2001 From: WesleyWong-972 Date: Fri, 27 Mar 2026 16:52:50 -0700 Subject: [PATCH] turret enable --- .../java/frc/robot/subsystems/Intake/Intake.java | 2 ++ .../java/frc/robot/subsystems/turret/Turret.java | 13 +++++++++---- .../robot/subsystems/turret/TurretConstants.java | 2 +- 3 files changed, 12 insertions(+), 5 deletions(-) diff --git a/src/main/java/frc/robot/subsystems/Intake/Intake.java b/src/main/java/frc/robot/subsystems/Intake/Intake.java index 35ae912..c0d7364 100644 --- a/src/main/java/frc/robot/subsystems/Intake/Intake.java +++ b/src/main/java/frc/robot/subsystems/Intake/Intake.java @@ -202,6 +202,8 @@ public class Intake extends SubsystemBase implements IntakeIO{ SmartDashboard.putBoolean("Intake Calibrated", !calibrating); SmartDashboard.putBoolean("Intake At Setpoint", Math.abs(inchExtension - setpointInches) < 0.5); + SmartDashboard.putData("Extend Intake", new InstantCommand(() -> extend())); + SmartDashboard.putData("Retract Intake", new InstantCommand(() -> retract())); } public void simulationPeriodic(){ diff --git a/src/main/java/frc/robot/subsystems/turret/Turret.java b/src/main/java/frc/robot/subsystems/turret/Turret.java index 64177da..d268956 100644 --- a/src/main/java/frc/robot/subsystems/turret/Turret.java +++ b/src/main/java/frc/robot/subsystems/turret/Turret.java @@ -105,6 +105,7 @@ public class Turret extends SubsystemBase implements TurretIO{ SmartDashboard.putData("Turret Mech", mech); SmartDashboard.putData("Start turret calibration", new InstantCommand(()-> calibrate())); SmartDashboard.putData("Stop turret calibration", new InstantCommand(()-> stopCalibrating())); + SmartDashboard.putData("Reset Turret Position to Zero", new InstantCommand(() -> resetTurretPosition())); SendableChooser turretTestChooser = new SendableChooser<>(); turretTestChooser.setDefaultOption("Turn to 0", new InstantCommand(()-> setFieldRelativeTarget(Rotation2d.fromDegrees(0), 0.0))); @@ -130,6 +131,10 @@ public class Turret extends SubsystemBase implements TurretIO{ goalVelocityRadPerSec = velocityRadPerSec; } + public void resetTurretPosition() { + inputs.positionDeg = 0.0; + } + /** * @return If the turret is at setpoint with tolerance of 10 degrees */ @@ -201,16 +206,16 @@ public class Turret extends SubsystemBase implements TurretIO{ double robotTurnCompensation = goalVelocityRadPerSec * TurretConstants.FEEDFORWARD_KV; if(calibrating){ - // motor.set(0.05); + motor.set(0.05); boolean calibrated = Math.abs(motor.getStatorCurrent().getValueAsDouble()) >= TurretConstants.CALIBRATION_CURRENT_THRESHOLD; if(calibrationDebouncer.calculate(calibrated)){ stopCalibrating(); } } else { // Sets motor control with feedforward - // motor.setControl(mmVoltageRequest - // .withPosition(motorGoalRotations) - // .withFeedForward(robotTurnCompensation)); + motor.setControl(mmVoltageRequest + .withPosition(motorGoalRotations) + .withFeedForward(robotTurnCompensation)); } Logger.recordOutput("Turret/Voltage", motor.getMotorVoltage().getValue()); diff --git a/src/main/java/frc/robot/subsystems/turret/TurretConstants.java b/src/main/java/frc/robot/subsystems/turret/TurretConstants.java index 5dac346..838dbb4 100644 --- a/src/main/java/frc/robot/subsystems/turret/TurretConstants.java +++ b/src/main/java/frc/robot/subsystems/turret/TurretConstants.java @@ -26,7 +26,7 @@ public class TurretConstants { public static final double FEEDFORWARD_KV = 0.185; - public static final double NORMAL_CURRENT_LIMIT = 40.0; // A + public static final double NORMAL_CURRENT_LIMIT = 5.0; // A public static final double CALIBRATION_CURRENT_LIMIT = 10.0; // A public static final double CALIBRATION_CURRENT_THRESHOLD = 9.0; // A -- 2.39.5