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/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);
}
}