From 73ba3bbfd40dc6aa0d13a03ea954e5fce4a97b3f Mon Sep 17 00:00:00 2001 From: EJain-Dev Date: Mon, 27 Oct 2025 20:31:41 -0400 Subject: [PATCH 01/11] Started to setup turning to target. --- src/main/java/frc/robot/RobotState.java | 13 +++++++++++++ src/main/java/frc/robot/subsystems/drive/Drive.java | 2 ++ .../java/frc/robot/subsystems/vision/Vision.java | 2 ++ 3 files changed, 17 insertions(+) create mode 100644 src/main/java/frc/robot/RobotState.java diff --git a/src/main/java/frc/robot/RobotState.java b/src/main/java/frc/robot/RobotState.java new file mode 100644 index 0000000..048f2a2 --- /dev/null +++ b/src/main/java/frc/robot/RobotState.java @@ -0,0 +1,13 @@ +package frc.robot; + +public class RobotState { + static private RobotState state; + public double vision_yaw; + + public static void init() { + } + + public static RobotState get() { + return state; + } +} diff --git a/src/main/java/frc/robot/subsystems/drive/Drive.java b/src/main/java/frc/robot/subsystems/drive/Drive.java index 21b398e..ba8c567 100644 --- a/src/main/java/frc/robot/subsystems/drive/Drive.java +++ b/src/main/java/frc/robot/subsystems/drive/Drive.java @@ -7,6 +7,8 @@ import edu.wpi.first.wpilibj2.command.Commands; import java.util.function.BooleanSupplier; import java.util.function.DoubleSupplier; + +import frc.robot.RobotState; import org.littletonrobotics.junction.Logger; public class Drive extends SubsystemBase { diff --git a/src/main/java/frc/robot/subsystems/vision/Vision.java b/src/main/java/frc/robot/subsystems/vision/Vision.java index 75d853d..481de61 100644 --- a/src/main/java/frc/robot/subsystems/vision/Vision.java +++ b/src/main/java/frc/robot/subsystems/vision/Vision.java @@ -1,6 +1,7 @@ package frc.robot.subsystems.vision; import edu.wpi.first.wpilibj2.command.SubsystemBase; +import frc.robot.RobotState; import org.littletonrobotics.junction.Logger; public class Vision extends SubsystemBase { @@ -21,6 +22,7 @@ public Vision( VisionIO io) { public void periodic() { io.updateInputs(inputs); Logger.processInputs("Vision", inputs); + RobotState.get().vision_yaw = inputs.yaw; } public double distanceFromTarget(){ From b940225c678a8ebd26cb33021b2c614a0cbfa013 Mon Sep 17 00:00:00 2001 From: EJain-Dev Date: Mon, 27 Oct 2025 20:50:59 -0400 Subject: [PATCH 02/11] Added version of rotating to target. --- src/main/java/frc/robot/Orchestrator.java | 15 +++++++++- src/main/java/frc/robot/RobotContainer.java | 10 ++++--- .../frc/robot/subsystems/drive/Drive.java | 8 +++++ .../subsystems/drive/DriveConstants.java | 2 +- .../frc/robot/subsystems/drive/DriveIO.java | 3 +- .../subsystems/drive/DriveIOSparkMax.java | 30 +++++++++++++++++++ .../subsystems/shooter/ShooterIOSparkMax.java | 1 + .../frc/robot/subsystems/vision/Vision.java | 6 +++- 8 files changed, 66 insertions(+), 9 deletions(-) diff --git a/src/main/java/frc/robot/Orchestrator.java b/src/main/java/frc/robot/Orchestrator.java index f1d1b2d..0e49d1d 100644 --- a/src/main/java/frc/robot/Orchestrator.java +++ b/src/main/java/frc/robot/Orchestrator.java @@ -4,18 +4,24 @@ import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.Commands; +import frc.robot.subsystems.drive.Drive; import frc.robot.subsystems.indexer.Indexer; import frc.robot.subsystems.shooter.Shooter; import frc.robot.subsystems.shooter.ShooterConstants; +import frc.robot.subsystems.vision.Vision; public class Orchestrator { private final Indexer indexer; private final Shooter shooter; + private final Vision vision; + private final Drive drive; - public Orchestrator(Indexer indexer, Shooter shooter) { + public Orchestrator(Indexer indexer, Shooter shooter, Vision vision, Drive drive) { this.indexer = indexer; this.shooter = shooter; + this.vision = vision; + this.drive = drive; } public Command spinUp(BooleanSupplier fastMode) { @@ -51,6 +57,13 @@ public Command shootCycle(BooleanSupplier fastMode) { .repeatedly()); } + public Command rotateToTag() { + return Commands.parallel( + vision.updateYaw(), + drive.toAprilTag() + ); + } + public Command reverseAll() { return Commands.parallel(shooter.reverse(), indexer.reverse()); } diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 5274408..6f66929 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -34,7 +34,7 @@ public class RobotContainer { private final Vision vision; private final Indexer indexer = new Indexer(new IndexerIOSparkMax()); private final Shooter shooter = new Shooter(new ShooterIOSparkMax()); - private final Orchestrator orchestrator = new Orchestrator(indexer, shooter); + private final Orchestrator orchestrator; private final CommandXboxController driverController = new CommandXboxController(0); private boolean fastMode = false; private final Trigger fastModeTrigger = new Trigger(() -> fastMode); @@ -45,6 +45,7 @@ public RobotContainer() { // Configure the trigger bindings drive = new Drive(new DriveIOSparkMax()); vision = new Vision(new VisionIOPhotonVision("VGA_USB_Camera"){}); + orchestrator = new Orchestrator(indexer, shooter, vision, drive); configureBindings(); } @@ -67,9 +68,9 @@ private void configureBindings() { () -> driverController.getHID() .setRumble(edu.wpi.first.wpilibj.GenericHID.RumbleType.kBothRumble, 0.0) )); - drive.setDefaultCommand(drive.arcadeDrive( - driverController::getLeftY, - driverController::getRightY)); + drive.setDefaultCommand(drive.arcadeDrive( + driverController::getLeftY, + driverController::getRightY)); driverController .rightTrigger() @@ -85,6 +86,7 @@ private void configureBindings() { .rightBumper() .onTrue(orchestrator.shootOnce(() -> fastMode)) .onFalse(orchestrator.stopAll()); + driverController.x().whileTrue(orchestrator.rotateToTag()); } /** diff --git a/src/main/java/frc/robot/subsystems/drive/Drive.java b/src/main/java/frc/robot/subsystems/drive/Drive.java index ba8c567..467d29f 100644 --- a/src/main/java/frc/robot/subsystems/drive/Drive.java +++ b/src/main/java/frc/robot/subsystems/drive/Drive.java @@ -32,4 +32,12 @@ public Command arcadeDrive(DoubleSupplier speedX, DoubleSupplier rotation) { io.setVoltageRight(speeds.right * 7); }, this); } + + public Command toAprilTag() { + return Commands.run( + () -> { + io.rotateToTag();; + }, this + ); + } } diff --git a/src/main/java/frc/robot/subsystems/drive/DriveConstants.java b/src/main/java/frc/robot/subsystems/drive/DriveConstants.java index 4d7114a..5b45b05 100644 --- a/src/main/java/frc/robot/subsystems/drive/DriveConstants.java +++ b/src/main/java/frc/robot/subsystems/drive/DriveConstants.java @@ -2,6 +2,6 @@ public class DriveConstants { - + public static final int RPM_PER_RAD = 1; } \ No newline at end of file diff --git a/src/main/java/frc/robot/subsystems/drive/DriveIO.java b/src/main/java/frc/robot/subsystems/drive/DriveIO.java index 71aa206..c5f161e 100644 --- a/src/main/java/frc/robot/subsystems/drive/DriveIO.java +++ b/src/main/java/frc/robot/subsystems/drive/DriveIO.java @@ -26,6 +26,5 @@ default void setVelocityRadPerSecL(double leftVelocityRadPerSec){} default void setVelocityRadPerSecR(double rightVelocityRadPerSec){} - - + default public void rotateToTag() {} } diff --git a/src/main/java/frc/robot/subsystems/drive/DriveIOSparkMax.java b/src/main/java/frc/robot/subsystems/drive/DriveIOSparkMax.java index e8a7ead..cc05f69 100644 --- a/src/main/java/frc/robot/subsystems/drive/DriveIOSparkMax.java +++ b/src/main/java/frc/robot/subsystems/drive/DriveIOSparkMax.java @@ -5,8 +5,14 @@ import com.revrobotics.spark.SparkClosedLoopController; import com.revrobotics.spark.SparkLowLevel; import com.revrobotics.spark.SparkMax; +import com.revrobotics.spark.config.ClosedLoopConfig; +import com.revrobotics.spark.config.EncoderConfig; import com.revrobotics.spark.config.SparkBaseConfig; import com.revrobotics.spark.config.SparkMaxConfig; +import frc.robot.RobotState; + +import static com.revrobotics.spark.config.ClosedLoopConfig.FeedbackSensor.kPrimaryEncoder; +import static frc.robot.subsystems.drive.DriveConstants.RPM_PER_RAD; public class DriveIOSparkMax implements DriveIO{ private final SparkMax leftLeader; @@ -17,6 +23,8 @@ public class DriveIOSparkMax implements DriveIO{ private final RelativeEncoder leftLeaderEncoder; private final RelativeEncoder rightLeaderEncoder; + private SparkClosedLoopController leftLeaderController; + private SparkClosedLoopController rightLeaderController; public DriveIOSparkMax(){ leftLeader = new SparkMax(3, SparkLowLevel.MotorType.kBrushless); @@ -26,6 +34,9 @@ public DriveIOSparkMax(){ rightFollower = new SparkMax(6, SparkLowLevel.MotorType.kBrushless); rightLeaderEncoder = rightLeader.getEncoder(); + leftLeaderController = leftLeader.getClosedLoopController(); + rightLeaderController = rightLeader.getClosedLoopController(); + var LeftLeaderConfig = new SparkMaxConfig(); LeftLeaderConfig.inverted(false) .idleMode(SparkBaseConfig.IdleMode.kBrake) @@ -51,6 +62,20 @@ public DriveIOSparkMax(){ .voltageCompensation(12) .smartCurrentLimit(60) .follow(5, false); + + var loopConfig = new ClosedLoopConfig(); + loopConfig + .feedbackSensor(kPrimaryEncoder) + .positionWrappingEnabled(false) + .pid(0.000009,0.0000003,0.0001);// TODO: tune PIDF values + +// EncoderConfig enc = new EncoderConfig(); +// enc.velocityConversionFactor(RPM_TO_RADS); +// config.apply(enc); +// config.apply(loopConfig); + + LeftLeaderConfig.apply(loopConfig); + RightLeaderConfig.apply(loopConfig); leftLeader.configure(LeftLeaderConfig, SparkBase.ResetMode.kResetSafeParameters, SparkBase.PersistMode.kPersistParameters); leftFollower.configure(LeftFollowerConfig, SparkBase.ResetMode.kResetSafeParameters, SparkBase.PersistMode.kPersistParameters); rightLeader.configure(RightLeaderConfig, SparkBase.ResetMode.kResetSafeParameters, SparkBase.PersistMode.kPersistParameters); @@ -88,4 +113,9 @@ public void setVelocityRadPerSecR(double rightVelocityRadPerSec){ rightLeader.set(rightVelocityRadPerSec); } + @Override + public void rotateToTag() { + leftLeaderController.setReference(RobotState.get().vision_yaw * RPM_PER_RAD, SparkBase.ControlType.kVelocity); + rightLeaderController.setReference(RobotState.get().vision_yaw * RPM_PER_RAD, SparkBase.ControlType.kVelocity); + } } diff --git a/src/main/java/frc/robot/subsystems/shooter/ShooterIOSparkMax.java b/src/main/java/frc/robot/subsystems/shooter/ShooterIOSparkMax.java index 70c2267..e250efb 100644 --- a/src/main/java/frc/robot/subsystems/shooter/ShooterIOSparkMax.java +++ b/src/main/java/frc/robot/subsystems/shooter/ShooterIOSparkMax.java @@ -5,6 +5,7 @@ import com.revrobotics.RelativeEncoder; import com.revrobotics.spark.SparkBase.PersistMode; import com.revrobotics.spark.SparkBase.ResetMode; +import com.revrobotics.spark.SparkClosedLoopController; import com.revrobotics.spark.SparkLimitSwitch; import com.revrobotics.spark.SparkLowLevel.MotorType; import com.revrobotics.spark.SparkMax; diff --git a/src/main/java/frc/robot/subsystems/vision/Vision.java b/src/main/java/frc/robot/subsystems/vision/Vision.java index 481de61..99812b8 100644 --- a/src/main/java/frc/robot/subsystems/vision/Vision.java +++ b/src/main/java/frc/robot/subsystems/vision/Vision.java @@ -1,5 +1,7 @@ package frc.robot.subsystems.vision; +import edu.wpi.first.wpilibj2.command.Command; +import edu.wpi.first.wpilibj2.command.Commands; import edu.wpi.first.wpilibj2.command.SubsystemBase; import frc.robot.RobotState; import org.littletonrobotics.junction.Logger; @@ -22,9 +24,11 @@ public Vision( VisionIO io) { public void periodic() { io.updateInputs(inputs); Logger.processInputs("Vision", inputs); - RobotState.get().vision_yaw = inputs.yaw; } + public Command updateYaw() { + return Commands.runOnce(() -> {RobotState.get().vision_yaw = inputs.yaw;}, this); + } public double distanceFromTarget(){ return inputs.distanceFromTarget; } From 99dfd478f4db8bedc5ec63a2f046e8adf6ae6ab6 Mon Sep 17 00:00:00 2001 From: Ekansh Jain Date: Mon, 27 Oct 2025 21:49:25 -0400 Subject: [PATCH 03/11] Initialize RobotState in static block --- src/main/java/frc/robot/RobotState.java | 7 ++++++- 1 file changed, 6 insertions(+), 1 deletion(-) diff --git a/src/main/java/frc/robot/RobotState.java b/src/main/java/frc/robot/RobotState.java index 048f2a2..6dd8e07 100644 --- a/src/main/java/frc/robot/RobotState.java +++ b/src/main/java/frc/robot/RobotState.java @@ -4,7 +4,12 @@ public class RobotState { static private RobotState state; public double vision_yaw; - public static void init() { + static { + state = new RobotState(); + } + + public RobotState() { + vision_yaw = 0.0; } public static RobotState get() { From e6422be72df88f4de8d4614fb71dd3ebfe80079b Mon Sep 17 00:00:00 2001 From: Ekansh Jain Date: Mon, 27 Oct 2025 21:51:42 -0400 Subject: [PATCH 04/11] Fix turning function Co-authored-by: Copilot <175728472+Copilot@users.noreply.github.com> --- src/main/java/frc/robot/subsystems/drive/DriveIOSparkMax.java | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/main/java/frc/robot/subsystems/drive/DriveIOSparkMax.java b/src/main/java/frc/robot/subsystems/drive/DriveIOSparkMax.java index cc05f69..3e5c0fd 100644 --- a/src/main/java/frc/robot/subsystems/drive/DriveIOSparkMax.java +++ b/src/main/java/frc/robot/subsystems/drive/DriveIOSparkMax.java @@ -116,6 +116,6 @@ public void setVelocityRadPerSecR(double rightVelocityRadPerSec){ @Override public void rotateToTag() { leftLeaderController.setReference(RobotState.get().vision_yaw * RPM_PER_RAD, SparkBase.ControlType.kVelocity); - rightLeaderController.setReference(RobotState.get().vision_yaw * RPM_PER_RAD, SparkBase.ControlType.kVelocity); + rightLeaderController.setReference(-RobotState.get().vision_yaw * RPM_PER_RAD, SparkBase.ControlType.kVelocity); } } From f70046aa8b9cbfa58997a76ac22f64288cbfc521 Mon Sep 17 00:00:00 2001 From: Ekansh Jain Date: Mon, 27 Oct 2025 21:52:09 -0400 Subject: [PATCH 05/11] Format Co-authored-by: Copilot <175728472+Copilot@users.noreply.github.com> --- src/main/java/frc/robot/RobotState.java | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/main/java/frc/robot/RobotState.java b/src/main/java/frc/robot/RobotState.java index 6dd8e07..308c4e1 100644 --- a/src/main/java/frc/robot/RobotState.java +++ b/src/main/java/frc/robot/RobotState.java @@ -1,7 +1,7 @@ package frc.robot; public class RobotState { - static private RobotState state; + private static RobotState state; public double vision_yaw; static { From 0aa6c7cf79b25081fe618c9487de6b21fb53620f Mon Sep 17 00:00:00 2001 From: Ekansh Jain Date: Mon, 27 Oct 2025 21:52:40 -0400 Subject: [PATCH 06/11] Format Co-authored-by: Copilot <175728472+Copilot@users.noreply.github.com> --- src/main/java/frc/robot/subsystems/drive/Drive.java | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/main/java/frc/robot/subsystems/drive/Drive.java b/src/main/java/frc/robot/subsystems/drive/Drive.java index 467d29f..3f1b6e0 100644 --- a/src/main/java/frc/robot/subsystems/drive/Drive.java +++ b/src/main/java/frc/robot/subsystems/drive/Drive.java @@ -36,7 +36,7 @@ public Command arcadeDrive(DoubleSupplier speedX, DoubleSupplier rotation) { public Command toAprilTag() { return Commands.run( () -> { - io.rotateToTag();; + io.rotateToTag(); }, this ); } From 1acb6e70d9575d2a00087835a5bdca21ff03ee8b Mon Sep 17 00:00:00 2001 From: Ekansh Jain Date: Mon, 27 Oct 2025 21:54:24 -0400 Subject: [PATCH 07/11] Change rotateToTag to use sequence instead of parallel --- src/main/java/frc/robot/Orchestrator.java | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/src/main/java/frc/robot/Orchestrator.java b/src/main/java/frc/robot/Orchestrator.java index 0e49d1d..676eeb7 100644 --- a/src/main/java/frc/robot/Orchestrator.java +++ b/src/main/java/frc/robot/Orchestrator.java @@ -58,10 +58,10 @@ public Command shootCycle(BooleanSupplier fastMode) { } public Command rotateToTag() { - return Commands.parallel( + return Commands.sequence( vision.updateYaw(), drive.toAprilTag() - ); + ).repeatedly(); } public Command reverseAll() { From 7e9f6ce7f29496922b633c4f7b198abd84f6cb2a Mon Sep 17 00:00:00 2001 From: EJain-Dev Date: Tue, 28 Oct 2025 21:31:19 -0400 Subject: [PATCH 08/11] Changed PID calculation to be correct instead of using ClosedLoopController. --- src/main/java/frc/robot/RobotState.java | 2 +- .../subsystems/drive/DriveConstants.java | 4 ++- .../subsystems/drive/DriveIOSparkMax.java | 31 ++++--------------- 3 files changed, 10 insertions(+), 27 deletions(-) diff --git a/src/main/java/frc/robot/RobotState.java b/src/main/java/frc/robot/RobotState.java index 308c4e1..ef7add0 100644 --- a/src/main/java/frc/robot/RobotState.java +++ b/src/main/java/frc/robot/RobotState.java @@ -1,7 +1,7 @@ package frc.robot; public class RobotState { - private static RobotState state; + private static final RobotState state; public double vision_yaw; static { diff --git a/src/main/java/frc/robot/subsystems/drive/DriveConstants.java b/src/main/java/frc/robot/subsystems/drive/DriveConstants.java index 5b45b05..d8ffeef 100644 --- a/src/main/java/frc/robot/subsystems/drive/DriveConstants.java +++ b/src/main/java/frc/robot/subsystems/drive/DriveConstants.java @@ -2,6 +2,8 @@ public class DriveConstants { - public static final int RPM_PER_RAD = 1; + public static final double PID_P = 0.0; + public static final double PID_I = 0.0; + public static final double PID_D = 0.0; } \ No newline at end of file diff --git a/src/main/java/frc/robot/subsystems/drive/DriveIOSparkMax.java b/src/main/java/frc/robot/subsystems/drive/DriveIOSparkMax.java index 3e5c0fd..31a2a49 100644 --- a/src/main/java/frc/robot/subsystems/drive/DriveIOSparkMax.java +++ b/src/main/java/frc/robot/subsystems/drive/DriveIOSparkMax.java @@ -2,18 +2,13 @@ import com.revrobotics.RelativeEncoder; import com.revrobotics.spark.SparkBase; -import com.revrobotics.spark.SparkClosedLoopController; import com.revrobotics.spark.SparkLowLevel; import com.revrobotics.spark.SparkMax; -import com.revrobotics.spark.config.ClosedLoopConfig; -import com.revrobotics.spark.config.EncoderConfig; import com.revrobotics.spark.config.SparkBaseConfig; import com.revrobotics.spark.config.SparkMaxConfig; +import edu.wpi.first.math.controller.PIDController; import frc.robot.RobotState; -import static com.revrobotics.spark.config.ClosedLoopConfig.FeedbackSensor.kPrimaryEncoder; -import static frc.robot.subsystems.drive.DriveConstants.RPM_PER_RAD; - public class DriveIOSparkMax implements DriveIO{ private final SparkMax leftLeader; private final SparkMax leftFollower; @@ -23,8 +18,7 @@ public class DriveIOSparkMax implements DriveIO{ private final RelativeEncoder leftLeaderEncoder; private final RelativeEncoder rightLeaderEncoder; - private SparkClosedLoopController leftLeaderController; - private SparkClosedLoopController rightLeaderController; + private final PIDController pidController; public DriveIOSparkMax(){ leftLeader = new SparkMax(3, SparkLowLevel.MotorType.kBrushless); @@ -34,9 +28,6 @@ public DriveIOSparkMax(){ rightFollower = new SparkMax(6, SparkLowLevel.MotorType.kBrushless); rightLeaderEncoder = rightLeader.getEncoder(); - leftLeaderController = leftLeader.getClosedLoopController(); - rightLeaderController = rightLeader.getClosedLoopController(); - var LeftLeaderConfig = new SparkMaxConfig(); LeftLeaderConfig.inverted(false) .idleMode(SparkBaseConfig.IdleMode.kBrake) @@ -63,19 +54,8 @@ public DriveIOSparkMax(){ .smartCurrentLimit(60) .follow(5, false); - var loopConfig = new ClosedLoopConfig(); - loopConfig - .feedbackSensor(kPrimaryEncoder) - .positionWrappingEnabled(false) - .pid(0.000009,0.0000003,0.0001);// TODO: tune PIDF values - -// EncoderConfig enc = new EncoderConfig(); -// enc.velocityConversionFactor(RPM_TO_RADS); -// config.apply(enc); -// config.apply(loopConfig); + pidController = new PIDController(DriveConstants.PID_P, DriveConstants.PID_I, DriveConstants.PID_D); - LeftLeaderConfig.apply(loopConfig); - RightLeaderConfig.apply(loopConfig); leftLeader.configure(LeftLeaderConfig, SparkBase.ResetMode.kResetSafeParameters, SparkBase.PersistMode.kPersistParameters); leftFollower.configure(LeftFollowerConfig, SparkBase.ResetMode.kResetSafeParameters, SparkBase.PersistMode.kPersistParameters); rightLeader.configure(RightLeaderConfig, SparkBase.ResetMode.kResetSafeParameters, SparkBase.PersistMode.kPersistParameters); @@ -115,7 +95,8 @@ public void setVelocityRadPerSecR(double rightVelocityRadPerSec){ @Override public void rotateToTag() { - leftLeaderController.setReference(RobotState.get().vision_yaw * RPM_PER_RAD, SparkBase.ControlType.kVelocity); - rightLeaderController.setReference(-RobotState.get().vision_yaw * RPM_PER_RAD, SparkBase.ControlType.kVelocity); + var newVelocity = pidController.calculate(RobotState.get().vision_yaw, 0); + leftLeader.set(newVelocity); + rightLeader.set(-newVelocity); } } From c9e085477f33773b1fa934d249d1e7d98da1bba2 Mon Sep 17 00:00:00 2001 From: EJain-Dev Date: Tue, 28 Oct 2025 21:45:03 -0400 Subject: [PATCH 09/11] Renamed vision_yaw to visionYaw. --- src/main/java/frc/robot/RobotState.java | 4 ++-- src/main/java/frc/robot/subsystems/drive/DriveIOSparkMax.java | 2 +- src/main/java/frc/robot/subsystems/vision/Vision.java | 2 +- 3 files changed, 4 insertions(+), 4 deletions(-) diff --git a/src/main/java/frc/robot/RobotState.java b/src/main/java/frc/robot/RobotState.java index ef7add0..0086c16 100644 --- a/src/main/java/frc/robot/RobotState.java +++ b/src/main/java/frc/robot/RobotState.java @@ -2,14 +2,14 @@ public class RobotState { private static final RobotState state; - public double vision_yaw; + public double visionYaw; static { state = new RobotState(); } public RobotState() { - vision_yaw = 0.0; + visionYaw = 0.0; } public static RobotState get() { diff --git a/src/main/java/frc/robot/subsystems/drive/DriveIOSparkMax.java b/src/main/java/frc/robot/subsystems/drive/DriveIOSparkMax.java index 31a2a49..b79aa07 100644 --- a/src/main/java/frc/robot/subsystems/drive/DriveIOSparkMax.java +++ b/src/main/java/frc/robot/subsystems/drive/DriveIOSparkMax.java @@ -95,7 +95,7 @@ public void setVelocityRadPerSecR(double rightVelocityRadPerSec){ @Override public void rotateToTag() { - var newVelocity = pidController.calculate(RobotState.get().vision_yaw, 0); + var newVelocity = pidController.calculate(RobotState.get().visionYaw, 0); leftLeader.set(newVelocity); rightLeader.set(-newVelocity); } diff --git a/src/main/java/frc/robot/subsystems/vision/Vision.java b/src/main/java/frc/robot/subsystems/vision/Vision.java index 99812b8..0ba2268 100644 --- a/src/main/java/frc/robot/subsystems/vision/Vision.java +++ b/src/main/java/frc/robot/subsystems/vision/Vision.java @@ -27,7 +27,7 @@ public void periodic() { } public Command updateYaw() { - return Commands.runOnce(() -> {RobotState.get().vision_yaw = inputs.yaw;}, this); + return Commands.runOnce(() -> {RobotState.get().visionYaw = inputs.yaw;}, this); } public double distanceFromTarget(){ return inputs.distanceFromTarget; From f41ac5fa28b0c9d38a578771b9c253a2b7bcf5c7 Mon Sep 17 00:00:00 2001 From: Ekansh Jain Date: Thu, 30 Oct 2025 16:22:45 +0000 Subject: [PATCH 10/11] Switched to passing yaw as an argument and not using RobotState. --- src/main/java/frc/robot/Orchestrator.java | 4 +--- src/main/java/frc/robot/RobotState.java | 18 ------------------ .../java/frc/robot/subsystems/drive/Drive.java | 5 ++--- .../frc/robot/subsystems/drive/DriveIO.java | 2 +- .../subsystems/drive/DriveIOSparkMax.java | 5 ++--- .../frc/robot/subsystems/vision/Vision.java | 4 ---- 6 files changed, 6 insertions(+), 32 deletions(-) delete mode 100644 src/main/java/frc/robot/RobotState.java diff --git a/src/main/java/frc/robot/Orchestrator.java b/src/main/java/frc/robot/Orchestrator.java index 676eeb7..ae23e6d 100644 --- a/src/main/java/frc/robot/Orchestrator.java +++ b/src/main/java/frc/robot/Orchestrator.java @@ -10,7 +10,6 @@ import frc.robot.subsystems.shooter.ShooterConstants; import frc.robot.subsystems.vision.Vision; - public class Orchestrator { private final Indexer indexer; private final Shooter shooter; @@ -59,8 +58,7 @@ public Command shootCycle(BooleanSupplier fastMode) { public Command rotateToTag() { return Commands.sequence( - vision.updateYaw(), - drive.toAprilTag() + drive.toAprilTag(vision.getYaw()) ).repeatedly(); } diff --git a/src/main/java/frc/robot/RobotState.java b/src/main/java/frc/robot/RobotState.java deleted file mode 100644 index 0086c16..0000000 --- a/src/main/java/frc/robot/RobotState.java +++ /dev/null @@ -1,18 +0,0 @@ -package frc.robot; - -public class RobotState { - private static final RobotState state; - public double visionYaw; - - static { - state = new RobotState(); - } - - public RobotState() { - visionYaw = 0.0; - } - - public static RobotState get() { - return state; - } -} diff --git a/src/main/java/frc/robot/subsystems/drive/Drive.java b/src/main/java/frc/robot/subsystems/drive/Drive.java index 3f1b6e0..adb182e 100644 --- a/src/main/java/frc/robot/subsystems/drive/Drive.java +++ b/src/main/java/frc/robot/subsystems/drive/Drive.java @@ -8,7 +8,6 @@ import java.util.function.BooleanSupplier; import java.util.function.DoubleSupplier; -import frc.robot.RobotState; import org.littletonrobotics.junction.Logger; public class Drive extends SubsystemBase { @@ -33,10 +32,10 @@ public Command arcadeDrive(DoubleSupplier speedX, DoubleSupplier rotation) { }, this); } - public Command toAprilTag() { + public Command toAprilTag(double yaw) { return Commands.run( () -> { - io.rotateToTag(); + io.rotateToTag(yaw); }, this ); } diff --git a/src/main/java/frc/robot/subsystems/drive/DriveIO.java b/src/main/java/frc/robot/subsystems/drive/DriveIO.java index c5f161e..7f4018c 100644 --- a/src/main/java/frc/robot/subsystems/drive/DriveIO.java +++ b/src/main/java/frc/robot/subsystems/drive/DriveIO.java @@ -26,5 +26,5 @@ default void setVelocityRadPerSecL(double leftVelocityRadPerSec){} default void setVelocityRadPerSecR(double rightVelocityRadPerSec){} - default public void rotateToTag() {} + default public void rotateToTag(double yaw) {} } diff --git a/src/main/java/frc/robot/subsystems/drive/DriveIOSparkMax.java b/src/main/java/frc/robot/subsystems/drive/DriveIOSparkMax.java index b79aa07..cc7703f 100644 --- a/src/main/java/frc/robot/subsystems/drive/DriveIOSparkMax.java +++ b/src/main/java/frc/robot/subsystems/drive/DriveIOSparkMax.java @@ -7,7 +7,6 @@ import com.revrobotics.spark.config.SparkBaseConfig; import com.revrobotics.spark.config.SparkMaxConfig; import edu.wpi.first.math.controller.PIDController; -import frc.robot.RobotState; public class DriveIOSparkMax implements DriveIO{ private final SparkMax leftLeader; @@ -94,8 +93,8 @@ public void setVelocityRadPerSecR(double rightVelocityRadPerSec){ } @Override - public void rotateToTag() { - var newVelocity = pidController.calculate(RobotState.get().visionYaw, 0); + public void rotateToTag(double yaw) { + var newVelocity = pidController.calculate(yaw, 0); leftLeader.set(newVelocity); rightLeader.set(-newVelocity); } diff --git a/src/main/java/frc/robot/subsystems/vision/Vision.java b/src/main/java/frc/robot/subsystems/vision/Vision.java index 0ba2268..06c50df 100644 --- a/src/main/java/frc/robot/subsystems/vision/Vision.java +++ b/src/main/java/frc/robot/subsystems/vision/Vision.java @@ -3,7 +3,6 @@ import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.Commands; import edu.wpi.first.wpilibj2.command.SubsystemBase; -import frc.robot.RobotState; import org.littletonrobotics.junction.Logger; public class Vision extends SubsystemBase { @@ -26,9 +25,6 @@ public void periodic() { Logger.processInputs("Vision", inputs); } - public Command updateYaw() { - return Commands.runOnce(() -> {RobotState.get().visionYaw = inputs.yaw;}, this); - } public double distanceFromTarget(){ return inputs.distanceFromTarget; } From 0806cb8b821b8759028d9e6d051c21799f7f19b2 Mon Sep 17 00:00:00 2001 From: Ekansh Jain Date: Thu, 30 Oct 2025 16:26:20 +0000 Subject: [PATCH 11/11] Clamped velocity output from PID --- src/main/java/frc/robot/subsystems/drive/DriveIOSparkMax.java | 3 +++ 1 file changed, 3 insertions(+) diff --git a/src/main/java/frc/robot/subsystems/drive/DriveIOSparkMax.java b/src/main/java/frc/robot/subsystems/drive/DriveIOSparkMax.java index cc7703f..c2fadb5 100644 --- a/src/main/java/frc/robot/subsystems/drive/DriveIOSparkMax.java +++ b/src/main/java/frc/robot/subsystems/drive/DriveIOSparkMax.java @@ -7,6 +7,7 @@ import com.revrobotics.spark.config.SparkBaseConfig; import com.revrobotics.spark.config.SparkMaxConfig; import edu.wpi.first.math.controller.PIDController; +import java.lang.Math; public class DriveIOSparkMax implements DriveIO{ private final SparkMax leftLeader; @@ -95,6 +96,8 @@ public void setVelocityRadPerSecR(double rightVelocityRadPerSec){ @Override public void rotateToTag(double yaw) { var newVelocity = pidController.calculate(yaw, 0); + newVelocity = Math.max(-1, newVelocity); + newVelocity = Math.min(1, newVelocity); leftLeader.set(newVelocity); rightLeader.set(-newVelocity); }