first commit

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

@@ -0,0 +1,281 @@
+package frc.robot.subsystems;
+import java.util.Optional;
+import java.util.function.Supplier;
+
+import com.ctre.phoenix6.Utils;
+import com.ctre.phoenix6.swerve.SwerveDrivetrainConstants;
+import com.ctre.phoenix6.swerve.SwerveModuleConstants;
+import com.ctre.phoenix6.swerve.SwerveRequest;
+import com.pathplanner.lib.auto.AutoBuilder;
+import com.pathplanner.lib.config.PIDConstants;
+import com.pathplanner.lib.config.RobotConfig;
+import com.pathplanner.lib.controllers.PPHolonomicDriveController;
+
+import edu.wpi.first.math.Matrix;
+import edu.wpi.first.math.VecBuilder;
+import edu.wpi.first.math.geometry.Pose2d;
+import edu.wpi.first.math.geometry.Rotation2d;
+import edu.wpi.first.math.kinematics.ChassisSpeeds;
+import edu.wpi.first.math.numbers.N1;
+import edu.wpi.first.math.numbers.N3;
+import edu.wpi.first.wpilibj.DriverStation;
+import edu.wpi.first.wpilibj.DriverStation.Alliance;
+import edu.wpi.first.wpilibj.smartdashboard.Field2d;
+import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
+import edu.wpi.first.wpilibj2.command.Command;
+import edu.wpi.first.wpilibj2.command.Subsystem;
+import frc.robot.Constants.DriveMode;
+import frc.robot.generated.TunerConstants.TunerSwerveDrivetrain;
+import frc.robot.Constants;
+
+import edu.wpi.first.networktables.*;
+
+
+/**
+ * Class that extends the Phoenix 6 SwerveDrivetrain class and implements
+ * Subsystem so it can easily be used in command-based projects.
+ *
+ * Generated by the 2026 Tuner X Swerve Project Generator
+ * https://v6.docs.ctr-electronics.com/en/stable/docs/tuner/tuner-swerve/index.html
+ */
+public class CommandSwerveDrivetrain extends TunerSwerveDrivetrain implements Subsystem {
+
+    /* Blue alliance sees forward as 0 degrees (toward red alliance wall) */
+    private static final Rotation2d kBlueAlliancePerspectiveRotation = Rotation2d.kZero;
+    /* Red alliance sees forward as 180 degrees (toward blue alliance wall) */
+    private static final Rotation2d kRedAlliancePerspectiveRotation = Rotation2d.kCW_90deg;
+    /* Keep track if we've ever applied the operator perspective before or not */
+    private boolean m_hasAppliedOperatorPerspective = false;
+
+    private DriveMode m_driveMode = DriveMode.OTHER;
+    
+    private NetworkTable positioningNetworkTable;
+    private NetworkTable driveNetworkTable;
+    
+    private final StructPublisher<Pose2d> posePub =
+    NetworkTableInstance.getDefault()
+        .getStructTopic("/Odometry/RobotPose", Pose2d.struct)
+        .publish();
+
+    private Field2d m_field;
+    public CommandSwerveDrivetrain(
+        SwerveDrivetrainConstants drivetrainConstants,
+        SwerveModuleConstants<?, ?, ?>... modules
+    ) {
+        super(drivetrainConstants, modules);
+        m_field = new Field2d();
+        SmartDashboard.putData("Field", m_field);
+        LimelightHelpers.SetIMUMode(Constants.Limelight.LIMELIGHT_NAME, 0);
+        positioningNetworkTable = NetworkTableInstance.getDefault().getTable("Positioning");
+        driveNetworkTable = NetworkTableInstance.getDefault().getTable("Drive");
+
+        configueAutoBuilder();
+
+    }
+
+
+    private void configueAutoBuilder(){
+        
+        RobotConfig config;
+        try {
+            config = RobotConfig.fromGUISettings();
+        } catch (Exception e) {
+            throw new RuntimeException("Failed to load PathPlanner RobotConfig", e);
+        }
+
+        AutoBuilder.configure(
+            this::getPose,                  // 当前机器人 Pose
+            this::resetPose,                // 重置里程计
+            this::getRobotRelativeSpeeds,   // ROBOT RELATIVE speeds
+            (speeds, feedforwards) -> driveRobotRelative(speeds),      // 用 speeds 驱动机器人
+            new PPHolonomicDriveController(
+                new PIDConstants(5.0, 0.0, 0.0),   // X/Y PID
+                new PIDConstants(5.0, 0.0, 0.0)    // Theta PID
+            ),
+            config,
+            () -> false,
+            this
+        );
+
+    }
+    public Pose2d getPose() {
+        return getState().Pose;
+    }
+
+    public ChassisSpeeds getRobotRelativeSpeeds() {
+        return getState().Speeds;
+    }
+
+    public void driveRobotRelative(ChassisSpeeds speeds) {
+        ChassisSpeeds scaled = new ChassisSpeeds(
+            speeds.vxMetersPerSecond,                 // X 不变
+            speeds.vyMetersPerSecond,                 // Y 不变
+            speeds.omegaRadiansPerSecond * 1.3        // 只放大旋转
+        );
+
+        setControl(
+            new SwerveRequest.ApplyRobotSpeeds().withSpeeds(scaled)
+        );
+    }
+
+
+
+        
+    private static final double kMaxVisionJumpMeters = 1.0;
+    private static final double kMaxOmegaDegPerSec = 360.0;
+
+    /**
+     * Returns a command that applies the specified control request to this swerve drivetrain.
+     *
+     * @param request Function returning the request to apply
+     * @return Command to run
+     */
+    public Command applyRequest(Supplier<SwerveRequest> request) {
+        return run(() -> this.setControl(request.get()));
+    }
+
+    public Command applyRequest(Supplier<SwerveRequest> requestSupplier, Supplier<DriveMode> driveModeSupplier) {
+        return run(() -> {
+            DriveMode mode = driveModeSupplier.get();
+            m_driveMode = mode;
+            this.setControl(requestSupplier.get());
+        });
+    }
+    
+    public void forceUsingLimelightMT2() {
+        LimelightHelpers.PoseEstimate mt2 =
+        LimelightHelpers.getBotPoseEstimate_wpiBlue_MegaTag2(Constants.Limelight.LIMELIGHT_NAME);
+
+        if (mt2 == null || mt2.tagCount == 0) {
+            return;
+        }
+
+        var stdDevs = VecBuilder.fill(0.7, 0.7, 9999999);
+        addVisionMeasurement(mt2.pose, mt2.timestampSeconds, stdDevs);
+    }
+    
+    private void fuseLimelightMT2() {
+        
+        LimelightHelpers.PoseEstimate mt2 =
+        LimelightHelpers.getBotPoseEstimate_wpiBlue_MegaTag2(Constants.Limelight.LIMELIGHT_NAME);
+
+        Pose2d current = getState().Pose;
+        Pose2d mt2Pose = mt2.pose;
+
+        double distance = current.getTranslation().getDistance(mt2Pose.getTranslation());
+        Rotation2d deltaRotation = current.getRotation().minus(mt2Pose.getRotation());
+
+        positioningNetworkTable.getEntry("CurrentPoseX").setDouble(current.getX());
+        positioningNetworkTable.getEntry("CurrentPoseY").setDouble(current.getY());
+        positioningNetworkTable.getEntry("MetaTag2PoseX").setDouble(mt2Pose.getX());
+        positioningNetworkTable.getEntry("MetaTag2PoseY").setDouble(mt2Pose.getY());
+        positioningNetworkTable.getEntry("PoseDistance").setDouble(distance);
+        positioningNetworkTable.getEntry("RotationDifference").setDouble(Math.abs(deltaRotation.getDegrees()));
+
+        // 1) 没看到 tag：拒绝
+        if (mt2 == null || mt2.tagCount == 0) {
+            positioningNetworkTable.getEntry("MetaTag2Available").setBoolean(false);
+            return;
+        }
+        // 2) 转太快：拒绝（角速度你可以从 Pigeon2 或 drivetrain state 拿）
+        double omegaDegPerSec = getPigeon2().getAngularVelocityZWorld().getValueAsDouble(); // 若你有这个信号
+        if (Math.abs(omegaDegPerSec) > kMaxOmegaDegPerSec) {
+            positioningNetworkTable.getEntry("MetaTag2Available").setBoolean(false);
+            System.out.println("NoUsingMetaTag2ForTooFastRotationSpeed");
+            return;
+        }      
+
+        // 3) 跳变太大：拒绝（Phoenix6 推荐的稳定策略）
+        //Pose2d current = getState().Pose;
+        if (current.getTranslation().getDistance(mt2.pose.getTranslation()) > kMaxVisionJumpMeters) {
+            positioningNetworkTable.getEntry("MetaTag2Available").setBoolean(false);
+            System.out.println("NoUsingMetaTag2ForTooChangingPose");
+            return;
+        }
+
+        // 4) 设置测量噪声（示例：theta 基本不信任就给极大）
+        // 你也可以根据 mt2.avgTagDist / mt2.tagSpan 动态调
+        var stdDevs = VecBuilder.fill(0.7, 0.7, 9999999);
+
+        // 5) 融合：注意 timestampSeconds 的处理
+        addVisionMeasurement(mt2.pose, mt2.timestampSeconds, stdDevs);
+        positioningNetworkTable.getEntry("MetaTag2Available").setBoolean(true);
+    }
+
+
+    @Override
+    public void periodic() {
+        Pose2d pose = getState().Pose;
+        m_field.setRobotPose(pose);
+        posePub.set(pose);
+        driveNetworkTable.getEntry("YawFromState").setDouble(getState().Pose.getRotation().getDegrees());
+        driveNetworkTable.getEntry("DriveMode").setString(m_driveMode.name());
+        driveNetworkTable.getEntry("IsFieldCentric").setBoolean(m_driveMode == DriveMode.FIELD_CENTRIC);
+
+        // 1) 每帧喂给 LL：机器人当前 yaw（度），其余先全 0
+        LimelightHelpers.SetRobotOrientation(
+            Constants.Limelight.LIMELIGHT_NAME,
+            pose.getRotation().getDegrees(), // 关键：要符合 wpiBlue 约定
+            0, 0, 0, 0, 0
+        );
+
+        fuseLimelightMT2();
+
+        if (!m_hasAppliedOperatorPerspective || DriverStation.isDisabled()) {
+            DriverStation.getAlliance().ifPresent(allianceColor -> {
+                setOperatorPerspectiveForward(
+                    allianceColor == Alliance.Red
+                        ? kRedAlliancePerspectiveRotation
+                        : kBlueAlliancePerspectiveRotation
+                );
+                m_hasAppliedOperatorPerspective = true;
+            });
+        }
+    }        
+        
+        
+            /**
+     * Adds a vision measurement to the Kalman Filter. This will correct the odometry pose estimate
+     * while still accounting for measurement noise.
+     *
+     * @param visionRobotPoseMeters The pose of the robot as measured by the vision camera.
+     * @param timestampSeconds The timestamp of the vision measurement in seconds.
+     */
+    @Override
+    public void addVisionMeasurement(Pose2d visionRobotPoseMeters, double timestampSeconds) {
+        super.addVisionMeasurement(visionRobotPoseMeters, Utils.fpgaToCurrentTime(timestampSeconds));
+    }
+
+    /**
+     * Adds a vision measurement to the Kalman Filter. This will correct the odometry pose estimate
+     * while still accounting for measurement noise.
+     * <p>
+     * Note that the vision measurement standard deviations passed into this method
+     * will continue to apply to future measurements until a subsequent call to
+     * {@link #setVisionMeasurementStdDevs(Matrix)} or this method.
+     *
+     * @param visionRobotPoseMeters The pose of the robot as measured by the vision camera.
+     * @param timestampSeconds The timestamp of the vision measurement in seconds.
+     * @param visionMeasurementStdDevs Standard deviations of the vision pose measurement
+     *     in the form [x, y, theta]ᵀ, with units in meters and radians.
+     */
+    @Override
+    public void addVisionMeasurement(
+        Pose2d visionRobotPoseMeters,
+        double timestampSeconds,
+        Matrix<N3, N1> visionMeasurementStdDevs
+    ) {
+        super.addVisionMeasurement(visionRobotPoseMeters, Utils.fpgaToCurrentTime(timestampSeconds), visionMeasurementStdDevs);
+    }
+
+    /**
+     * Return the pose at a given timestamp, if the buffer is not empty.
+     *
+     * @param timestampSeconds The timestamp of the pose in seconds.
+     * @return The pose at the given timestamp (or Optional.empty() if the buffer is empty).
+     */
+    @Override
+    public Optional<Pose2d> samplePoseAt(double timestampSeconds) {
+        return super.samplePoseAt(Utils.fpgaToCurrentTime(timestampSeconds));
+    }
+}

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

@@ -0,0 +1,67 @@
+package frc.robot.subsystems;
+
+import com.ctre.phoenix6.controls.PositionVoltage;
+import com.ctre.phoenix6.controls.VelocityVoltage;
+import com.ctre.phoenix6.hardware.TalonFX;
+
+import edu.wpi.first.wpilibj2.command.SubsystemBase;
+import frc.robot.Constants;
+
+public class IntakerSubsystem extends SubsystemBase{
+    //定义intakerotater&intakegetter
+    private TalonFX intakerotater;
+    private TalonFX intakegetter;
+
+    //PID 
+    private final VelocityVoltage velocityRequest = new VelocityVoltage(0);
+    private final PositionVoltage positionRequest = new PositionVoltage(0);
+ 
+    private double intakeRotaterTargetPosition = 0.0;
+
+    public IntakerSubsystem() {
+        //初始化rotater&getter
+        intakegetter = new TalonFX(47);
+        intakerotater = new TalonFX(48);
+        intakegetter.getConfigurator().apply(Constants.Intaker.intakeGetterSlot0Configs);
+        intakerotater.getConfigurator().apply(Constants.Intaker.intakeRotaterSlot0Configs);        
+    }
+
+    public void setIntakerRotaterPosition(double targetPosition) {
+        //目标position
+        if (targetPosition > Constants.Intaker.intakeRotaterUpLimit) {
+            targetPosition = Constants.Intaker.intakeRotaterUpLimit;  // 限制最大上限
+        } else if (targetPosition < Constants.Intaker.intakeRotaterDownLimit) {
+            targetPosition = Constants.Intaker.intakeRotaterDownLimit;  // 限制最小下限
+        }
+
+        intakeRotaterTargetPosition = targetPosition;
+
+        intakerotater.setControl(positionRequest.withPosition(targetPosition)); // 设置rotater位置（TalonFX 内部闭环 PID）
+    }
+
+    // 获取intakerotater当前角度
+    public double getIntakerotaterPosition() {
+        return intakerotater.getPosition().getValueAsDouble();
+    }
+
+    public boolean isIntakerotaterAtTarget() {
+        // 到位判断容差
+        double error = Math.abs(intakeRotaterTargetPosition - getIntakerotaterPosition());
+        return error <= Constants.Intaker.positionToleranceRotations;
+    }
+
+     public void stopintakerotater() {
+        intakerotater.setControl(velocityRequest.withVelocity(0));
+        // 如果你希望停止后保持当前位置，建议改用中性模式/或维持 positionRequest（这句fromAI）
+    }
+
+    //intakegetter的on/off
+    public void setIntakeGetterOn(boolean on) {
+        if (on) {
+            intakegetter.setControl(velocityRequest.withVelocity(Constants.Intaker.IntakeGetterSpeed));
+        }
+        else {
+            intakegetter.setControl(velocityRequest.withVelocity(0));
+        }
+    }
+}

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

@@ -0,0 +1,151 @@
+package frc.robot.subsystems;
+
+import edu.wpi.first.wpilibj.AddressableLED;
+import edu.wpi.first.wpilibj.AddressableLEDBuffer;
+import edu.wpi.first.wpilibj.Timer;
+import edu.wpi.first.wpilibj2.command.SubsystemBase;
+import frc.robot.Constants;
+
+public class LEDSubsystem extends SubsystemBase {
+
+  public enum Mode {
+    OFF,
+    SOLID,
+    BREATH,
+    RAINBOW
+  }
+
+  private final AddressableLED led;
+  private final AddressableLEDBuffer buffer;
+  private final Timer timer = new Timer();
+
+  // 左右灯带分段（左 0~71，右 72~143）
+  private final int leftStart = 0;
+  private final int leftLen;
+  private final int rightStart;
+  private final int rightLen;
+
+  private Mode leftMode = Mode.OFF;
+  private Mode rightMode = Mode.OFF;
+
+  // 左右颜色
+  private int lr = 0, lg = 0, lb = 0;
+  private int rr = 0, rg = 0, rb = 0;
+
+  // 动画参数
+  private double leftBreathPeriodSec = 2.0;
+  private double rightBreathPeriodSec = 2.0;
+  private int leftRainbowOffset = 0;
+  private int rightRainbowOffset = 0;
+
+  /**
+   * @param pwmPort  PWM 端口（只能用一个！）
+   * @param leftCount 左灯数量
+   * @param rightCount 右灯数量
+   */
+  public LEDSubsystem(int pwmPort, int leftCount, int rightCount) {
+    this.leftLen = leftCount;
+    this.rightLen = rightCount;
+    this.rightStart = leftStart + leftLen;
+
+    led = new AddressableLED(pwmPort);
+    buffer = new AddressableLEDBuffer(leftLen + rightLen);
+
+    led.setLength(buffer.getLength());
+    led.start();
+
+    timer.start();
+  }
+
+  /* ====================== */
+  /*        对外API           */
+  /* ====================== */
+
+  public void offLeft() { leftMode = Mode.OFF; }
+  public void offRight() { rightMode = Mode.OFF; }
+  public void offAll() { leftMode = Mode.OFF; rightMode = Mode.OFF; }
+
+  public void solidLeft(int r, int g, int b) {
+    lr = clamp8(r); lg = clamp8(g); lb = clamp8(b);
+    leftMode = Mode.SOLID;
+  }
+
+  public void solidRight(int r, int g, int b) {
+    rr = clamp8(r); rg = clamp8(g); rb = clamp8(b);
+    rightMode = Mode.SOLID;
+  }
+
+  public void breathLeft(int r, int g, int b, double periodSeconds) {
+    lr = clamp8(r); lg = clamp8(g); lb = clamp8(b);
+    leftBreathPeriodSec = Math.max(0.2, periodSeconds);
+    leftMode = Mode.BREATH;
+  }
+
+  public void breathRight(int r, int g, int b, double periodSeconds) {
+    rr = clamp8(r); rg = clamp8(g); rb = clamp8(b);
+    rightBreathPeriodSec = Math.max(0.2, periodSeconds);
+    rightMode = Mode.BREATH;
+  }
+
+  public void rainbowLeft() { leftMode = Mode.RAINBOW; }
+  public void rainbowRight() { rightMode = Mode.RAINBOW; }
+  public void rainbowAll() { leftMode = Mode.RAINBOW; rightMode = Mode.RAINBOW; }
+
+  @Override
+  public void periodic() {
+    if(!Constants.LEDUsing) {led.close(); return;}
+    // 左段
+    switch (leftMode) {
+      case OFF -> setRange(leftStart, leftLen, 0, 0, 0);
+      case SOLID -> setRange(leftStart, leftLen, lr, lg, lb);
+      case BREATH -> updateBreath(leftStart, leftLen, lr, lg, lb, leftBreathPeriodSec);
+      case RAINBOW -> updateRainbow(leftStart, leftLen, true);
+    }
+
+    // 右段
+    switch (rightMode) {
+      case OFF -> setRange(rightStart, rightLen, 0, 0, 0);
+      case SOLID -> setRange(rightStart, rightLen, rr, rg, rb);
+      case BREATH -> updateBreath(rightStart, rightLen, rr, rg, rb, rightBreathPeriodSec);
+      case RAINBOW -> updateRainbow(rightStart, rightLen, false);
+    }
+
+    led.setData(buffer);
+  }
+
+  /* ====================== */
+  /*        内部实现          */
+  /* ====================== */
+
+  private void setRange(int start, int len, int r, int g, int b) {
+    for (int i = 0; i < len; i++) {
+      buffer.setRGB(start + i, r, g, b);
+    }
+  }
+
+  private void updateBreath(int start, int len, int r, int g, int b, double periodSec) {
+    double t = timer.get();
+    double phase = (t % periodSec) / periodSec; // 0..1
+    double brightness = 0.5 * (1.0 + Math.sin(2.0 * Math.PI * phase)); // 0..1
+    int rr = (int) (r * brightness);
+    int gg = (int) (g * brightness);
+    int bb = (int) (b * brightness);
+    setRange(start, len, rr, gg, bb);
+  }
+
+  private void updateRainbow(int start, int len, boolean isLeft) {
+    int offset = isLeft ? leftRainbowOffset : rightRainbowOffset;
+
+    for (int i = 0; i < len; i++) {
+      int hue = (offset + (i * 180 / len)) % 180;
+      buffer.setHSV(start + i, hue, 255, 128);
+    }
+
+    if (isLeft) leftRainbowOffset = (leftRainbowOffset + 2) % 180;
+    else rightRainbowOffset = (rightRainbowOffset + 2) % 180;
+  }
+
+  private static int clamp8(int v) {
+    return Math.max(0, Math.min(255, v));
+  }
+}

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

@@ -0,0 +1,171 @@
+package frc.robot.subsystems;
+
+import com.ctre.phoenix6.configs.MotorOutputConfigs;
+import com.ctre.phoenix6.configs.Slot0Configs;
+import com.ctre.phoenix6.controls.Follower;
+import com.ctre.phoenix6.controls.VelocityDutyCycle;
+import com.ctre.phoenix6.hardware.TalonFX;
+import com.ctre.phoenix6.hardware.TalonFXS;
+import com.ctre.phoenix6.signals.InvertedValue;
+import com.ctre.phoenix6.signals.MotorAlignmentValue;
+
+import edu.wpi.first.math.controller.ProfiledPIDController;
+import edu.wpi.first.math.trajectory.TrapezoidProfile.Constraints;
+import edu.wpi.first.networktables.NetworkTable;
+import edu.wpi.first.networktables.NetworkTableInstance;
+import edu.wpi.first.wpilibj.Encoder;
+import edu.wpi.first.wpilibj2.command.SubsystemBase;
+import frc.robot.Constants;
+
+public class ShooterSubsystem extends SubsystemBase {
+    // 定义飞轮、传动和背板电机
+    public TalonFX flywheelMotorLeft;
+    private TalonFX flywheelMotorRight;
+    public TalonFX conveyorMotor;
+    public TalonFXS backboardMotor;
+    public Encoder backboardEncoder;
+
+    // PID 控制器，控制背板角度
+    public ProfiledPIDController backboardPID;
+    private Follower flyWheelFollower;
+
+    private NetworkTable shooterNetworkTable;
+
+    private final VelocityDutyCycle velocityRequest = new VelocityDutyCycle(0);
+
+    public enum Mode{
+        Inalyzed,
+        UnInalyed,
+        Inalyzing;
+    }
+    public Mode currentBackboardInalyzeMode;
+    // 初始化各个部件
+    public ShooterSubsystem() {
+        // 初始化飞轮电机
+        flywheelMotorLeft = new TalonFX(41);  // 41是飞轮left的CAN ID
+        flywheelMotorRight = new TalonFX(42);  // 42是飞轮right的CAN ID
+        flywheelMotorLeft.getConfigurator().apply(Constants.Shooter.flyWheelSlot0Configs);
+        flywheelMotorRight.getConfigurator().apply(Constants.Shooter.flyWheelSlot0Configs);
+        // 初始化传动电机
+        conveyorMotor = new TalonFX(43);
+        conveyorMotor.getConfigurator().apply(Constants.Shooter.conveyorSlot0Configs);
+
+        var configs1 = new MotorOutputConfigs();
+        configs1.Inverted = InvertedValue.Clockwise_Positive;
+        flywheelMotorLeft.getConfigurator().apply(configs1);
+        flyWheelFollower = new Follower(flywheelMotorLeft.getDeviceID(), MotorAlignmentValue.Opposed).withUpdateFreqHz(50);
+        
+        var configs2 = new MotorOutputConfigs();
+        configs2.Inverted = InvertedValue.Clockwise_Positive;
+        conveyorMotor.getConfigurator().apply(configs2);
+        
+        // 初始化背板电机和编码器
+        backboardMotor = new TalonFXS(44);  
+        backboardMotor.getConfigurator().apply(Constants.Shooter.backboardSlot0Configs);
+        backboardEncoder = new Encoder(0, 1, false, Encoder.EncodingType.k4X);
+        backboardEncoder.setSamplesToAverage(5);
+        backboardEncoder.setMinRate(1.0);
+        backboardEncoder.setDistancePerPulse(1);
+        
+        // 初始化背板的PID控制器
+        backboardPID = new ProfiledPIDController(Constants.Shooter.backboardPositionPID.kP, Constants.Shooter.backboardPositionPID.kI, Constants.Shooter.backboardPositionPID.kD,
+                                                    new Constraints(10000,100000));
+        
+        backboardPID.setTolerance(10.0); 
+        
+        shooterNetworkTable = NetworkTableInstance.getDefault().getTable("Shooter");
+
+        currentBackboardInalyzeMode = Mode.UnInalyed;
+    }
+
+    public void setRightFollowLeft(){
+        flywheelMotorRight.setControl(flyWheelFollower);
+    }
+
+    public void applyLeftConfigurationToRight(){
+        Slot0Configs configs = new Slot0Configs();
+        flywheelMotorLeft.getConfigurator().refresh(configs);
+        flywheelMotorRight.getConfigurator().apply(configs);
+    }
+
+
+    // 设置飞轮的速度
+    public void setFlywheelSpeedByRPS(double targetSpeed) {
+        flywheelMotorLeft.setControl(velocityRequest.withVelocity(targetSpeed)); //RPS 转每秒
+        setRightFollowLeft();
+        // System.out.println("set flywheel "+targetSpeed);
+    }
+
+    // 设置传动的速度
+    public void setConveyorSpeedByRPS(double speed) {
+        conveyorMotor.setControl(velocityRequest.withVelocity(speed));  // 设置传动电机速度
+    }
+
+    //调试用
+    public void setBackboardSpeedByRPS(double speed){
+        backboardMotor.setControl(velocityRequest.withVelocity(speed));
+    }
+
+    // 设置背板的目标位置，通过PID控制器来调整  范围0-3000
+    public void setBackboardPosition(double targetPosition) {
+        // 限制目标位置在上下限之间
+        if (targetPosition > Constants.Shooter.backboardUpLimit) {
+            targetPosition = Constants.Shooter.backboardUpLimit;  // 限制最大上限
+        } else if (targetPosition < Constants.Shooter.backboardDownLimit) {
+            targetPosition = Constants.Shooter.backboardDownLimit;  // 限制最小下限
+        }
+        backboardPID.setGoal(targetPosition);
+    }
+    public void outputBackboard(){
+
+        // 使用PID控制器来计算背板的运动速度
+        double pidOutput = backboardPID.calculate(getBackboardPosition());
+        pidOutput = Math.max(-Constants.Shooter.backboardSpeedMax, Math.min(Constants.Shooter.backboardSpeedMax, pidOutput)); // 限制输出范围在-1到1之间
+        shooterNetworkTable.getEntry("backboardPIDOutput").setDouble(pidOutput);
+        // // 设置背板电机的速度，根据PID输出进行调整
+        setBackboardSpeedByRPS(pidOutput);
+    }
+    // 停止所有电机
+    public void stopMotors() {
+        flywheelMotorLeft.set(0);
+        flywheelMotorRight.set(0);
+        conveyorMotor.set(0);
+        backboardMotor.set(0);
+    }
+
+    // 获取背板当前角度
+    public double getBackboardPosition() {
+        return backboardEncoder.getDistance();  // 返回背板电机编码器的当前角度
+    }
+
+    // 检查背板是否到达目标角度
+    public boolean isBackboardAtTarget() {
+        return backboardPID.atGoal();  // 判断背板是否到达目标角度
+    }
+
+    @Override
+    public void periodic() {
+        if(currentBackboardInalyzeMode != Mode.Inalyzing){
+            backboardPID.calculate(getBackboardPosition());
+            if(isBackboardAtTarget()){
+                backboardMotor.set(0);
+            }else{
+                outputBackboard();
+            }
+        }
+        shooterNetworkTable.getEntry("flywheelSpeed").setDouble(flywheelMotorLeft.getVelocity().getValueAsDouble());
+        shooterNetworkTable.getEntry("flywheelPIDReference").setDouble(flywheelMotorLeft.getClosedLoopReference().getValueAsDouble());
+        shooterNetworkTable.getEntry("conveyorSpeed").setDouble(conveyorMotor.getVelocity().getValueAsDouble());
+        shooterNetworkTable.getEntry("conveyorPIDReference").setDouble(conveyorMotor.getClosedLoopReference().getValueAsDouble());
+        shooterNetworkTable.getEntry("backboardCurrentRate").setDouble(getBackboardPosition());
+        shooterNetworkTable.getEntry("backboardTargetRate").setDouble(backboardPID.getSetpoint().position);
+        shooterNetworkTable.getEntry("isBackboardAtTarget").setBoolean(isBackboardAtTarget());
+        shooterNetworkTable.getEntry("distanceGetted").setDouble(getDistance());
+        shooterNetworkTable.getEntry("InalyzeBackboardMode").setString(currentBackboardInalyzeMode.name());
+    }
+
+    private double getDistance(){
+        //获得车子距离Hub的距离
+        return NetworkTableInstance.getDefault().getTable("AutoControl").getEntry("distanceToHub").getDouble(0.0);
+    }
+}