admin6494/FRC6494_2026Regional-offseason
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

gpt的超级状态机

Browse Source
This commit is contained in:
OnDYCLaptop
2026-07-04 15:02:09 +08:00
parent 132b231c36
commit 3c5d17b527
19 changed files with 1335 additions and 1317 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/main/java/frc/robot/subsystems/ClimberSubsystem.java
-127
View File
@@ -1,127 +0,0 @@
package frc.robot.subsystems;
import com.ctre.phoenix6.configs.MotorOutputConfigs;
import com.ctre.phoenix6.controls.DutyCycleOut;
import com.ctre.phoenix6.controls.PositionVoltage;
import com.ctre.phoenix6.controls.VelocityDutyCycle;
import com.ctre.phoenix6.controls.VelocityVoltage;
import com.ctre.phoenix6.hardware.TalonFX;
import com.ctre.phoenix6.signals.InvertedValue;
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.SubsystemBase;
import frc.robot.Constants;
public class ClimberSubsystem extends SubsystemBase {
private boolean autoClimbUsedInAuto = false;
private final TalonFX climberMotor;
private double targetPositionTicks;
private final PositionVoltage positionHoldRequest =
new PositionVoltage(0).withSlot(0);
private final VelocityVoltage velocityRequest = new VelocityVoltage(0).withSlot(0);
private double holdPositionTicks = 0.0;
private final NetworkTable climberTable;
public ClimberSubsystem() {
climberMotor = new TalonFX(Constants.Climber.motorID);
climberMotor.getConfigurator().apply(Constants.Climber.slot0Configs);
var cfg = new MotorOutputConfigs();
cfg.Inverted = Constants.Climber.inverted
? InvertedValue.Clockwise_Positive
: InvertedValue.CounterClockwise_Positive;
climberMotor.getConfigurator().apply(cfg);
climberTable = NetworkTableInstance.getDefault().getTable("Climber");
holdPositionTicks = getPositionTicks();
}
public void initialize() {
climberMotor.set(-0.07);
}
public double getPositionTicks() {
return climberMotor.getPosition().getValueAsDouble();
}
public double getVelocityRps() {
return climberMotor.getVelocity().getValueAsDouble();
}
public void climbUp() {
targetPositionTicks = Constants.Climber.climbUpTicks;
climberMotor.setControl(positionHoldRequest.withPosition(targetPositionTicks));
}
public void climbDown() {
targetPositionTicks = Constants.Climber.climbDownTicks;
climberMotor.setControl(positionHoldRequest.withPosition(targetPositionTicks));
}
public void climbDownAuto() {
targetPositionTicks = Constants.Climber.climbDownAutoTicks;
climberMotor.setControl(positionHoldRequest.withPosition(targetPositionTicks));
}
public void climbDownNonStop(){
climberMotor.setControl(velocityRequest.withVelocity(Constants.Climber.climbVelocity));
}
public void climbUpNonStop(){
climberMotor.setControl(velocityRequest.withVelocity(-Constants.Climber.climbVelocity));
}
public void climbUpNonStopforAuto(){
climberMotor.set(0.07);
}
public void captureHoldPosition() {
holdPositionTicks = getPositionTicks();
}
public void holdPosition() {
climberMotor.setControl(
positionHoldRequest.withPosition(holdPositionTicks)
);
}
public void stopMotor() {
climberMotor.set(0);
}
public void setMotor(double speed) {
climberMotor.set(speed);
}
public Command initCommand() {
return Commands.sequence(
Commands.run(() -> climberMotor.set(-0.07), this).withTimeout(0.5),
Commands.runOnce(this::zeroEncoder, this)
);
}
public void zeroEncoder() {
climberMotor.setPosition(0);
stopMotor();
}
public void stopAll() {
climberMotor.set(0);
}
@Override
public void periodic() {
climberTable.getEntry("velocityRps").setDouble(getVelocityRps());
climberTable.getEntry("positionTicks").setDouble(getPositionTicks());
climberTable.getEntry("holdPositionTicks").setDouble(holdPositionTicks);
}
}
src/main/java/frc/robot/subsystems/CommandSwerveDrivetrain.java
+123 -226
View File
@@ -1,4 +1,5 @@
package frc.robot.subsystems;
import java.util.Optional;
import java.util.function.Supplier;
@@ -15,58 +16,56 @@ 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.geometry.Translation2d;
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.networktables.NetworkTable;
import edu.wpi.first.networktables.NetworkTableInstance;
import edu.wpi.first.networktables.StructPublisher;
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;
import frc.robot.Constants.DriveMode;
import frc.robot.generated.TunerConstants.TunerSwerveDrivetrain;
import frc.robot.subsystems.LimelightHelpers.RawFiducial;
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.
* Phoenix 6 Tuner X 生成底盘的命令式封装。
*
* Generated by the 2026 Tuner X Swerve Project Generator
* https://v6.docs.ctr-electronics.com/en/stable/docs/tuner/tuner-swerve/index.html
* 底层 swerve、模块控制和里程计保持原结构；本类只在外层补充
* Command-Based、PathPlanner、Limelight 融合和上层状态读取接口。
*/
public class CommandSwerveDrivetrain extends TunerSwerveDrivetrain implements Subsystem {
/* Blue alliance sees forward as 0 degrees (toward red alliance wall) */
/* 蓝方视角下，正前方为 0 度。 */
private static final Rotation2d kBlueAlliancePerspectiveRotation = Rotation2d.kZero;
/* Red alliance sees forward as 180 degrees (toward blue alliance wall) */
/* 红方视角下，正前方为 180 度。 */
private static final Rotation2d kRedAlliancePerspectiveRotation = Rotation2d.k180deg;
/* Keep track if we've ever applied the operator perspective before or not */
/* 记录是否已经应用过操作员视角。 */
private boolean m_hasAppliedOperatorPerspective = false;
private boolean isAlignedToAprilTag = false;
// 上层状态机射击门控使用的目标朝向；正常底盘控制仍由手动驾驶掌控。
private Rotation2d targetHeading = Rotation2d.kZero;
private DriveMode m_driveMode = DriveMode.OTHER;
private NetworkTable positioningNetworkTable;
private NetworkTable ll_shooter_NT;
private NetworkTable ll_intaker_NT;
private NetworkTable driveNetworkTable;
private final StructPublisher<Pose2d> posePub =
NetworkTableInstance.getDefault()
.getStructTopic("/Odometry/RobotPose", Pose2d.struct)
.publish();
NetworkTableInstance.getDefault()
.getStructTopic("/Odometry/RobotPose", Pose2d.struct)
.publish();
private Field2d m_field;
private Field2d shooter_ll_field;
private Field2d intaker_ll_field;
public CommandSwerveDrivetrain(
SwerveDrivetrainConstants drivetrainConstants,
SwerveModuleConstants<?, ?, ?>... modules
@@ -86,12 +85,9 @@ public class CommandSwerveDrivetrain extends TunerSwerveDrivetrain implements Su
ll_intaker_NT = NetworkTableInstance.getDefault().getTable("Positioning/limelight-intaker");
configueAutoBuilder();
}
private void configueAutoBuilder(){
private void configueAutoBuilder() {
RobotConfig config;
try {
config = RobotConfig.fromGUISettings();
@@ -100,20 +96,20 @@ public class CommandSwerveDrivetrain extends TunerSwerveDrivetrain implements Su
}
AutoBuilder.configure(
this::getPose, // 当前机器人 Pose
this::resetPose, // 重置里程计
this::getRobotRelativeSpeeds, // ROBOT RELATIVE speeds
(speeds, feedforwards) -> driveRobotRelative(speeds), // 用 speeds 驱动机器人
this::getPose, // 当前机器人位姿。
this::resetPose, // 重置里程计。
this::getRobotRelativeSpeeds, // 机器人坐标系速度。
(speeds, feedforwards) -> driveRobotRelative(speeds), // 按目标速度驱动机器人。
new PPHolonomicDriveController(
new PIDConstants(5.0, 0.0, 0.0), // X/Y PID
new PIDConstants(5.0, 0.0, 0.0) // Theta PID
new PIDConstants(5.0, 0.0, 0.0), // X/Y 位置 PID。
new PIDConstants(5.0, 0.0, 0.0) // 朝向 PID。
),
config,
() -> false,
this
);
}
public Pose2d getPose() {
return getState().Pose;
}
@@ -122,32 +118,70 @@ public class CommandSwerveDrivetrain extends TunerSwerveDrivetrain implements Su
return getState().Speeds;
}
public ChassisSpeeds getChassisSpeeds() {
// 给上层代码使用的速度别名；原机器人坐标系方法继续留给 PathPlanner。
return getRobotRelativeSpeeds();
}
public ChassisSpeeds getFieldRelativeSpeeds() {
return ChassisSpeeds.fromRobotRelativeSpeeds(getRobotRelativeSpeeds(), getRotation());
}
public Rotation2d getRotation() {
return getState().Pose.getRotation();
}
public void setTargetHeading(Rotation2d targetHeading) {
this.targetHeading = targetHeading;
}
public double getHeadingError() {
return targetHeading.minus(getRotation()).getDegrees();
}
public boolean isAimed() {
// 上层状态机只读取这个门控，不直接控制 swerve 模块。
return Math.abs(getHeadingError()) <= Constants.Superstructure.aimToleranceDegrees;
}
public void driveRobotRelative(ChassisSpeeds speeds) {
ChassisSpeeds scaled = new ChassisSpeeds(
speeds.vxMetersPerSecond, // X 不变
speeds.vyMetersPerSecond, // Y 不变
speeds.omegaRadiansPerSecond * 1.3 // 只放大旋转
speeds.vxMetersPerSecond, // X 方向速度保持原值。
speeds.vyMetersPerSecond, // Y 方向速度保持原值。
speeds.omegaRadiansPerSecond * 1.3 // 只放大旋转输出，沿用原驾驶手感。
);
setControl(
new SwerveRequest.ApplyRobotSpeeds().withSpeeds(scaled)
);
);
}
public void stop() {
setControl(
new SwerveRequest.ApplyRobotSpeeds().withSpeeds(new ChassisSpeeds(0, 0, 0))
);
);
}
public void lockWheels() {
setControl(new SwerveRequest.SwerveDriveBrake());
}
public void aimToTarget() {
// 给未来命令预留的高层辅助接口，仍走现有底盘请求路径，不触碰模块底层逻辑。
double headingErrorDegrees = getHeadingError();
double omega = Math.toRadians(headingErrorDegrees) * Constants.AutoPositioning.TurningkP;
setControl(
new SwerveRequest.ApplyRobotSpeeds().withSpeeds(new ChassisSpeeds(0, 0, omega))
);
}
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.
* 返回一个持续下发指定 swerve 请求的命令。
*
* @param request Function returning the request to apply
* @return Command to run
* @param request 生成待下发请求的函数。
* @return 可调度执行的 Command。
*/
public Command applyRequest(Supplier<SwerveRequest> request) {
return run(() -> this.setControl(request.get()));
@@ -160,10 +194,10 @@ public class CommandSwerveDrivetrain extends TunerSwerveDrivetrain implements Su
this.setControl(requestSupplier.get());
});
}
public void forceUsingLimelightmt2() {
LimelightHelpers.PoseEstimate mt2 =
LimelightHelpers.getBotPoseEstimate_wpiBlue_MegaTag2(Constants.Limelight.LIMELIGHT_NAME_Shooter);
LimelightHelpers.getBotPoseEstimate_wpiBlue_MegaTag2(Constants.Limelight.LIMELIGHT_NAME_Shooter);
if (mt2 == null || mt2.tagCount == 0) {
return;
@@ -172,10 +206,10 @@ public class CommandSwerveDrivetrain extends TunerSwerveDrivetrain implements Su
var stdDevs = VecBuilder.fill(0.7, 0.7, 9999999);
addVisionMeasurement(mt2.pose, mt2.timestampSeconds, stdDevs);
}
public void forceUsingLimelightmt2WithllName(String llString) {
LimelightHelpers.PoseEstimate mt2 =
LimelightHelpers.getBotPoseEstimate_wpiBlue_MegaTag2(llString);
LimelightHelpers.getBotPoseEstimate_wpiBlue_MegaTag2(llString);
if (mt2 == null || mt2.tagCount == 0) {
return;
@@ -184,14 +218,15 @@ public class CommandSwerveDrivetrain extends TunerSwerveDrivetrain implements Su
var stdDevs = VecBuilder.fill(0.7, 0.7, 9999999);
addVisionMeasurement(mt2.pose, mt2.timestampSeconds, stdDevs);
}
private void fuseLimelightmt2() {
Pose2d current = getState().Pose;
LimelightHelpers.PoseEstimate mt2_shooter =
getLimelightPoseEstimate(Constants.Limelight.LIMELIGHT_NAME_Shooter);
getLimelightPoseEstimate(Constants.Limelight.LIMELIGHT_NAME_Shooter);
LimelightHelpers.PoseEstimate mt2_intaker =
getLimelightPoseEstimate(Constants.Limelight.LIMELIGHT_NAME_Intaker);
getLimelightPoseEstimate(Constants.Limelight.LIMELIGHT_NAME_Intaker);
boolean shooterAvailable = ifLimelightPositioningAvailable(mt2_shooter, current);
boolean intakerAvailable = ifLimelightPositioningAvailable(mt2_intaker, current);
@@ -199,35 +234,34 @@ public class CommandSwerveDrivetrain extends TunerSwerveDrivetrain implements Su
ll_intaker_NT.getEntry("available").setBoolean(intakerAvailable);
ll_shooter_NT.getEntry("TagCount").setNumber(mt2_shooter != null ? mt2_shooter.tagCount : 0);
ll_intaker_NT.getEntry("TagCount").setNumber(mt2_intaker != null ? mt2_intaker.tagCount : 0);
LimelightHelpers.PoseEstimate mt2_final = null;
if(!shooterAvailable && !intakerAvailable) {
if (!shooterAvailable && !intakerAvailable) {
positioningNetworkTable.getEntry("MetaTag2Available").setBoolean(false);
positioningNetworkTable.getEntry("Usingll").setString("None");
//positioningNetworkTable.getEntry("MetaTag2DistanceDifference").setDouble(Double.NaN);
// 未使用视觉时不发布距离差，避免仪表盘误读旧值。
return;
}else if (shooterAvailable && !intakerAvailable) {
} 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);
// 单个 Limelight 可用时直接采用该测量。
mt2_final = mt2_shooter;
}else if (!shooterAvailable && intakerAvailable) {
} 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);
// 单个 Limelight 可用时直接采用该测量。
mt2_final = mt2_intaker;
}else {
// 两个都可用时的策略：这里简单选更近的，你也
} else {
// 两个 Limelight 都可用时，先选择与当前位姿更接近的测量。
double shooterDistance = current.getTranslation().getDistance(mt2_shooter.pose.getTranslation());
double intakerDistance = current.getTranslation().getDistance(mt2_intaker.pose.getTranslation());
double difference = mt2_shooter.pose.getTranslation().getDistance(mt2_intaker.pose.getTranslation());
@@ -245,7 +279,7 @@ public class CommandSwerveDrivetrain extends TunerSwerveDrivetrain implements Su
mt2_final = shooterDistance <= intakerDistance ? mt2_shooter : mt2_intaker;
}
Pose2d mt2Pose = mt2_final.pose;
double distance = current.getTranslation().getDistance(mt2Pose.getTranslation());
@@ -258,11 +292,11 @@ public class CommandSwerveDrivetrain extends TunerSwerveDrivetrain implements Su
positioningNetworkTable.getEntry("PoseDistance").setDouble(distance);
positioningNetworkTable.getEntry("RotationDifference").setDouble(Math.abs(deltaRotation.getDegrees()));
// 4) 设置测量噪声（示例：theta 基本不信任就给极大）
// 你也可以根据 mt2.avgTagDist / mt2.tagSpan 动态调
// 视觉测量噪声：theta 基本不信任，只融合平移。
// 后续可以根据 mt2.avgTagDist / mt2.tagSpan 动态调整。
var stdDevs = VecBuilder.fill(0.7, 0.7, 9999999);
// 5) 融合：注意 timestampSeconds 的处理
// 融合视觉测量，注意 timestampSeconds 的时间基转换。
addVisionMeasurement(mt2_final.pose, mt2_final.timestampSeconds, stdDevs);
positioningNetworkTable.getEntry("MetaTag2Available").setBoolean(true);
}
@@ -273,156 +307,28 @@ public class CommandSwerveDrivetrain extends TunerSwerveDrivetrain implements Su
}
return LimelightHelpers.getBotPoseEstimate_wpiBlue(limelightName);
}
private boolean ifLimelightPositioningAvailable(LimelightHelpers.PoseEstimate mt2, Pose2d currentPose) {
// 1) 没看到 tag：拒绝
// 没看到 tag 时拒绝该帧视觉定位。
if (mt2 == null || mt2.tagCount == 0) {
return false;
}
// 2) 转太快：拒绝（角速度你可以从 Pigeon2 或 drivetrain state 拿）
double omegaDegPerSec = getPigeon2().getAngularVelocityZWorld().getValueAsDouble(); // 若你有这个信号
if (Math.abs(omegaDegPerSec) > kMaxOmegaDegPerSec) {
//System.out.println("NoUsingMetaTag2ForTooFastRotationSpeed");
return false;
}
// 3) 跳变太大：拒绝（Phoenix6 推荐的稳定策略）
//Pose2d current = getState().Pose;
// if (currentPose.getTranslation().getDistance(mt2.pose.getTranslation()) > kMaxVisionJumpMeters) {
// //System.out.println("NoUsingMetaTag2ForTooChangingPose");
// return false;
// }
// 旋转太快时拒绝该帧，避免高速转动时的视觉跳变。
double omegaDegPerSec = getPigeon2().getAngularVelocityZWorld().getValueAsDouble();
if (Math.abs(omegaDegPerSec) > kMaxOmegaDegPerSec) {
return false;
}
return true;
}
private double clamp(double value, double min, double max) {
return Math.max(min, Math.min(max, value));
}
// public String driveToAprilTag() {
// System.out.println("Pipeline = " + LimelightHelpers.getCurrentPipelineIndex(Constants.Limelight.LIMELIGHT_NAME_Intaker));
// System.out.println("TV = " + LimelightSupplier.isTargetVisible());
// if (!LimelightSupplier.isTargetVisible()) {
// isAlignedToAprilTag = false;
// //stop();
// return "No Target";
// }
// RawFiducial[] fiducials =
// LimelightHelpers.getRawFiducials(Constants.Limelight.LIMELIGHT_NAME_Intaker);
// boolean foundDesiredTag = false;
// for (RawFiducial fiducial : fiducials) {
// if (fiducial.id == 16 || fiducial.id == 32) {
// foundDesiredTag = true;
// break;
// }
// }
// if (!foundDesiredTag) {
// isAlignedToAprilTag = false;
// stop();
// return "No Desired Tag";
// }
// // ===== 读取原始误差 =====
// double tx = LimelightSupplier.getTX(); // deg
// double tz = -LimelightSupplier.getTargetTZ(); // m，方向按你当前经验保留负号
// double ry = LimelightSupplier.getTargetRotationY(); // deg
// // ===== 用原始误差判断是否已对齐 =====
// isAlignedToAprilTag =
// Math.abs(tx) < Constants.Limelight.AutoClimbToleranceTX &&
// Math.abs(tz) < Constants.Limelight.AutoClimbToleranceTZ &&
// Math.abs(ry) < Constants.Limelight.AutoClimbToleranceRY;
// if (isAlignedToAprilTag) {
// stop();
// return "Aligned";
// }
// // ===== 计算输出 =====
// double vx = 0.0;
// double vy = 0.0;
// double omega = 0.0;
// // 1) 前后控制：根据 tz 前进/后退
// if (Math.abs(tz) > Constants.Limelight.AutoClimbToleranceTZ) {
// vx = tz * Constants.Limelight.AutoClimbKpTZ;
// // 最小输出，避免快到目标时不动
// if (Math.abs(vx) < Constants.Limelight.AutoClimbMinVX) {
// vx = Math.copySign(Constants.Limelight.AutoClimbMinVX, vx);
// }
// // 最大输出限幅
// vx = clamp(vx,
// -Constants.Limelight.AutoClimbMaxVX,
// Constants.Limelight.AutoClimbMaxVX);
// }
// // 2) 旋转控制：tx 为主，ry 为辅
// // tx 负责“镜头中心对准 tag”
// // ry 负责“机器人姿态微调”
// // double omegaFromTx = 0.0;
// // double omegaFromRy = 0.0;
// // if (Math.abs(tx) > Constants.Limelight.AutoClimbToleranceTX) {
// // omegaFromTx = -tx * Constants.Limelight.AutoClimbKpTX;
// // }
// // if (Math.abs(ry) > Constants.Limelight.AutoClimbToleranceRY) {
// // omegaFromRy = ry * Constants.Limelight.AutoClimbKpRY;
// // }
// // omega = omegaFromTx + omegaFromRy;
// // // 大偏角时直接给较明显的转速，先把朝向拉回来
// // if (Math.abs(tx) > Constants.Limelight.AutoClimbFastTurnThresholdTX) {
// // omega = Math.copySign(Constants.Limelight.AutoClimbFastTurnOmega, omega);
// // }
// // // 最小输出，避免转不动
// // if (Math.abs(omega) > 1e-6 && Math.abs(omega) < Constants.Limelight.AutoClimbMinOmega) {
// // omega = Math.copySign(Constants.Limelight.AutoClimbMinOmega, omega);
// // }
// // // 最大输出限幅
// // omega = clamp(omega,
// // -Constants.Limelight.AutoClimbMaxOmega,
// // Constants.Limelight.AutoClimbMaxOmega);
// Rotation2d ClimbcurrentAngle = getState().Pose.getRotation();
// double climbCurrentRad = ClimbcurrentAngle.getRadians();
// double TargetAngleforClimbing = Math.toRadians(0);
// double Climberror = TargetAngleforClimbing - climbCurrentRad;
// if (Math.abs(Climberror) < Math.toRadians(Constants.AutoPositioning.autoPositioningAngleError)) {
// omega = 0.0;
// } else {
// omega = Climberror * Constants.AutoPositioning.TurningkP;
// }
// // ===== 调试输出 =====
// driveNetworkTable.getEntry("ClimbTX").setDouble(tx);
// driveNetworkTable.getEntry("ClimbTZ").setDouble(tz);
// driveNetworkTable.getEntry("ClimbRY").setDouble(ry);
// driveNetworkTable.getEntry("ClimbVXCmd").setDouble(vx);
// driveNetworkTable.getEntry("ClimbOmegaCmd").setDouble(omega);
// driveNetworkTable.getEntry("ClimbAligned").setBoolean(isAlignedToAprilTag);
// // ===== 下发到底盘 =====
// driveRobotRelative(new ChassisSpeeds(vx, vy, omega));
// return "Driving to AprilTag";
// }
// public boolean isAlignedToAprilTag(){
// return isAlignedToAprilTag;
// }
@Override
public void periodic() {
Pose2d pose = getState().Pose;
m_field.setRobotPose(pose);
posePub.set(pose);
@@ -431,20 +337,19 @@ public class CommandSwerveDrivetrain extends TunerSwerveDrivetrain implements Su
driveNetworkTable.getEntry("YawFromState").setDouble(getState().Pose.getRotation().getDegrees());
driveNetworkTable.getEntry("DriveMode").setString(m_driveMode.name());
driveNetworkTable.getEntry("IsFieldCentric").setBoolean(m_driveMode == DriveMode.FIELD_CENTRIC);
//positioningNetworkTable.getEntry("isAlignedToAprilTag").setBoolean(isAlignedToAprilTag());
// positioningNetworkTable.getEntry("interestPointsClimber TX").setDouble(LimelightSupplier.getTX());
// positioningNetworkTable.getEntry("interestPointsClimber RY").setDouble(LimelightSupplier.getTargetRotationY());
// positioningNetworkTable.getEntry("interestPointsClimber TZ").setDouble(-LimelightSupplier.getTargetTZ());
// 保留 Positioning 表中的基础状态输出。
positioningNetworkTable.getEntry("interestPointsCl").setBoolean(false);
// 1) 每帧喂给 LL：机器人当前 yaw（度），其余先全 0
// 每帧向 Limelight 写入当前 yaw，供 MegaTag2 使用。
LimelightHelpers.SetRobotOrientation(
Constants.Limelight.LIMELIGHT_NAME_Shooter,
pose.getRotation().getDegrees(), // 关键：要符合 wpiBlue 约定
pose.getRotation().getDegrees(), // 符合 wpiBlue 坐标约定。
0, 0, 0, 0, 0
);
LimelightHelpers.SetRobotOrientation(
Constants.Limelight.LIMELIGHT_NAME_Intaker,
pose.getRotation().getDegrees(), // 关键：要符合 wpiBlue 约定
pose.getRotation().getDegrees(), // 符合 wpiBlue 坐标约定。
0, 0, 0, 0, 0
);
@@ -460,15 +365,13 @@ public class CommandSwerveDrivetrain extends TunerSwerveDrivetrain implements Su
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.
* @param visionRobotPoseMeters 视觉测得的机器人位姿。
* @param timestampSeconds 视觉测量时间戳，单位秒。
*/
@Override
public void addVisionMeasurement(Pose2d visionRobotPoseMeters, double timestampSeconds) {
@@ -476,17 +379,11 @@ public class CommandSwerveDrivetrain extends TunerSwerveDrivetrain implements Su
}
/**
* 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.
* @param visionRobotPoseMeters 视觉测得的机器人位姿。
* @param timestampSeconds 视觉测量时间戳，单位秒。
* @param visionMeasurementStdDevs 视觉测量标准差，格式为 [x, y, theta]。
*/
@Override
public void addVisionMeasurement(
@@ -498,10 +395,10 @@ public class CommandSwerveDrivetrain extends TunerSwerveDrivetrain implements Su
}
/**
* 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).
* @param timestampSeconds 位姿时间戳，单位秒。
* @return 指定时间的位姿；缓存为空时返回 Optional.empty()。
*/
@Override
public Optional<Pose2d> samplePoseAt(double timestampSeconds) {
src/main/java/frc/robot/subsystems/ConveyorSubsystem.java
+60
View File
@@ -0,0 +1,60 @@
package frc.robot.subsystems;
import com.ctre.phoenix6.controls.VoltageOut;
import com.ctre.phoenix6.hardware.TalonFX;
import edu.wpi.first.math.MathUtil;
import edu.wpi.first.networktables.NetworkTable;
import edu.wpi.first.networktables.NetworkTableInstance;
import edu.wpi.first.wpilibj2.command.SubsystemBase;
import frc.robot.Constants;
/**
* 主 conveyor 的开环控制封装。
*
* 本赛季没有球检测传感器，因此这里仅报告电机状态；
* 所有动作顺序判断都放在 Superstructure。
*/
public class ConveyorSubsystem extends SubsystemBase {
public final TalonFX mainConveyor;
private final VoltageOut voltageRequest = new VoltageOut(0);
private final NetworkTable table = NetworkTableInstance.getDefault().getTable("Conveyor");
private double voltage = 0.0;
public ConveyorSubsystem() {
mainConveyor = new TalonFX(Constants.Conveyor.mainConveyorID);
}
public void feedToShooter() {
setVoltage(Constants.Conveyor.feedVoltage);
}
public void reverse() {
setVoltage(Constants.Conveyor.reverseVoltage);
}
public void hold() {
setVoltage(Constants.Conveyor.holdVoltage);
}
public void stop() {
setVoltage(0.0);
}
public void setPercent(double percent) {
setVoltage(percent * 12.0);
}
public void setVoltage(double volts) {
voltage = MathUtil.clamp(volts, -12.0, 12.0);
mainConveyor.setControl(voltageRequest.withOutput(voltage));
}
@Override
public void periodic() {
table.getEntry("voltage").setDouble(voltage);
table.getEntry("velocity").setDouble(mainConveyor.getVelocity().getValueAsDouble());
}
}
src/main/java/frc/robot/subsystems/IntakeRollerSubsystem.java
+77
View File
@@ -0,0 +1,77 @@
package frc.robot.subsystems;
import com.ctre.phoenix6.controls.Follower;
import com.ctre.phoenix6.controls.VoltageOut;
import com.ctre.phoenix6.hardware.TalonFX;
import com.ctre.phoenix6.signals.MotorAlignmentValue;
import edu.wpi.first.math.MathUtil;
import edu.wpi.first.networktables.NetworkTable;
import edu.wpi.first.networktables.NetworkTableInstance;
import edu.wpi.first.wpilibj2.command.SubsystemBase;
import frc.robot.Constants;
/**
* 进球滚轮的硬件封装。
*
* 何时 intake、outtake、hold 或 stop 由 Superstructure 决定；
* 本类只负责成对 TalonFX 输出和 follower 关系。
*/
public class IntakeRollerSubsystem extends SubsystemBase {
public final TalonFX leftIntakeRoller;
public final TalonFX rightIntakeRoller;
private final VoltageOut voltageRequest = new VoltageOut(0);
private final Follower rightFollower;
private final NetworkTable table = NetworkTableInstance.getDefault().getTable("IntakeRoller");
private double leftVoltage = 0.0;
private double rightVoltage = 0.0;
public IntakeRollerSubsystem() {
leftIntakeRoller = new TalonFX(Constants.Intaker.leftIntakeRollerID);
rightIntakeRoller = new TalonFX(Constants.Intaker.rightIntakeRollerID);
rightFollower = new Follower(leftIntakeRoller.getDeviceID(), MotorAlignmentValue.Opposed).withUpdateFreqHz(50);
followLeft();
}
public void intake() {
setVoltage(Constants.Intaker.intakeRollerVoltage, -Constants.Intaker.intakeRollerVoltage);
}
public void outtake() {
setVoltage(Constants.Intaker.outtakeRollerVoltage, -Constants.Intaker.outtakeRollerVoltage);
}
public void hold() {
setVoltage(Constants.Intaker.holdRollerVoltage, -Constants.Intaker.holdRollerVoltage);
}
public void stop() {
setVoltage(0.0, 0.0);
}
public void setPercent(double left, double right) {
setVoltage(left * 12.0, right * 12.0);
}
public void setVoltage(double left, double right) {
// rightVoltage 仅用于 dashboard 显示；真实硬件通过反向 follower 跟随左侧电机。
leftVoltage = MathUtil.clamp(left, -12.0, 12.0);
rightVoltage = MathUtil.clamp(right, -12.0, 12.0);
leftIntakeRoller.setControl(voltageRequest.withOutput(leftVoltage));
followLeft();
}
private void followLeft() {
rightIntakeRoller.setControl(rightFollower);
}
@Override
public void periodic() {
table.getEntry("leftVoltage").setDouble(leftVoltage);
table.getEntry("rightVoltage").setDouble(rightVoltage);
table.getEntry("leftVelocity").setDouble(leftIntakeRoller.getVelocity().getValueAsDouble());
table.getEntry("rightVelocity").setDouble(rightIntakeRoller.getVelocity().getValueAsDouble());
}
}
src/main/java/frc/robot/subsystems/IntakeRotaterSubsystem.java
+145
View File
@@ -0,0 +1,145 @@
package frc.robot.subsystems;
import com.ctre.phoenix6.controls.Follower;
import com.ctre.phoenix6.controls.PositionVoltage;
import com.ctre.phoenix6.controls.VoltageOut;
import com.ctre.phoenix6.hardware.TalonFX;
import com.ctre.phoenix6.signals.MotorAlignmentValue;
import edu.wpi.first.math.MathUtil;
import edu.wpi.first.networktables.NetworkTable;
import edu.wpi.first.networktables.NetworkTableInstance;
import edu.wpi.first.wpilibj2.command.SubsystemBase;
import frc.robot.Constants;
/**
* 进球旋转臂的位置闭环和开环控制。
*
* 机构使用 TalonFX 集成编码器。归零前只允许 SAFE 或受限手动移动，
* 避免相对编码器未校准时发生大幅度意外旋转。
*/
public class IntakeRotaterSubsystem extends SubsystemBase {
public enum IntakePosition {
STOW(Constants.Intaker.intakeRotaterStowPosition),
GROUND_INTAKE(Constants.Intaker.intakeRotaterGroundPosition),
HANDOFF(Constants.Intaker.intakeRotaterHandoffPosition),
SAFE(Constants.Intaker.intakeRotaterSafePosition);
public final double rotations;
IntakePosition(double rotations) {
this.rotations = rotations;
}
}
public final TalonFX leftIntakeRotater;
public final TalonFX rightIntakeRotater;
private final PositionVoltage positionRequest = new PositionVoltage(0);
private final VoltageOut voltageRequest = new VoltageOut(0);
private final Follower rightFollower;
private final NetworkTable table = NetworkTableInstance.getDefault().getTable("IntakeRotater");
private IntakePosition goal = IntakePosition.SAFE;
private boolean zeroed = false;
private double manualVoltage = 0.0;
public IntakeRotaterSubsystem() {
leftIntakeRotater = new TalonFX(Constants.Intaker.leftIntakeRotaterID);
rightIntakeRotater = new TalonFX(Constants.Intaker.rightIntakeRotaterID);
leftIntakeRotater.getConfigurator().apply(Constants.Intaker.intakeRotaterSlot0Configs);
rightIntakeRotater.getConfigurator().apply(Constants.Intaker.intakeRotaterSlot0Configs);
rightFollower = new Follower(leftIntakeRotater.getDeviceID(), MotorAlignmentValue.Opposed).withUpdateFreqHz(50);
followLeft();
}
public void setGoal(IntakePosition position) {
// 相对编码器没有对准机械零点前，禁止执行大幅度位置闭环移动。
goal = position;
if (!zeroed && position != IntakePosition.SAFE) {
stop();
return;
}
double target = MathUtil.clamp(
position.rotations,
Constants.Intaker.intakeRotaterDownLimit,
Constants.Intaker.intakeRotaterUpLimit
);
leftIntakeRotater.setControl(positionRequest.withPosition(target));
manualVoltage = 0.0;
followLeft();
}
public void stow() {
setGoal(IntakePosition.STOW);
}
public void deployToGround() {
setGoal(IntakePosition.GROUND_INTAKE);
}
public void moveToHandoff() {
setGoal(IntakePosition.HANDOFF);
}
public boolean atGoal() {
return Math.abs(getPosition() - goal.rotations) <= Constants.Intaker.positionToleranceRotations;
}
public void zeroPosition() {
// 只有 intake 物理上处于已知零点姿态时才调用此方法。
leftIntakeRotater.setPosition(0.0);
rightIntakeRotater.setPosition(0.0);
zeroed = true;
goal = IntakePosition.STOW;
stow();
}
public boolean isZeroed() {
return zeroed;
}
public void clearZeroed() {
zeroed = false;
goal = IntakePosition.SAFE;
stop();
}
public double getPosition() {
return leftIntakeRotater.getPosition().getValueAsDouble();
}
public IntakePosition getGoal() {
return goal;
}
public void setOpenLoopVoltage(double voltage) {
// 归零前允许手动小幅移动，但输出会被限制到较温和的范围。
if (!zeroed && Math.abs(voltage) > Constants.Intaker.intakeRotaterManualVoltage) {
voltage = Math.copySign(Constants.Intaker.intakeRotaterManualVoltage, voltage);
}
manualVoltage = MathUtil.clamp(voltage, -12.0, 12.0);
leftIntakeRotater.setControl(voltageRequest.withOutput(manualVoltage));
followLeft();
}
public void stop() {
manualVoltage = 0.0;
leftIntakeRotater.setControl(voltageRequest.withOutput(0.0));
followLeft();
}
private void followLeft() {
rightIntakeRotater.setControl(rightFollower);
}
@Override
public void periodic() {
table.getEntry("goal").setString(goal.name());
table.getEntry("zeroed").setBoolean(zeroed);
table.getEntry("position").setDouble(getPosition());
table.getEntry("atGoal").setBoolean(atGoal());
table.getEntry("manualVoltage").setDouble(manualVoltage);
}
}
src/main/java/frc/robot/subsystems/IntakerSubsystem.java
-100
View File
@@ -1,100 +0,0 @@
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 double getGravityFFVolts(){
double intakerotaterPosition = getIntakerotaterPosition();
double gravityFFVolts = Constants.Intaker.IntakeRotaterGravityFF * intakerotaterPosition;
return gravityFFVolts;
}
public void setIntakerRotaterVelocityRps(double IntakeRotaterTargetVelocity) {
double gravityFeedforward = getGravityFFVolts();
intakerotater.setControl(velocityRequest.withVelocity(IntakeRotaterTargetVelocity).withFeedForward(gravityFeedforward));// 重力前馈);
}
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.set(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));
// }
// }
public void setIntakerGetterSpeed(double speed) {
intakegetter.set(speed);
}
public void zeroEncoder(){
intakerotater.setPosition(0);
stopIntakerotater();
}
public void IntakerDownNonStop(){
intakerotater.set(-0.1);
}
public void IntakerUpNonStop(){
intakerotater.set(0.2);
}
public void setIntakerRotater(double rotaterspeed){
intakerotater.set(rotaterspeed);
}
public void stopAll() {
stopIntakerotater();
setIntakerGetterSpeed(0);
}
}
src/main/java/frc/robot/subsystems/LimelightSubsystem.java
+55
View File
@@ -0,0 +1,55 @@
package frc.robot.subsystems;
import edu.wpi.first.math.geometry.Pose2d;
import edu.wpi.first.networktables.NetworkTableInstance;
import edu.wpi.first.wpilibj2.command.SubsystemBase;
import frc.robot.Constants;
/**
* 对现有 LimelightHelpers 的薄只读封装。
*
* 本类不做视觉融合，也不控制任何机构；只给 Superstructure 提供稳定的
* 目标和距离读取接口。
*/
public class LimelightSubsystem extends SubsystemBase {
private final String limelightName;
public LimelightSubsystem() {
this(Constants.Limelight.LIMELIGHT_NAME_Shooter);
}
public LimelightSubsystem(String limelightName) {
this.limelightName = limelightName;
}
public boolean hasTarget() {
return LimelightHelpers.getTV(limelightName);
}
public double getTargetYaw() {
return LimelightHelpers.getTX(limelightName);
}
public double getTargetPitch() {
return LimelightHelpers.getTY(limelightName);
}
public double getDistanceToTarget() {
// DriveControls 已经把基于场地位姿计算的距离发布到 AutoControl；
// 这里复用该值，保留旧定位和目标计算逻辑。
return NetworkTableInstance.getDefault()
.getTable("AutoControl")
.getEntry("distanceToHub")
.getDouble(0.0);
}
public Pose2d getEstimatedPose() {
return LimelightHelpers.getBotPoseEstimate_wpiBlue_MegaTag2(limelightName).pose;
}
public double getShooterSetpointByDistance() {
// 初始测试用临时映射；飞轮和背板调好后应替换为实测射击表。
double distance = getDistanceToTarget();
return Constants.Shooter.defaultFlywheelTargetRps + (distance - 2.5) * 4.0;
}
}
src/main/java/frc/robot/subsystems/ShooterSubsystem.java
+196 -106
View File
@@ -3,15 +3,15 @@ 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.controls.VelocityVoltage;
import com.ctre.phoenix6.controls.VoltageOut;
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.MathUtil;
import edu.wpi.first.math.controller.ProfiledPIDController;
import edu.wpi.first.math.geometry.Pose2d;
import edu.wpi.first.math.trajectory.TrapezoidProfile.Constraints;
import edu.wpi.first.networktables.NetworkTable;
import edu.wpi.first.networktables.NetworkTableInstance;
@@ -19,158 +19,261 @@ import edu.wpi.first.wpilibj.Encoder;
import edu.wpi.first.wpilibj2.command.SubsystemBase;
import frc.robot.Constants;
/**
* Shooter 总成的硬件封装。
*
* 飞轮和 shooter 内部 conveyor 都使用电压开环输出。
* 背板控制沿用旧代码中的电机、编码器和 PID 调参流程。
*/
public class ShooterSubsystem extends SubsystemBase {
// 定义飞轮、传动和背板电机
public TalonFX flywheelMotorLeft;
private TalonFX flywheelMotorRight;
public TalonFX conveyorMotor;
public TalonFXS backboardMotor;
public Encoder backboardEncoder;
public final TalonFX leftFlywheel;
public final TalonFX rightFlywheel;
public final TalonFX leftConveyor;
public final TalonFX rightConveyor;
// PID 控制器，控制背板角度
public ProfiledPIDController backboardPID;
private Follower flyWheelFollower;
// 保留旧字段别名，让原有 dashboard tuner 在重构后仍能指向左飞轮和左 conveyor。
public final TalonFX flywheelMotorLeft;
public final TalonFX conveyorMotor;
private NetworkTable shooterNetworkTable;
public final TalonFXS backboardMotor;
public final Encoder backboardEncoder;
public final ProfiledPIDController backboardPID;
private final VoltageOut flywheelVoltageRequest = new VoltageOut(0);
private final VoltageOut conveyorVoltageRequest = new VoltageOut(0);
private final VelocityVoltage velocityRequest = new VelocityVoltage(0);
private final Follower flywheelFollower;
private final Follower conveyorFollower;
private final NetworkTable shooterNetworkTable;
private double targetFlywheelRps = 0.0;
private double targetLeftFlywheelRps = 0.0;
private double targetRightFlywheelRps = 0.0;
private double shooterConveyorPercent = 0.0;
// 初始化各个部件
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);
leftFlywheel = new TalonFX(Constants.Shooter.leftFlywheelID);
rightFlywheel = new TalonFX(Constants.Shooter.rightFlywheelID);
leftConveyor = new TalonFX(Constants.Shooter.leftConveyorID);
rightConveyor = new TalonFX(Constants.Shooter.rightConveyorID);
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);
flywheelMotorLeft = leftFlywheel;
conveyorMotor = leftConveyor;
leftFlywheel.getConfigurator().apply(Constants.Shooter.flyWheelSlot0Configs);
rightFlywheel.getConfigurator().apply(Constants.Shooter.flyWheelSlot0Configs);
leftConveyor.getConfigurator().apply(Constants.Shooter.conveyorSlot0Configs);
rightConveyor.getConfigurator().apply(Constants.Shooter.conveyorSlot0Configs);
var motorOutputConfigs = new MotorOutputConfigs();
motorOutputConfigs.Inverted = InvertedValue.Clockwise_Positive;
leftFlywheel.getConfigurator().apply(motorOutputConfigs);
leftConveyor.getConfigurator().apply(motorOutputConfigs);
flywheelFollower = new Follower(leftFlywheel.getDeviceID(), MotorAlignmentValue.Opposed).withUpdateFreqHz(50);
conveyorFollower = new Follower(leftConveyor.getDeviceID(), MotorAlignmentValue.Opposed).withUpdateFreqHz(50);
setRightFollowLeft();
backboardMotor = new TalonFXS(Constants.Shooter.backboardMotorID);
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);
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");
}
public void setRightFollowLeft(){
flywheelMotorRight.setControl(flyWheelFollower);
public void setRightFollowLeft() {
// 成对机构机械方向相反，因此右侧电机以反向 follower 跟随左侧电机。
rightFlywheel.setControl(flywheelFollower);
rightConveyor.setControl(conveyorFollower);
}
public void applyLeftConfigurationToRight(){
public void applyLeftConfigurationToRight() {
Slot0Configs configs = new Slot0Configs();
flywheelMotorLeft.getConfigurator().refresh(configs);
flywheelMotorRight.getConfigurator().apply(configs);
leftFlywheel.getConfigurator().refresh(configs);
rightFlywheel.getConfigurator().apply(configs);
}
public void setFlywheelVelocity(double leftRps, double rightRps) {
// 对外接口使用 RPS，便于 Superstructure 阅读；实际下发仍是电压开环。
targetLeftFlywheelRps = leftRps;
targetRightFlywheelRps = rightRps;
targetFlywheelRps = (Math.abs(leftRps) + Math.abs(rightRps)) / 2.0;
// 设置飞轮的速度
public void setFlywheelSpeedByRPS(double targetSpeed) {
flywheelMotorLeft.setControl(velocityRequest.withVelocity(targetSpeed)); //RPS 转每秒
double requestedVoltage = targetFlywheelRps * Constants.Shooter.flywheelVoltsPerRps;
requestedVoltage = MathUtil.clamp(requestedVoltage, -12.0, 12.0);
leftFlywheel.setControl(flywheelVoltageRequest.withOutput(requestedVoltage));
setRightFollowLeft();
// System.out.println("set flywheel "+targetSpeed);
}
// 设置传动的速度
public void setFlywheelVelocityByDistance(double distance) {
// 临时距离映射。拿到真实射击数据后，应替换为实测表或重新调常量。
double clampedDistance = MathUtil.clamp(distance, 1.2, 5.0);
double targetRps = Constants.Shooter.defaultFlywheelTargetRps + (clampedDistance - 2.5) * 4.0;
setFlywheelVelocity(targetRps, targetRps);
}
public void setFlywheelSpeedByRPS(double targetSpeed) {
setFlywheelVelocity(targetSpeed, targetSpeed);
}
public void setFlywheelVoltage(double volts) {
targetLeftFlywheelRps = 0.0;
targetRightFlywheelRps = 0.0;
targetFlywheelRps = 0.0;
leftFlywheel.setControl(flywheelVoltageRequest.withOutput(MathUtil.clamp(volts, -12.0, 12.0)));
setRightFollowLeft();
}
public void runShooterConveyor(double percent) {
shooterConveyorPercent = MathUtil.clamp(percent, -1.0, 1.0);
leftConveyor.setControl(conveyorVoltageRequest.withOutput(shooterConveyorPercent * 12.0));
setRightFollowLeft();
}
public void stopShooterConveyor() {
runShooterConveyor(0.0);
}
public void setConveyorSpeedByRPS(double speed) {
conveyorMotor.setControl(velocityRequest.withVelocity(speed)); // 设置传动电机速度
}
public void setConveyorSpeedOpen(double speed){
conveyorMotor.set(speed);
leftConveyor.setControl(velocityRequest.withVelocity(speed));
setRightFollowLeft();
}
//调试用
public void setBackboardSpeedByRPS(double speed){
if (speed > 0 && speed < 10) {
speed = 10;
public void setConveyorSpeedOpen(double speed) {
runShooterConveyor(speed);
}
public void stopFlywheel() {
setFlywheelVoltage(Constants.Shooter.flywheelIdleVoltage);
}
public void stopAll() {
stopFlywheel();
stopShooterConveyor();
backboardMotor.set(0);
}
public void stopMotors() {
stopAll();
}
public boolean atSpeed() {
// 虽然飞轮控制是电压开环，但仍读取 TalonFX 速度作为喂球门控。
if (targetFlywheelRps <= 1.0) {
return false;
}
return Math.abs(getLeftFlywheelVelocity() - targetLeftFlywheelRps) <= Constants.Shooter.flywheelSpeedToleranceRps
&& Math.abs(Math.abs(getRightFlywheelVelocity()) - Math.abs(targetRightFlywheelRps)) <= Constants.Shooter.flywheelSpeedToleranceRps;
}
public double getLeftFlywheelVelocity() {
return leftFlywheel.getVelocity().getValueAsDouble();
}
public double getRightFlywheelVelocity() {
return rightFlywheel.getVelocity().getValueAsDouble();
}
public double getAverageFlywheelVelocity() {
return (Math.abs(getLeftFlywheelVelocity()) + Math.abs(getRightFlywheelVelocity())) / 2.0;
}
public double getFlywheelVelocityDifference() {
return Math.abs(getLeftFlywheelVelocity()) - Math.abs(getRightFlywheelVelocity());
}
public double getTargetFlywheelRps() {
return targetFlywheelRps;
}
public void setBackboardSpeedByRPS(double speed) {
if (speed > 0 && speed < 10) {
speed = 10;
} else if (speed < 0 && speed > -10) {
speed = -10;
speed = -10;
}
backboardMotor.setControl(velocityRequest.withVelocity(speed));
}
// 设置背板的目标位置，通过PID控制器来调整 范围0-3000
public void setBackboardPosition(double targetPosition) {
// 限制目标位置在上下限之间
// 保留旧背板机构的软件限位。
if (targetPosition > Constants.Shooter.backboardUpLimit) {
targetPosition = Constants.Shooter.backboardUpLimit; // 限制最大上限
targetPosition = Constants.Shooter.backboardUpLimit;
} else if (targetPosition < Constants.Shooter.backboardDownLimit) {
targetPosition = Constants.Shooter.backboardDownLimit; // 限制最小下限
targetPosition = Constants.Shooter.backboardDownLimit;
}
backboardPID.setGoal(targetPosition);
}
public void outputBackboard(){
// 使用PID控制器来计算背板的运动速度
public void outputBackboard() {
// 背板仍使用旧外部编码器和 ProfiledPID；Superstructure 只选择目标位置。
double pidOutput = backboardPID.calculate(getBackboardPosition());
pidOutput = Math.max(-Constants.Shooter.backboardSpeedMax, Math.min(Constants.Shooter.backboardSpeedMax, pidOutput)); // 限制输出范围
pidOutput = MathUtil.clamp(pidOutput, -Constants.Shooter.backboardSpeedMax, Constants.Shooter.backboardSpeedMax);
if (Math.abs(pidOutput) < 0.8) {
pidOutput = 0.0;
}//防止抖动
// // 设置背板电机的速度，根据PID输出进行调整
pidOutput = 0.0;
}
shooterNetworkTable.getEntry("backboardPIDOutput").setDouble(pidOutput);
setBackboardSpeedByRPS(pidOutput);
}
// 停止所有电机
public void stopMotors() {
flywheelMotorLeft.set(0);
flywheelMotorRight.set(0);
conveyorMotor.set(0);
backboardMotor.set(0);
}
// 获取背板当前角度
public double getBackboardPosition() {
return backboardEncoder.getDistance(); // 返回背板电机编码器的当前角度
return backboardEncoder.getDistance();
}
// 检查背板是否到达目标角度
public boolean isBackboardAtTarget() {
return backboardPID.atGoal(); // 判断背板是否到达目标角度
return backboardPID.atGoal();
}
public void conveyorWaitForAcceleration(){
conveyorMotor.set(0);
public void conveyorWaitForAcceleration() {
stopShooterConveyor();
}
public void conveyorRun(){
setConveyorSpeedByRPS(Constants.Shooter.conveyorSpeed);
public void conveyorRun() {
runShooterConveyor(Constants.Superstructure.shooterFeedPercent);
}
public void resetbackboardencoder(){
public void resetbackboardencoder() {
backboardEncoder.reset();
}
public double getConveyorStatorCurrent(){
double statorCurrent = conveyorMotor.getStatorCurrent().getValueAsDouble();
return statorCurrent;
public double getConveyorStatorCurrent() {
return leftConveyor.getStatorCurrent().getValueAsDouble();
}
public double getDistanceToHub() {
return NetworkTableInstance.getDefault().getTable("AutoControl").getEntry("distanceToHub").getDouble(0.0);
}
public double getDistanceToPassball() {
return NetworkTableInstance.getDefault().getTable("AutoControl").getEntry("distanceToPassball").getDouble(0.0);
}
@Override
public void periodic() {
backboardPID.calculate(getBackboardPosition());
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("flywheelSpeed").setDouble(getAverageFlywheelVelocity());
shooterNetworkTable.getEntry("leftFlywheelSpeed").setDouble(getLeftFlywheelVelocity());
shooterNetworkTable.getEntry("rightFlywheelSpeed").setDouble(getRightFlywheelVelocity());
shooterNetworkTable.getEntry("flywheelTargetSpeed").setDouble(targetFlywheelRps);
shooterNetworkTable.getEntry("flywheelAtSpeed").setBoolean(atSpeed());
shooterNetworkTable.getEntry("shooterConveyorPercent").setDouble(shooterConveyorPercent);
shooterNetworkTable.getEntry("conveyorSpeed").setDouble(leftConveyor.getVelocity().getValueAsDouble());
shooterNetworkTable.getEntry("backboardCurrentRate").setDouble(getBackboardPosition());
shooterNetworkTable.getEntry("backboardTargetRate").setDouble(backboardPID.getSetpoint().position);
shooterNetworkTable.getEntry("isBackboardAtTarget").setBoolean(isBackboardAtTarget());
@@ -178,17 +281,4 @@ public class ShooterSubsystem extends SubsystemBase {
shooterNetworkTable.getEntry("distancetopassball").setDouble(getDistanceToPassball());
shooterNetworkTable.getEntry("statorCurrent").setDouble(getConveyorStatorCurrent());
}
public double getDistanceToHub(){
//获得车子距离Hub的距离
return NetworkTableInstance.getDefault().getTable("AutoControl").getEntry("distanceToHub").getDouble(0.0);
}
public double getDistanceToPassball(){
return NetworkTableInstance.getDefault().getTable("AutoControl").getEntry("distanceToPassball").getDouble(0.0);
}
public void stopAll() {
setFlywheelSpeedByRPS(0);
setConveyorSpeedByRPS(0);
backboardMotor.set(0);
}
}
src/main/java/frc/robot/subsystems/Superstructure.java
+342
View File
@@ -0,0 +1,342 @@
package frc.robot.subsystems;
import edu.wpi.first.math.geometry.Rotation2d;
import edu.wpi.first.networktables.NetworkTable;
import edu.wpi.first.networktables.NetworkTableInstance;
import edu.wpi.first.wpilibj.DriverStation;
import edu.wpi.first.wpilibj.Timer;
import edu.wpi.first.wpilibj2.command.Command;
import edu.wpi.first.wpilibj2.command.Commands;
import edu.wpi.first.wpilibj2.command.SubsystemBase;
import frc.robot.Constants;
import frc.robot.subsystems.IntakeRotaterSubsystem.IntakePosition;
/**
* 统一协调所有得分相关机构。
*
* subsystem 只暴露硬件级动作，OperatorControls 只表达操作员意图；
* shooter、intake、conveyor、底盘瞄准和 Limelight 距离如何配合，
* 统一由这里决定。
*/
public class Superstructure extends SubsystemBase {
// 操作员或自动程序请求的目标状态。
public enum WantedState {
IDLE,
INTAKE,
PREP_SHOOT,
SHOOT,
EJECT,
MANUAL
}
// 机器人内部实际执行状态，用于动作排序和仪表盘诊断。
public enum SystemState {
DISABLED,
STOWED,
DEPLOYING_INTAKE,
INTAKING,
SPINNING_UP,
AIMING,
READY_TO_SHOOT,
SHOOTING,
CLEANUP,
EJECTING,
MANUAL_OVERRIDE
}
private final ShooterSubsystem shooter;
private final IntakeRollerSubsystem intakeRoller;
private final IntakeRotaterSubsystem intakeRotater;
private final ConveyorSubsystem conveyor;
private final CommandSwerveDrivetrain drive;
private final LimelightSubsystem limelight;
private final NetworkTable table = NetworkTableInstance.getDefault().getTable("Superstructure");
private WantedState wantedState = WantedState.IDLE;
private SystemState systemState = SystemState.STOWED;
private double stateStartTimestamp = Timer.getFPGATimestamp();
private double flywheelSpeedOffsetRps = 0.0;
private double backboardPositionOffset = 0.0;
public Superstructure(
ShooterSubsystem shooter,
IntakeRollerSubsystem intakeRoller,
IntakeRotaterSubsystem intakeRotater,
ConveyorSubsystem conveyor,
CommandSwerveDrivetrain drive,
LimelightSubsystem limelight
) {
this.shooter = shooter;
this.intakeRoller = intakeRoller;
this.intakeRotater = intakeRotater;
this.conveyor = conveyor;
this.drive = drive;
this.limelight = limelight;
}
public void requestIdle() {
wantedState = WantedState.IDLE;
}
public void requestIntake() {
wantedState = WantedState.INTAKE;
}
public void requestPrepShoot() {
wantedState = WantedState.PREP_SHOOT;
}
public void requestShoot() {
wantedState = WantedState.SHOOT;
}
public void requestEject() {
wantedState = WantedState.EJECT;
}
public void requestManual() {
wantedState = WantedState.MANUAL;
}
public Command requestIdleCommand() {
return Commands.runOnce(this::requestIdle, this);
}
public Command requestIntakeCommand() {
return Commands.runOnce(this::requestIntake, this);
}
public Command requestPrepShootCommand() {
return Commands.runOnce(this::requestPrepShoot, this);
}
public Command requestShootCommand() {
return Commands.runOnce(this::requestShoot, this);
}
public Command requestEjectCommand() {
return Commands.runOnce(this::requestEject, this);
}
public Command requestManualCommand() {
return Commands.runOnce(this::requestManual, this);
}
public WantedState getWantedState() {
return wantedState;
}
public SystemState getSystemState() {
return systemState;
}
public boolean canShoot() {
// 本赛季不使用球检测传感器，因此射击门控只检查机器人能实际测量的条件。
return shooter.atSpeed()
&& drive.isAimed();
}
public void adjustFlywheelSpeedOffset(double offsetRps) {
flywheelSpeedOffsetRps += offsetRps;
}
public void adjustBackboardPositionOffset(double offset) {
backboardPositionOffset += offset;
}
public void resetOffsets() {
flywheelSpeedOffsetRps = 0.0;
backboardPositionOffset = 0.0;
}
public void resetBackboardEncoder() {
shooter.resetbackboardencoder();
}
@Override
public void periodic() {
// 禁用状态优先级最高，防止旧按钮输入或自动请求在禁用后继续驱动机构。
if (DriverStation.isDisabled()) {
setSystemState(SystemState.DISABLED);
stopAllMechanisms();
publishTelemetry();
return;
}
updateAimingSetpoint();
switch (wantedState) {
case IDLE -> handleIdle();
case INTAKE -> handleIntake();
case PREP_SHOOT -> handlePrepShoot(false);
case SHOOT -> handlePrepShoot(true);
case EJECT -> handleEject();
case MANUAL -> handleManual();
}
publishTelemetry();
}
private void handleIdle() {
// 安全默认状态：传球不动，intake 收回，背板保持受控。
conveyor.hold();
intakeRoller.stop();
intakeRotater.stop();
shooter.stopFlywheel();
shooter.stopShooterConveyor();
holdBackboardAtZero();
setSystemState(SystemState.STOWED);
}
private void handleIntake() {
if (systemState != SystemState.DEPLOYING_INTAKE
&& systemState != SystemState.INTAKING) {
setSystemState(SystemState.DEPLOYING_INTAKE);
}
shooter.stopFlywheel();
shooter.stopShooterConveyor();
intakeRotater.deployToGround();
intakeRoller.intake();
conveyor.feedToShooter();
holdBackboardAtZero();
if (intakeRotater.atGoal()) {
setSystemState(SystemState.INTAKING);
}
// 本赛季没有球检测，因此 intake 通过时间退出。
// 真车确认 roller/conveyor 速度后，需要现场调整这个时间。
if (systemState == SystemState.INTAKING && timeInState() > 1.0) {
wantedState = WantedState.IDLE;
setSystemState(SystemState.STOWED);
}
}
private void handlePrepShoot(boolean feedWhenReady) {
if (systemState != SystemState.SPINNING_UP
&& systemState != SystemState.AIMING
&& systemState != SystemState.READY_TO_SHOOT
&& systemState != SystemState.SHOOTING
&& systemState != SystemState.CLEANUP) {
setSystemState(SystemState.SPINNING_UP);
}
intakeRotater.moveToHandoff();
intakeRoller.hold();
conveyor.hold();
shooter.stopShooterConveyor();
// 距离到射速的映射由 LimelightSubsystem 提供；操作员 offset 用于现场微调，
// 但不会绕过 Superstructure 状态机。
double targetRps = limelight.getShooterSetpointByDistance() + flywheelSpeedOffsetRps;
shooter.setFlywheelVelocity(targetRps, targetRps);
holdBackboardForShot();
if (shooter.atSpeed()) {
setSystemState(SystemState.AIMING);
}
if (shooter.atSpeed() && drive.isAimed() && intakeRotater.atGoal()) {
setSystemState(SystemState.READY_TO_SHOOT);
}
if (feedWhenReady && canShoot()) {
setSystemState(SystemState.SHOOTING);
}
if (systemState == SystemState.SHOOTING) {
// 只有 canShoot() 通过后才允许喂球。
conveyor.feedToShooter();
shooter.runShooterConveyor(Constants.Superstructure.shooterFeedPercent);
if (timeInState() > Constants.Superstructure.shootTimeoutSeconds) {
setSystemState(SystemState.CLEANUP);
}
}
if (systemState == SystemState.CLEANUP) {
conveyor.stop();
shooter.stopShooterConveyor();
if (timeInState() > Constants.Superstructure.cleanupSeconds) {
wantedState = WantedState.IDLE;
setSystemState(SystemState.STOWED);
}
}
}
private void handleEject() {
// Eject 会反转整条球路，但飞轮保持停止。
setSystemState(SystemState.EJECTING);
intakeRotater.deployToGround();
intakeRoller.outtake();
conveyor.reverse();
shooter.runShooterConveyor(Constants.Superstructure.shooterReversePercent);
shooter.stopFlywheel();
holdBackboardAtZero();
}
private void handleManual() {
setSystemState(SystemState.MANUAL_OVERRIDE);
shooter.stopFlywheel();
shooter.stopShooterConveyor();
conveyor.stop();
intakeRoller.stop();
}
private void updateAimingSetpoint() {
// Superstructure 可以更新底盘目标朝向，但不直接下发 swerve 模块或底盘速度命令。
if (limelight.hasTarget()) {
drive.setTargetHeading(drive.getRotation().plus(Rotation2d.fromDegrees(limelight.getTargetYaw())));
}
}
private void holdBackboardAtZero() {
shooter.setBackboardPosition(0.0);
if (shooter.isBackboardAtTarget()) {
shooter.backboardMotor.set(0.0);
} else {
shooter.outputBackboard();
}
}
private void holdBackboardForShot() {
shooter.setBackboardPosition(175.0 + backboardPositionOffset);
if (shooter.isBackboardAtTarget()) {
shooter.backboardMotor.set(0.0);
} else {
shooter.outputBackboard();
}
}
private void stopAllMechanisms() {
shooter.stopAll();
intakeRoller.stop();
intakeRotater.stop();
conveyor.stop();
}
private void setSystemState(SystemState nextState) {
if (systemState != nextState) {
systemState = nextState;
stateStartTimestamp = Timer.getFPGATimestamp();
}
}
private double timeInState() {
return Timer.getFPGATimestamp() - stateStartTimestamp;
}
private void publishTelemetry() {
// 发布状态和门控信号，方便判断机器人为什么还没进入射击。
table.getEntry("wantedState").setString(wantedState.name());
table.getEntry("systemState").setString(systemState.name());
table.getEntry("canShoot").setBoolean(canShoot());
table.getEntry("shooterAtSpeed").setBoolean(shooter.atSpeed());
table.getEntry("driveAimed").setBoolean(drive.isAimed());
table.getEntry("intakeAtGoal").setBoolean(intakeRotater.atGoal());
table.getEntry("flywheelSpeedOffsetRps").setDouble(flywheelSpeedOffsetRps);
table.getEntry("backboardPositionOffset").setDouble(backboardPositionOffset);
}
}