From da6e3d4a5af281023b58ad2096f832e5e2303941 Mon Sep 17 00:00:00 2001 From: OnDYCLaptop Date: Mon, 23 Feb 2026 14:25:19 +0800 Subject: [PATCH] =?UTF-8?q?=E6=89=93=E8=A1=A8=E5=89=8D=E5=87=86=E5=A4=87?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- elastic-layout.json | 353 ++++++++++++++++++ src/main/java/frc/robot/Constants.java | 4 +- src/main/java/frc/robot/RobotContainer.java | 4 +- .../frc/robot/controls/ShooterControls.java | 37 +- .../subsystems/CommandSwerveDrivetrain.java | 8 + 5 files changed, 389 insertions(+), 17 deletions(-) diff --git a/elastic-layout.json b/elastic-layout.json index 33656e2..e592787 100644 --- a/elastic-layout.json +++ b/elastic-layout.json @@ -972,6 +972,359 @@ "color": 4278238420, "line_width": 2.0 } + }, + { + "title": "Robot", + "x": 0.0, + "y": 512.0, + "width": 384.0, + "height": 128.0, + "type": "Text Display", + "properties": { + "topic": "/SmartDashboard/PositioningIntakerll/Robot", + "period": 0.06, + "data_type": "double[]", + "show_submit_button": true + } + }, + { + "title": "Robot", + "x": 384.0, + "y": 512.0, + "width": 384.0, + "height": 128.0, + "type": "Text Display", + "properties": { + "topic": "/SmartDashboard/PositioningShooterll/Robot", + "period": 0.06, + "data_type": "double[]", + "show_submit_button": true + } + }, + { + "title": "X", + "x": 0.0, + "y": 640.0, + "width": 128.0, + "height": 128.0, + "type": "Text Display", + "properties": { + "topic": "/Positioning/limelight-intaker/X", + "period": 0.06, + "data_type": "double", + "show_submit_button": true + } + }, + { + "title": "Y", + "x": 128.0, + "y": 640.0, + "width": 128.0, + "height": 128.0, + "type": "Text Display", + "properties": { + "topic": "/Positioning/limelight-intaker/Y", + "period": 0.06, + "data_type": "double", + "show_submit_button": true + } + }, + { + "title": "X", + "x": 384.0, + "y": 640.0, + "width": 128.0, + "height": 128.0, + "type": "Text Display", + "properties": { + "topic": "/Positioning/limelight-shooter/X", + "period": 0.06, + "data_type": "double", + "show_submit_button": true + } + }, + { + "title": "Y", + "x": 512.0, + "y": 640.0, + "width": 128.0, + "height": 128.0, + "type": "Text Display", + "properties": { + "topic": "/Positioning/limelight-shooter/Y", + "period": 0.06, + "data_type": "double", + "show_submit_button": true + } + } + ] + } + }, + { + "name": "Shooter", + "grid_layout": { + "layouts": [], + "containers": [ + { + "title": "limelight-shooter", + "x": 1152.0, + "y": 0.0, + "width": 384.0, + "height": 384.0, + "type": "Camera Stream", + "properties": { + "topic": "/CameraPublisher/limelight-shooter", + "period": 0.06, + "rotation_turns": 0 + } + }, + { + "title": "backboardCurrentRate", + "x": 896.0, + "y": 0.0, + "width": 256.0, + "height": 256.0, + "type": "Graph", + "properties": { + "topic": "/Shooter/backboardCurrentRate", + "period": 0.033, + "data_type": "double", + "time_displayed": 5.0, + "color": 4278238420, + "line_width": 2.0 + } + }, + { + "title": "conveyorSpeed", + "x": 384.0, + "y": 256.0, + "width": 128.0, + "height": 128.0, + "type": "Text Display", + "properties": { + "topic": "/Shooter/conveyorSpeed", + "period": 0.06, + "data_type": "double", + "show_submit_button": true + } + }, + { + "title": "distanceGetted", + "x": 0.0, + "y": 0.0, + "width": 384.0, + "height": 256.0, + "type": "Graph", + "properties": { + "topic": "/Shooter/distanceGetted", + "period": 0.033, + "data_type": "double", + "time_displayed": 5.0, + "color": 4278238420, + "line_width": 2.0 + } + }, + { + "title": "isBackboardAtTarget", + "x": 896.0, + "y": 640.0, + "width": 256.0, + "height": 128.0, + "type": "Boolean Box", + "properties": { + "topic": "/Shooter/isBackboardAtTarget", + "period": 0.06, + "data_type": "boolean", + "true_color": 4283215696, + "false_color": 4294198070, + "true_icon": "None", + "false_icon": "None" + } + }, + { + "title": "flywheelSpeed", + "x": 640.0, + "y": 0.0, + "width": 256.0, + "height": 256.0, + "type": "Graph", + "properties": { + "topic": "/Shooter/flywheelSpeed", + "period": 0.033, + "data_type": "double", + "time_displayed": 5.0, + "color": 4278238420, + "line_width": 2.0 + } + }, + { + "title": "Field", + "x": 1152.0, + "y": 384.0, + "width": 384.0, + "height": 256.0, + "type": "Field", + "properties": { + "topic": "/SmartDashboard/Field", + "period": 0.06, + "field_game": "Rebuilt", + "robot_width": 0.85, + "robot_length": 0.85, + "show_other_objects": true, + "show_trajectories": true, + "field_rotation": 180.0, + "robot_color": 4294198070, + "trajectory_color": 4294967295, + "show_robot_outside_widget": true + } + }, + { + "title": "distanceGetted", + "x": 0.0, + "y": 256.0, + "width": 384.0, + "height": 128.0, + "type": "Text Display", + "properties": { + "topic": "/Shooter/distanceGetted", + "period": 0.033, + "data_type": "double", + "show_submit_button": true + } + }, + { + "title": "conveyorSpeed", + "x": 384.0, + "y": 0.0, + "width": 256.0, + "height": 256.0, + "type": "Graph", + "properties": { + "topic": "/Shooter/conveyorSpeed", + "period": 0.033, + "data_type": "double", + "time_displayed": 5.0, + "color": 4278238420, + "line_width": 2.0 + } + }, + { + "title": "flywheelSpeed", + "x": 640.0, + "y": 256.0, + "width": 128.0, + "height": 128.0, + "type": "Text Display", + "properties": { + "topic": "/Shooter/flywheelSpeed", + "period": 0.033, + "data_type": "double", + "show_submit_button": true + } + }, + { + "title": "backboardCurrentRate", + "x": 896.0, + "y": 256.0, + "width": 128.0, + "height": 128.0, + "type": "Text Display", + "properties": { + "topic": "/Shooter/backboardCurrentRate", + "period": 0.06, + "data_type": "double", + "show_submit_button": true + } + }, + { + "title": "backboardTargetRate", + "x": 1024.0, + "y": 256.0, + "width": 128.0, + "height": 128.0, + "type": "Text Display", + "properties": { + "topic": "/Shooter/backboardTargetRate", + "period": 0.06, + "data_type": "double", + "show_submit_button": true + } + }, + { + "title": "conveyorPIDReference", + "x": 512.0, + "y": 256.0, + "width": 128.0, + "height": 128.0, + "type": "Text Display", + "properties": { + "topic": "/Shooter/conveyorPIDReference", + "period": 0.06, + "data_type": "double", + "show_submit_button": true + } + }, + { + "title": "flywheelPIDReference", + "x": 768.0, + "y": 256.0, + "width": 128.0, + "height": 128.0, + "type": "Text Display", + "properties": { + "topic": "/Shooter/flywheelPIDReference", + "period": 0.06, + "data_type": "double", + "show_submit_button": true + } + }, + { + "title": "conveyorPIDReference", + "x": 384.0, + "y": 384.0, + "width": 256.0, + "height": 256.0, + "type": "Graph", + "properties": { + "topic": "/Shooter/conveyorPIDReference", + "period": 0.033, + "data_type": "double", + "time_displayed": 5.0, + "color": 4278238420, + "line_width": 2.0 + } + }, + { + "title": "flywheelPIDReference", + "x": 640.0, + "y": 384.0, + "width": 256.0, + "height": 256.0, + "type": "Graph", + "properties": { + "topic": "/Shooter/flywheelPIDReference", + "period": 0.033, + "data_type": "double", + "time_displayed": 5.0, + "color": 4278238420, + "line_width": 2.0 + } + }, + { + "title": "backboardTargetRate", + "x": 896.0, + "y": 384.0, + "width": 256.0, + "height": 256.0, + "type": "Graph", + "properties": { + "topic": "/Shooter/backboardTargetRate", + "period": 0.033, + "data_type": "double", + "time_displayed": 5.0, + "color": 4278238420, + "line_width": 2.0 + } } ] } diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java index 3dfc4f3..2615536 100644 --- a/src/main/java/frc/robot/Constants.java +++ b/src/main/java/frc/robot/Constants.java @@ -6,7 +6,7 @@ import edu.wpi.first.math.geometry.Pose2d; public final class Constants { public static final boolean LEDUsing = false; public class Shooter { - public static final double conveyorSpeedkP = 10; + public static final double conveyorSpeed = 20; public static final Slot0Configs flyWheelSlot0Configs = new Slot0Configs() .withKP(0.0).withKI(0).withKD(0) .withKV(0.1).withKA(0.0).withKS(0.15); @@ -67,7 +67,7 @@ public final class Constants { public class Limelight { public static final String LIMELIGHT_NAME_Shooter = "limelight-shooter"; public static final String LIMELIGHT_NAME_Intaker = "limelight-intaker"; - public static final Boolean UsingMetaTag2 = false; + public static final Boolean UsingMetaTag2 = true; } public class Field { diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 71eac46..79fb7b0 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -104,8 +104,8 @@ public class RobotContainer { controller.povLeft().onTrue(Commands.runOnce(() -> shooterControls.adjustFlywheelSpeedOffset(-5))); controller.povRight().onTrue(Commands.runOnce(() -> shooterControls.adjustFlywheelSpeedOffset(5))); - controller.povUp().onTrue(Commands.runOnce(() -> shooterControls.adjustBackboardRateOffset(1500))); - controller.povDown().onTrue(Commands.runOnce(() -> shooterControls.adjustBackboardRateOffset(-1500))); + controller.povUp().onTrue(Commands.runOnce(() -> shooterControls.adjustBackboardRateOffset(500))); + controller.povDown().onTrue(Commands.runOnce(() -> shooterControls.adjustBackboardRateOffset(-500))); controller.b().onTrue(shooterControls.resetBackboardCommand()); } diff --git a/src/main/java/frc/robot/controls/ShooterControls.java b/src/main/java/frc/robot/controls/ShooterControls.java index b850b8f..c92fd2f 100644 --- a/src/main/java/frc/robot/controls/ShooterControls.java +++ b/src/main/java/frc/robot/controls/ShooterControls.java @@ -1,8 +1,9 @@ package frc.robot.controls; +import edu.wpi.first.networktables.NetworkTable; +import edu.wpi.first.networktables.NetworkTableInstance; import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.Commands; -import edu.wpi.first.wpilibj2.command.RunCommand; import edu.wpi.first.wpilibj2.command.button.CommandXboxController; import frc.robot.Constants; import frc.robot.subsystems.ShooterSubsystem; @@ -14,31 +15,35 @@ public class ShooterControls { private double flywheelSpeedOffset = 0.0; private double backboardPositionOffset = 0.0; - + private NetworkTable shooterControlTable; public ShooterControls(ShooterSubsystem shooterSubsystem, CommandXboxController controller) { this.shooterSubsystem = shooterSubsystem; this.controller = controller; + + shooterControlTable = NetworkTableInstance.getDefault().getTable("shooterControl"); } public Command defaultShooterCommand() { return Commands.run(() -> { - // System.out.println("running"); - if (controller.getRightTriggerAxis() < 0.1) { - shooterSubsystem.setConveyorSpeedByRPS(0); - shooterSubsystem.setFlywheelSpeedByRPS(0); - return; - } double flywheelSpeed; double backboardPosition; double conveyorSpeed; - flywheelSpeed = calculateFlywheelSpeedOnlywithOffset(20); + flywheelSpeed = calculateFlywheelSpeedOnlywithOffset(30); backboardPosition = calculateBackboardPositionOnlywithOffset(100); - conveyorSpeed = controller.getRightTriggerAxis() * Constants.Shooter.conveyorSpeedkP; - + conveyorSpeed = Constants.Shooter.conveyorSpeed; + shooterControlTable.getEntry("flywheelTargetSpeed").setDouble(flywheelSpeed); + shooterControlTable.getEntry("conveyerTargetSpeed").setDouble(conveyorSpeed); + shooterControlTable.getEntry("backboardTargetPosition").setDouble(backboardPosition); + // System.out.println("running"); + if (controller.getRightTriggerAxis() < 0.1) { + flywheelSpeed = 0; + conveyorSpeed = 0; + } + // double flywheelSpeed = calculateFlywheelSpeed(0); // double BackboardPosition = calculateBackboardPosition(0); - // shooterSubsystem.setFlywheelSpeedByRPS(flywheelSpeed); - // shooterSubsystem.setConveyorSpeedByRPS(conveyorSpeed); + shooterSubsystem.setFlywheelSpeedByRPS(flywheelSpeed); + shooterSubsystem.setConveyorSpeedByRPS(conveyorSpeed); shooterSubsystem.setBackboardPosition(backboardPosition); }, shooterSubsystem); } @@ -60,6 +65,12 @@ public class ShooterControls { // shooterSubsystem.currentBackboardInalyzeMode = Mode.Inalyzed;}, shooterSubsystem)); // } + public void outputInfo(){ + double currentConveyorSpeed = shooterSubsystem.conveyorMotor.getVelocity().getValueAsDouble(); + double currentFlywheelSpeed = shooterSubsystem.flywheelMotorLeft.getVelocity().getValueAsDouble(); + double currentBackboardRate = shooterSubsystem.getBackboardPosition(); + System.out.println("Conveyor: " + currentConveyorSpeed + "\t Flywheel: " + currentFlywheelSpeed + "\t Backboard:" + currentBackboardRate); + } public Command resetBackboardCommand(){ return Commands.runOnce(()->{ diff --git a/src/main/java/frc/robot/subsystems/CommandSwerveDrivetrain.java b/src/main/java/frc/robot/subsystems/CommandSwerveDrivetrain.java index acaaabf..64b4b90 100644 --- a/src/main/java/frc/robot/subsystems/CommandSwerveDrivetrain.java +++ b/src/main/java/frc/robot/subsystems/CommandSwerveDrivetrain.java @@ -193,6 +193,8 @@ public class CommandSwerveDrivetrain extends TunerSwerveDrivetrain implements Su }else if (shooterAvailable && !intakerAvailable) { double shooterDistance = current.getTranslation().getDistance(mt2_shooter.pose.getTranslation()); ll_shooter_NT.getEntry("Distance").setDouble(shooterDistance); + ll_shooter_NT.getEntry("X").setDouble(mt2_shooter.pose.getX()); + ll_shooter_NT.getEntry("Y").setDouble(mt2_shooter.pose.getY()); positioningNetworkTable.getEntry("MetaTag2Available").setBoolean(true); positioningNetworkTable.getEntry("Usingll").setString("Shooter"); //positioningNetworkTable.getEntry("MetaTag2DistanceDifference").setDouble(Double.NaN); @@ -200,6 +202,8 @@ public class CommandSwerveDrivetrain extends TunerSwerveDrivetrain implements Su }else if (!shooterAvailable && intakerAvailable) { double intakerDistance = current.getTranslation().getDistance(mt2_intaker.pose.getTranslation()); ll_intaker_NT.getEntry("Distance").setDouble(intakerDistance); + ll_intaker_NT.getEntry("X").setDouble(mt2_intaker.pose.getX()); + ll_intaker_NT.getEntry("Y").setDouble(mt2_intaker.pose.getY()); positioningNetworkTable.getEntry("MetaTag2Available").setBoolean(true); positioningNetworkTable.getEntry("Usingll").setString("Intaker"); //positioningNetworkTable.getEntry("MetaTag2DistanceDifference").setDouble(Double.NaN); @@ -212,6 +216,10 @@ public class CommandSwerveDrivetrain extends TunerSwerveDrivetrain implements Su ll_shooter_NT.getEntry("Distance").setDouble(shooterDistance); ll_intaker_NT.getEntry("Distance").setDouble(intakerDistance); + ll_shooter_NT.getEntry("X").setDouble(mt2_shooter.pose.getX()); + ll_shooter_NT.getEntry("Y").setDouble(mt2_shooter.pose.getY()); + ll_intaker_NT.getEntry("X").setDouble(mt2_intaker.pose.getX()); + ll_intaker_NT.getEntry("Y").setDouble(mt2_intaker.pose.getY()); positioningNetworkTable.getEntry("MetaTag2DistanceDifference").setDouble(difference); positioningNetworkTable.getEntry("MetaTag2XDifference").setDouble(Math.abs(mt2_shooter.pose.getX() - mt2_intaker.pose.getX())); positioningNetworkTable.getEntry("MetaTag2YDifference").setDouble(Math.abs(mt2_shooter.pose.getY() - mt2_intaker.pose.getY()));