打表前准备

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
+            }
           }
         ]
       }

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 {

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.povUp().onTrue(Commands.runOnce(() -> shooterControls.adjustBackboardRateOffset(500)));
-    controller.povDown().onTrue(Commands.runOnce(() -> shooterControls.adjustBackboardRateOffset(-1500)));
+    controller.povDown().onTrue(Commands.runOnce(() -> shooterControls.adjustBackboardRateOffset(-500)));
 
     controller.b().onTrue(shooterControls.resetBackboardCommand());
   }

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.setFlywheelSpeedByRPS(flywheelSpeed);
-                // shooterSubsystem.setConveyorSpeedByRPS(conveyorSpeed);
+                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(()->{

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()));