conveyor有bug版

src/main/java/frc/robot/Constants.java

@@ -15,7 +15,7 @@ public final class Constants {
         CrossingTrench
     }
     public class Shooter {
-        public static final double conveyorSpeed = 25;
+        public static final double conveyorSpeed = 50;
         public static final Slot0Configs flyWheelSlot0Configs = new Slot0Configs()
             .withKP(0.0).withKI(0).withKD(0)
             .withKV(0.099).withKA(0.0).withKS(0.0);

src/main/java/frc/robot/controls/ShooterControls.java

@@ -76,14 +76,14 @@ public class ShooterControls {
                         flywheelSpeed = 0;
                         conveyorSpeed = 0;
                     }
-                    if(shooterSubsystem.flywheelMotorLeft.getVelocity().getValueAsDouble()<flywheelSpeed*0.95){
+                    if(shooterSubsystem.flywheelMotorLeft.getVelocity().getValueAsDouble()<flywheelSpeed*0.9){
                         conveyorSpeed = 0;
                     }
                     shooterControlTable.getEntry("flywheelTargetSpeed").setDouble(flywheelSpeed);
                     shooterControlTable.getEntry("conveyerTargetSpeed").setDouble(conveyorSpeed);
                     shooterControlTable.getEntry("backboardTargetPosition").setDouble(backboardPosition);
                     shooterSubsystem.setFlywheelSpeedByRPS(flywheelSpeed);
-                    shooterSubsystem.setConveyorSpeedByRPS(conveyorSpeed);
+                    shooterSubsystem.setConveyorSpeedOpen(5);
                     shooterSubsystem.setBackboardPosition(backboardPosition);  
                     if(shooterSubsystem.isBackboardAtTarget()){
                         shooterSubsystem.backboardMotor.set(0);

src/main/java/frc/robot/subsystems/CommandSwerveDrivetrain.java

@@ -284,10 +284,12 @@ public class CommandSwerveDrivetrain extends TunerSwerveDrivetrain implements Su
         for(int id : new int[]{16,32}){
             if(id == trueId){
                 ifRightTag = true;
+                System.out.println("Target get.");
                 break;
             }
         }
         if(!ifRightTag){
+            System.out.println("No Target.");
             return "No Target";
         }
         double tx = LimelightSupplier.getTX() ;

src/main/java/frc/robot/subsystems/ShooterSubsystem.java

@@ -95,6 +95,9 @@ public class ShooterSubsystem extends SubsystemBase {
     public void setConveyorSpeedByRPS(double speed) {
         conveyorMotor.setControl(velocityRequest.withVelocity(speed));  // 设置传动电机速度
     }
+    public void setConveyorSpeedOpen(double speed){
+        conveyorMotor.set(speed);
+    }
 
     //调试用
     public void setBackboardSpeedByRPS(double speed){
@@ -167,6 +170,7 @@ public class ShooterSubsystem extends SubsystemBase {
         shooterNetworkTable.getEntry("backboardTargetRate").setDouble(backboardPID.getSetpoint().position);
         shooterNetworkTable.getEntry("isBackboardAtTarget").setBoolean(isBackboardAtTarget());
         shooterNetworkTable.getEntry("distanceGetted").setDouble(getDistanceToHub());
+        shooterNetworkTable.getEntry("distancetopassball").setDouble(getDistanceToPassball());
     }
 
     public double getDistanceToHub(){