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

Merge branch '1' of https://github.com/HarryWang163/FRC6494_2026Regional into 1

Browse Source
This commit is contained in:
OnDYCLaptop
2026-03-07 00:11:03 +08:00
parent 74bf1b992d 31b00a4cf3
commit 093a0b93c9
11 changed files with 396 additions and 72 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/main/java/frc/robot/Constants.java
+38 -16
View File
@@ -15,35 +15,30 @@ public final class Constants {
CrossingTrench
}
public class Shooter {
public static final double conveyorSpeed = 20;
public static final double conveyorSpeed =25;
public static final Slot0Configs flyWheelSlot0Configs = new Slot0Configs()
.withKP(0.0).withKI(0).withKD(0)
.withKV(0.0088).withKA(0.0).withKS(0.0);
.withKV(0.099).withKA(0.0).withKS(0.0);
public static final Slot0Configs conveyorSlot0Configs = new Slot0Configs()
.withKP(0.0).withKI(0).withKD(0)
.withKV(0.02).withKA(0.15).withKS(0);
.withKV(0.16).withKA(0.15).withKS(0);
public static final Slot0Configs backboardSlot0Configs = new Slot0Configs()
.withKP(0.01).withKI(0).withKD(0)
.withKV(0.008).withKA(0.0).withKS(0.010);
.withKV(0.03).withKA(0.0).withKS(0.010);
public class backboardPositionPID {
public static final double kP = 0.1;
public static final double kP = 0.3;
public static final double kI = 0.0;
public static final double kD = 0.0;
}
public static final double backboardSpeedMax = 30; //也就是output
public static final double backboardSpeedMax = 60; //也就是output
// 背板的上下限位角度
public static final double backboardUpLimit = 3000.0; // 背板上限角度（根据实际需求设置）
public static final double backboardDownLimit = 0.0; // 背板下限角度（根据实际需求设置）
}
public class ShooterCalculation{
public static final double positionNear = 0; // 近距离背板角度
public static final double positionMid = 1600; // 中距离背板角度
public static final double postionFar = 3200; // 远距离背板角度
}
public class AutoPositioning {
public static final double autoRotationkP = 0.1;
public static final double autoRotationForBumpTargetDegrees = 45;
@@ -83,14 +78,41 @@ public final class Constants {
public static final String LIMELIGHT_NAME_Shooter = "limelight-shooter";
public static final String LIMELIGHT_NAME_Intaker = "limelight-intaker";
public static final Boolean UsingMetaTag2 = true;
public static final int climbPipelineIndex = 8;
public static final int locatePipelineIndex = 0;
// 误差死区 / 对齐判定
public static final double AutoClimbToleranceTX = 1.5; // deg
public static final double AutoClimbToleranceTZ = 0.10; // m
public static final double AutoClimbToleranceRY = 2.0; // deg
// P 控制系数
public static final double AutoClimbKpTX = 0.020; // deg -> rad/s
public static final double AutoClimbKpTZ = 0.90; // m -> m/s
public static final double AutoClimbKpRY = 0.015; // deg -> rad/s
// 最小输出（防止快到目标时推不动）
public static final double AutoClimbMinVX = 0.18; // m/s
public static final double AutoClimbMinOmega = 0.10; // rad/s
// 最大输出（防止冲太猛）
public static final double AutoClimbMaxVX = 1.20; // m/s
public static final double AutoClimbMaxOmega = 0.60; // rad/s
// 远距离 / 大角度时额外限速，可后续微调
public static final double AutoClimbFastTurnThresholdTX = 15.0; // deg
public static final double AutoClimbFastTurnOmega = 0.45; // rad/s
}
public class Field {
public static final double RedHubPositionX = 11.914;
public static final double RedHubPositionY = 4.034;
public static final double PassingBallPosX = 0;
public static final double PassingBallPosY1 = 2.51;
public static final double PassingBallPosY2 = 5.556;
public static final double BlueHubPositionX = 4.625;
public static final double BlueHubPositionY = 4.034;
public static final double PassingBallPosY1 = 1.405;
public static final double PassingBallPosY2 = 6.664;
public static final double RedPassingBallPosX = 13.355;
public static final double BluePassingBallPosX = 3.184;
}
@@ -111,12 +133,12 @@ public final class Constants {
.withKV(0.00).withKA(0.00).withKS(0.00);
//如果 rotater 受重力影响明显，可能需要用 withKG/withGravityType/withGravityArmPositionOffset（看机构形式）(这句fromAI)
public static final double IntakeGetterSpeedforAuto = 0.0;
public static final double IntakeGetterSpeedforAuto = 0.4;
// 单位RPS
public static final double positionToleranceRotations = 0.00;
//rotater容差
public static final double IntakeGetterK = 0.3; // intake getter速度与触发器输入的比例
public static final double Getterminspeed = 0.2; // intake getter的基础速度（即触发器输入为0时的速度，正值表示默认向内转）
public static final double Getterminspeed = 0.3; // intake getter的基础速度（即触发器输入为0时的速度，正值表示默认向内转）
}
public static class Climber {
src/main/java/frc/robot/RobotContainer.java
+19 -4
View File
@@ -9,6 +9,7 @@ import edu.wpi.first.wpilibj.smartdashboard.SendableChooser;
import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
import edu.wpi.first.wpilibj2.command.Command;
import edu.wpi.first.wpilibj2.command.Commands;
import edu.wpi.first.wpilibj2.command.InstantCommand;
import edu.wpi.first.wpilibj2.command.button.CommandXboxController;
import edu.wpi.first.wpilibj2.command.button.RobotModeTriggers;
import edu.wpi.first.wpilibj2.command.button.Trigger;
@@ -58,7 +59,7 @@ public class RobotContainer {
private final DriveControls driveControls = new DriveControls(drivetrain, controllerlower, robotStatusManager );
private final ShooterControls shooterControls = new ShooterControls(shooterSubsystem, controllerupper,robotStatusManager);
private final ClimberControls climberControls = new ClimberControls(climberSubsystem, controllerupper);
private final IntakerControls intakerControls = new IntakerControls(IntakerSubsystem, controllerupper);
private final IntakerControls intakerControls = new IntakerControls(IntakerSubsystem, shooterSubsystem, controllerupper);
private final DriveGainsTuner driveGainsTuner = new DriveGainsTuner(drivetrain);
private ConfigTalonFXMotorTuner configFlywheelTuner = new ConfigTalonFXMotorTuner(shooterSubsystem.flywheelMotorLeft, "flywheel", Constants.Shooter.flyWheelSlot0Configs);
private ConfigTalonFXMotorTuner configConveyorTuner = new ConfigTalonFXMotorTuner(shooterSubsystem.conveyorMotor, "conveyor", Constants.Shooter.conveyorSlot0Configs);
@@ -118,9 +119,19 @@ public class RobotContainer {
// Start：强行使用metaTag2全场定位
controllerlower.start().whileTrue(drivetrain.run(drivetrain::forceUsingLimelightMT2));
//LM 按下时autoaimming
controllerlower.leftStick().onTrue(robotStatusManager.setStatusCommand(RobotStatus.AutoAimming));
controllerlower.leftStick().onFalse(robotStatusManager.setStatusCommand(RobotStatus.AllTelop));
//LM 按下时autoaimming，松开时alltelop
// controllerlower.leftStick().onTrue(robotStatusManager.setStatusCommand(RobotStatus.AutoAimming));
// controllerlower.leftStick().onFalse(robotStatusManager.setStatusCommand(RobotStatus.AllTelop));
//双操均可触发autoaimming模式，松开时alltelop
Trigger autoAim = controllerupper.rightBumper().or(controllerlower.rightStick());
autoAim.onTrue(robotStatusManager.setStatusCommand(RobotStatus.AutoAimming));
autoAim.onFalse(robotStatusManager.setStatusCommand(RobotStatus.AllTelop));
//双操均可触发passingball模式，松开时alltelop
Trigger autoAim2 = controllerupper.leftBumper().or(controllerlower.leftStick());
autoAim2.onTrue(robotStatusManager.setStatusCommand(RobotStatus.PassingBall));
autoAim2.onFalse(robotStatusManager.setStatusCommand(RobotStatus.AllTelop));
controllerlower.leftTrigger()
.onTrue(robotStatusManager.setStatusCommand(RobotStatus.CrossingTrench))
@@ -129,6 +140,10 @@ public class RobotContainer {
controllerlower.rightTrigger()
.onTrue(robotStatusManager.setStatusCommand(RobotStatus.CrossingBump))
.onFalse(robotStatusManager.setStatusCommand(RobotStatus.AllTelop));
controllerlower.a()
.onTrue(robotStatusManager.setStatusCommand(RobotStatus.Climbing))
.onFalse(robotStatusManager.setStatusCommand(RobotStatus.AllTelop));
}
private void configueShooter(){
src/main/java/frc/robot/controls/Autocommand.java
+20
View File
@@ -1,5 +1,6 @@
package frc.robot.controls;
import edu.wpi.first.math.geometry.Translation2d;
import edu.wpi.first.networktables.NetworkTable;
import edu.wpi.first.networktables.NetworkTableInstance;
import edu.wpi.first.wpilibj2.command.Command;
@@ -11,6 +12,7 @@ import edu.wpi.first.wpilibj2.command.SequentialCommandGroup;
import frc.robot.subsystems.ClimberSubsystem;
import frc.robot.RobotStatusManager;
import frc.robot.subsystems.IntakerSubsystem;
import frc.robot.subsystems.LimelightSupplier;
import frc.robot.subsystems.ShooterSubsystem;
import frc.robot.subsystems.CommandSwerveDrivetrain;
import frc.robot.Constants;
@@ -58,6 +60,24 @@ import frc.robot.Constants.RobotStatus;
intaker.stopIntakerotater();; // 停止旋转
}, intaker));
}
public static Command autoAlignToClimb(CommandSwerveDrivetrain drivetrain) {
return Commands.sequence(
new InstantCommand(() -> {
LimelightSupplier.setPipeline(Constants.Limelight.climbPipelineIndex);
}),
Commands.runEnd(
() -> {
drivetrain.driveToAprilTag();
},
() -> {
LimelightSupplier.setPipeline(Constants.Limelight.locatePipelineIndex);
},
drivetrain
).until(() -> drivetrain.isAlignedToAprilTag())
);
}
}
src/main/java/frc/robot/controls/ClimberControls.java
+8
View File
@@ -1,11 +1,16 @@
package frc.robot.controls;
import edu.wpi.first.math.geometry.Translation2d;
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.InstantCommand;
import edu.wpi.first.wpilibj2.command.RunCommand;
import edu.wpi.first.wpilibj2.command.button.CommandXboxController;
import frc.robot.Constants;
import frc.robot.subsystems.ClimberSubsystem;
import frc.robot.subsystems.LimelightSupplier;
public class ClimberControls {
@@ -53,4 +58,7 @@ public class ClimberControls {
public Command resetClimberEncoderCommand(ClimberSubsystem climber) {
return Commands.runOnce(climber::zeroEncoder, climber);
}
}
src/main/java/frc/robot/controls/DriveControls.java
+26 -5
View File
@@ -19,12 +19,14 @@ import frc.robot.Constants.DriveMode;
import frc.robot.Constants.RobotStatus;
import frc.robot.generated.TunerConstants;
import frc.robot.subsystems.CommandSwerveDrivetrain;
import frc.robot.subsystems.LimelightSupplier;
import java.util.Optional;
import edu.wpi.first.wpilibj.DriverStation;
import edu.wpi.first.wpilibj.DriverStation.Alliance;
public class DriveControls {
public boolean isRedAlliance;
private final CommandSwerveDrivetrain drivetrain;
@@ -122,6 +124,8 @@ public class DriveControls {
// 将距离值写入 NetworkTable
autoControlNetworkTable.getEntry("distanceToHub").setDouble(distanceAndRotation[0]);
autoControlNetworkTable.getEntry("angleDifferenceToHub").setDouble(distanceAndRotation[1]);
autoControlNetworkTable.getEntry("distanceToPassball").setDouble(calculateDistanceAndRotationToPassBall()[0]);
autoControlNetworkTable.getEntry("angleDifferenceToPassball").setDouble(calculateDistanceAndRotationToPassBall()[1]);
switch (robotStatusManager.getStatus()) {
case Stopped:
@@ -130,10 +134,12 @@ public class DriveControls {
vomega = 0;
break;
case PassingBall:
LimelightSupplier.setPipeline(Constants.Limelight.locatePipelineIndex);
vomega = calculateRotationSpeedFromRotationAngle(calculateDistanceAndRotationToPassBall()[1]);
autoControlNetworkTable.getEntry("autoRotationRate").setDouble(vomega);
break;
case AutoAimming:
LimelightSupplier.setPipeline(Constants.Limelight.locatePipelineIndex);
vomega = calculateRotationSpeedFromRotationAngle(distanceAndRotation[1]);
autoControlNetworkTable.getEntry("autoRotationRate").setDouble(vomega);
break;
@@ -141,6 +147,7 @@ public class DriveControls {
autoControlNetworkTable.getEntry("autoRotationRate").setDouble(Double.NaN);
break;
case Climbing:
alignToClimb();
break;
case CrossingBump:
double differenceBump = calculateDifferenceToTwoTarget(drivetrain.getState().Pose.getY(), Constants.AutoPositioning.bumpY[0], Constants.AutoPositioning.bumpY[1]);
@@ -152,6 +159,7 @@ public class DriveControls {
Math.max(Math.abs(raw), 0.4),
raw
);
vy = isRedAlliance ? -vy : vy;
}
autoControlNetworkTable.getEntry("BumpDifference").setDouble(differenceBump);
@@ -181,6 +189,7 @@ public class DriveControls {
Math.max(Math.abs(raw), 0.4),
raw
);
vy = isRedAlliance ? -vy : vy;
}
autoControlNetworkTable.getEntry("TrenchDifference").setDouble(differenceTrench);
Rotation2d currentAngle = drivetrain.getState().Pose.getRotation();
@@ -204,7 +213,8 @@ public class DriveControls {
if (fieldCentricEnabled || isAutoLike) {
// 自动状态用 BlueAlliance（不翻转），手柄 field-centric 仍用原 fieldCentric（OperatorPerspective）
var chosen = isAutoLike ? fieldCentricAuto : fieldCentric;
//var chosen = isAutoLike ? fieldCentricAuto : fieldCentric;
var chosen = fieldCentric;
return chosen
.withVelocityX(vx)
.withVelocityY(vy)
@@ -233,8 +243,8 @@ public class DriveControls {
}
public double[] calculateDistanceAndRotationToHub() {
Pose2d currentPose = drivetrain.getState().Pose; // 获取机器人当前的位置和角度
double targetX = Constants.Field.RedHubPositionX; // 目标 X 坐标
double targetY = Constants.Field.RedHubPositionY; // 目标 Y 坐标
double targetX = isRedAlliance ? Constants.Field.RedHubPositionX : Constants.Field.BlueHubPositionX;
double targetY = isRedAlliance ? Constants.Field.RedHubPositionY : Constants.Field.BlueHubPositionY;
// 计算目标角度（相对于场地坐标系）
double deltaX = targetX - currentPose.getX();
@@ -261,7 +271,7 @@ public class DriveControls {
}
public double[] calculateDistanceAndRotationToPassBall() {
Pose2d currentPose = drivetrain.getState().Pose; // 获取机器人当前的位置和角度
double targetX = Constants.Field.PassingBallPosX; // 目标 X 坐标
double targetX = isRedAlliance ? Constants.Field.RedPassingBallPosX : Constants.Field.BluePassingBallPosX; // 目标 X 坐标
double targetY1 = Constants.Field.PassingBallPosY1; // 目标 Y 坐标
double targetY2 = Constants.Field.PassingBallPosY2; // 目标 Y 坐标
@@ -389,6 +399,17 @@ public class DriveControls {
}
public Command getAllianceColorCommand() {
return Commands.runOnce(this::setTeamColors);
}
}
public void alignToClimb(){
LimelightSupplier.setPipeline(Constants.Limelight.climbPipelineIndex);
if(!drivetrain.isAlignedToAprilTag()){
drivetrain.driveToAprilTag();
}
else{
LimelightSupplier.setPipeline(Constants.Limelight.locatePipelineIndex);
drivetrain.stop();
}
}
}
src/main/java/frc/robot/controls/IntakerControls.java
+12 -4
View File
@@ -6,16 +6,19 @@ import edu.wpi.first.wpilibj2.command.Command;
import edu.wpi.first.wpilibj2.command.Commands;
import edu.wpi.first.wpilibj2.command.button.CommandXboxController;
import frc.robot.subsystems.IntakerSubsystem;
import frc.robot.subsystems.ShooterSubsystem;
import frc.robot.Constants;
public class IntakerControls {
private final IntakerSubsystem IntakerSubsystem;
private final CommandXboxController controller;
private final ShooterSubsystem ShooterSubsystem;
//private static final double ROTATER_STEP_PER_CYCLE = 0.00;
public IntakerControls(IntakerSubsystem Intaker, CommandXboxController controller) {
public IntakerControls(IntakerSubsystem Intaker, ShooterSubsystem Shooter, CommandXboxController controller) {
this.ShooterSubsystem = Shooter;
this.IntakerSubsystem = Intaker;
this.controller = controller;
}
@@ -69,10 +72,15 @@ public class IntakerControls {
public Command defaultIntakerCommand() {
return Commands.run(() -> {
double getterspeed = controller.getLeftTriggerAxis() * Constants.Intaker.IntakeGetterK + Constants.Intaker.Getterminspeed;
if (controller.getLeftTriggerAxis() < 0.1) {
getterspeed = 0;
double getterspeed = 0;
double conveyoyspeedforintake = 15;
double statorCurrent = ShooterSubsystem.getConveyorStatorCurrent();
if (controller.getLeftTriggerAxis() > 0.1) {
getterspeed = controller.getLeftTriggerAxis() * Constants.Intaker.IntakeGetterK + Constants.Intaker.Getterminspeed;
ShooterSubsystem.setConveyorSpeedByRPS(conveyoyspeedforintake);
}
IntakerSubsystem.setIntakerGetterSpeed(getterspeed);
if(controller.getLeftY() < -0.5){
src/main/java/frc/robot/controls/ShooterControls.java
+6 -11
View File
@@ -8,6 +8,7 @@ import edu.wpi.first.wpilibj2.command.button.CommandXboxController;
import frc.robot.Constants;
import frc.robot.RobotStatusManager;
import frc.robot.Constants.Intaker;
import frc.robot.Constants.RobotStatus;
import frc.robot.subsystems.IntakerSubsystem;
import frc.robot.subsystems.ShooterSubsystem;
@@ -37,7 +38,7 @@ public class ShooterControls {
break;
case AllTelop:
case PassingBall:
var xP = shooterSubsystem.getDistance();
var xP = shooterSubsystem.getDistanceToPassball();
//xP = Math.max(1.2, Math.min(4.5, xP));
flywheelSpeed = 0.6956596811733782*xP*xP*xP*xP*xP*xP*xP-14.164408860183267*xP*xP*xP*xP*xP*xP+119.7115375240918*xP*xP*xP*xP*xP-543.0984876677477*xP*xP*xP*xP+1425.1928543240738*xP*xP*xP-2156.199296490315*xP*xP+1736.568139600292*xP-516.097598057351;
flywheelSpeed += flywheelSpeedOffset;
@@ -48,12 +49,9 @@ public class ShooterControls {
flywheelSpeed = 0;
conveyorSpeed = 0;
}
if(shooterSubsystem.flywheelMotorLeft.getVelocity().getValueAsDouble()<flywheelSpeed*0.7){
if(shooterSubsystem.flywheelMotorLeft.getVelocity().getValueAsDouble()<flywheelSpeed*0.95){
conveyorSpeed = 0;
}
// flywheelSpeed = calculateFlywheelSpeedOnlywithOffset(60);
// double flywheelSpeed = calculateFlywheelSpeed(0);
// double BackboardPosition = calculateBackboardPosition(0);
shooterControlTable.getEntry("flywheelTargetSpeed").setDouble(flywheelSpeed);
shooterControlTable.getEntry("conveyerTargetSpeed").setDouble(conveyorSpeed);
shooterControlTable.getEntry("backboardTargetPosition").setDouble(backboardPosition);
@@ -67,7 +65,7 @@ public class ShooterControls {
}
break;
case AutoAimming:
var x = shooterSubsystem.getDistance();
var x = shooterSubsystem.getDistanceToHub();
x = Math.max(1.2, Math.min(4.5, x));
flywheelSpeed = 0.6956596811733782*x*x*x*x*x*x*x-14.164408860183267*x*x*x*x*x*x+119.7115375240918*x*x*x*x*x-543.0984876677477*x*x*x*x+1425.1928543240738*x*x*x-2156.199296490315*x*x+1736.568139600292*x-516.097598057351;
flywheelSpeed += flywheelSpeedOffset;
@@ -78,17 +76,14 @@ public class ShooterControls {
flywheelSpeed = 0;
conveyorSpeed = 0;
}
if(shooterSubsystem.flywheelMotorLeft.getVelocity().getValueAsDouble()<flywheelSpeed*0.7){
if(shooterSubsystem.flywheelMotorLeft.getVelocity().getValueAsDouble()<flywheelSpeed*0.95){
conveyorSpeed = 0;
}
// flywheelSpeed = calculateFlywheelSpeedOnlywithOffset(60);
// double flywheelSpeed = calculateFlywheelSpeed(0);
// double BackboardPosition = calculateBackboardPosition(0);
shooterControlTable.getEntry("flywheelTargetSpeed").setDouble(flywheelSpeed);
shooterControlTable.getEntry("conveyerTargetSpeed").setDouble(conveyorSpeed);
shooterControlTable.getEntry("backboardTargetPosition").setDouble(backboardPosition);
shooterSubsystem.setFlywheelSpeedByRPS(flywheelSpeed);
shooterSubsystem.setConveyorSpeedByRPS(conveyorSpeed);
shooterSubsystem.setConveyorSpeedOpen(5);
shooterSubsystem.setBackboardPosition(backboardPosition);
if(shooterSubsystem.isBackboardAtTarget()){
shooterSubsystem.backboardMotor.set(0);
src/main/java/frc/robot/subsystems/CommandSwerveDrivetrain.java
+131 -5
View File
@@ -15,6 +15,7 @@ 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;
@@ -26,6 +27,7 @@ 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.subsystems.LimelightHelpers.RawFiducial;
import frc.robot.Constants;
import edu.wpi.first.networktables.*;
@@ -46,6 +48,7 @@ public class CommandSwerveDrivetrain extends TunerSwerveDrivetrain implements Su
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 DriveMode m_driveMode = DriveMode.OTHER;
@@ -63,6 +66,7 @@ public class CommandSwerveDrivetrain extends TunerSwerveDrivetrain implements Su
private Field2d shooter_ll_field;
private Field2d intaker_ll_field;
public CommandSwerveDrivetrain(
SwerveDrivetrainConstants drivetrainConstants,
SwerveModuleConstants<?, ?, ?>... modules
@@ -130,8 +134,11 @@ public class CommandSwerveDrivetrain extends TunerSwerveDrivetrain implements Su
);
}
public void stop() {
setControl(
new SwerveRequest.ApplyRobotSpeeds().withSpeeds(new ChassisSpeeds(0, 0, 0))
);
}
private static final double kMaxVisionJumpMeters = 1.0;
private static final double kMaxOmegaDegPerSec = 360.0;
@@ -263,19 +270,134 @@ public class CommandSwerveDrivetrain extends TunerSwerveDrivetrain implements Su
// 2) 转太快：拒绝（角速度你可以从 Pigeon2 或 drivetrain state 拿）
double omegaDegPerSec = getPigeon2().getAngularVelocityZWorld().getValueAsDouble(); // 若你有这个信号
if (Math.abs(omegaDegPerSec) > kMaxOmegaDegPerSec) {
System.out.println("NoUsingMetaTag2ForTooFastRotationSpeed");
//System.out.println("NoUsingMetaTag2ForTooFastRotationSpeed");
return false;
}
// 3) 跳变太大：拒绝（Phoenix6 推荐的稳定策略）
//Pose2d current = getState().Pose;
if (currentPose.getTranslation().getDistance(mt2.pose.getTranslation()) > kMaxVisionJumpMeters) {
System.out.println("NoUsingMetaTag2ForTooChangingPose");
//System.out.println("NoUsingMetaTag2ForTooChangingPose");
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);
// ===== 调试输出 =====
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() {
@@ -287,7 +409,11 @@ 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());
positioningNetworkTable.getEntry("interestPointsCl").setBoolean(false);
// 1) 每帧喂给 LL：机器人当前 yaw（度），其余先全 0
LimelightHelpers.SetRobotOrientation(
Constants.Limelight.LIMELIGHT_NAME_Shooter,
src/main/java/frc/robot/subsystems/IntakerSubsystem.java
+2 -19
View File
@@ -83,26 +83,9 @@ public class IntakerSubsystem extends SubsystemBase{
stopIntakerotater();
}
public void IntakerDownNonStop(){
double currentPosition = getIntakerotaterPosition();
if (currentPosition > Constants.Intaker.intakeRotaterDownLimit) {
intakerotater.set(-0.05);}
// double currentPosition = getIntakerotaterPosition();
// if (currentPosition > Constants.Intaker.intakeRotaterDownLimit) {
// intakerotater.setControl(velocityRequest.withVelocity(Constants.Intaker.intakeRotaterVelocity));
// } else {
// stopIntakerotater();
// }
intakerotater.set(-0.05);
}
public void IntakerUpNonStop(){
double currentPosition = getIntakerotaterPosition();
if (currentPosition < Constants.Intaker.intakeRotaterUpLimit) {
intakerotater.set(0.2);}
// if (currentPosition < Constants.Intaker.intakeRotaterUpLimit) {
// intakerotater.setControl(velocityRequest.withVelocity(-Constants.Intaker.intakeRotaterVelocity));
// } else {
// stopIntakerotater();
// }
intakerotater.set(0.1);
}
}
src/main/java/frc/robot/subsystems/LimelightSupplier.java
+108
View File
@@ -0,0 +1,108 @@
package frc.robot.subsystems;
import frc.robot.Constants;
import java.lang.annotation.Target;
import edu.wpi.first.math.geometry.Pose2d;
import edu.wpi.first.math.geometry.Pose3d;
import edu.wpi.first.wpilibj.GenericHID.RumbleType;
import edu.wpi.first.wpilibj.Joystick;
import edu.wpi.first.wpilibj.Timer;
import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
import edu.wpi.first.wpilibj2.command.SubsystemBase;
import frc.robot.subsystems.LimelightHelpers;
import frc.robot.subsystems.LimelightHelpers.RawFiducial;
public class LimelightSupplier extends SubsystemBase {
public LimelightSupplier() {
setPipeline(8);
}
public static double getTX() {
return LimelightHelpers.getTX(Constants.Limelight.LIMELIGHT_NAME_Intaker);
}
public static double getTY() {
return LimelightHelpers.getTY(Constants.Limelight.LIMELIGHT_NAME_Intaker);
}
public static double getTA() {
// Logic to get area from Limelight
return LimelightHelpers.getTA(Constants.Limelight.LIMELIGHT_NAME_Intaker);
}
public static boolean isTargetVisible() {
if(!LimelightHelpers.getTV(Constants.Limelight.LIMELIGHT_NAME_Intaker)){
return false;
}
// Logic to check if a target is visible
return LimelightHelpers.getTV(Constants.Limelight.LIMELIGHT_NAME_Intaker);
}
public static int getAprilTagID(){
RawFiducial[] fiducials = LimelightHelpers.getRawFiducials(Constants.Limelight.LIMELIGHT_NAME_Intaker);
int id = -1;
for (RawFiducial fiducial : fiducials) {
id = fiducial.id; // 标签ID
}
return id;
}
public static void setPipeline(int pipeline) {
if(((int)LimelightHelpers.getCurrentPipelineIndex(Constants.Limelight.LIMELIGHT_NAME_Intaker)) != pipeline){
LimelightHelpers.setPipelineIndex(Constants.Limelight.LIMELIGHT_NAME_Intaker, pipeline);
}
}
public static double[] getTargetPose() {
// Logic to get the target pose from Limelight
return LimelightHelpers.getBotPose_TargetSpace(Constants.Limelight.LIMELIGHT_NAME_Intaker);
}
public static Pose2d getBotPose2d_wpiBlue(){
return LimelightHelpers.getBotPose2d_wpiBlue(Constants.Limelight.LIMELIGHT_NAME_Intaker);
}
public static double getTargetRotationY(){
if(isTargetVisible()){
return getTargetPose()[4];
}
else{
return 0.00;
}
}
public static double getTargetTZ(){
if(isTargetVisible()){
return getTargetPose()[2];
}
else{
return 0.00;
}
}
@Override
public void periodic() {
SmartDashboard.putNumber("LimelightPipeIndex", LimelightHelpers.getCurrentPipelineIndex(Constants.Limelight.LIMELIGHT_NAME_Intaker));
SmartDashboard.putString("LimelightPipeType", LimelightHelpers.getCurrentPipelineType(Constants.Limelight.LIMELIGHT_NAME_Intaker));
if (isTargetVisible()){
SmartDashboard.putString("LimelightTargetVisible", "YES");
SmartDashboard.putNumber("LimelightTA", getTA());
SmartDashboard.putNumber("TargetRY", getTargetRotationY());
SmartDashboard.putNumber("TargetTX", getTX());
SmartDashboard.putNumber("TargetTZ", getTargetTZ());
RawFiducial[] fiducials = LimelightHelpers.getRawFiducials(Constants.Limelight.LIMELIGHT_NAME_Intaker);
int id = -1;
for (RawFiducial fiducial : fiducials) {
id = fiducial.id; // 标签ID
}
SmartDashboard.putNumber("LimelightTargetID", id);
}
else {
SmartDashboard.putString("LimelightTargetVisible", "NO");
SmartDashboard.putNumber("TargetRY", 0);
SmartDashboard.putNumber("TargetTX", 0);
SmartDashboard.putNumber("TargetTZ", 0);
SmartDashboard.putNumber("LimelightTargetID", -1);
}
}
}
src/main/java/frc/robot/subsystems/ShooterSubsystem.java
+21 -3
View File
@@ -4,6 +4,7 @@ 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.hardware.TalonFX;
import com.ctre.phoenix6.hardware.TalonFXS;
import com.ctre.phoenix6.signals.InvertedValue;
@@ -31,7 +32,7 @@ public class ShooterSubsystem extends SubsystemBase {
private NetworkTable shooterNetworkTable;
private final VelocityDutyCycle velocityRequest = new VelocityDutyCycle(0);
private final VelocityVoltage velocityRequest = new VelocityVoltage(0);
// 初始化各个部件
@@ -94,9 +95,17 @@ public class ShooterSubsystem extends SubsystemBase {
public void setConveyorSpeedByRPS(double speed) {
conveyorMotor.setControl(velocityRequest.withVelocity(speed)); // 设置传动电机速度
}
public void setConveyorSpeedOpen(double speed){
conveyorMotor.set(speed);
}
//调试用
public void setBackboardSpeedByRPS(double speed){
if (speed > 0 && speed < 10) {
speed = 10;
} else if (speed < 0 && speed > -10) {
speed = -10;
}
backboardMotor.setControl(velocityRequest.withVelocity(speed));
}
@@ -149,6 +158,10 @@ public class ShooterSubsystem extends SubsystemBase {
public void resetbackboardencoder(){
backboardEncoder.reset();
}
public double getConveyorStatorCurrent(){
double statorCurrent = conveyorMotor.getStatorCurrent().getValueAsDouble();
return statorCurrent;
}
@Override
public void periodic() {
@@ -160,11 +173,16 @@ public class ShooterSubsystem extends SubsystemBase {
shooterNetworkTable.getEntry("backboardCurrentRate").setDouble(getBackboardPosition());
shooterNetworkTable.getEntry("backboardTargetRate").setDouble(backboardPID.getSetpoint().position);
shooterNetworkTable.getEntry("isBackboardAtTarget").setBoolean(isBackboardAtTarget());
shooterNetworkTable.getEntry("distanceGetted").setDouble(getDistance());
shooterNetworkTable.getEntry("distanceGetted").setDouble(getDistanceToHub());
shooterNetworkTable.getEntry("distancetopassball").setDouble(getDistanceToPassball());
shooterNetworkTable.getEntry("statorCurrent").setDouble(getConveyorStatorCurrent());
}
public double getDistance(){
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);
}
}