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

双手柄版本

Browse Source
This commit is contained in:
OnDYCLaptop
2026-03-02 14:35:18 +08:00
parent 06340c5724
commit 5f3452e091
8 changed files with 106 additions and 182 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/main/java/frc/robot/Constants.java
+3
View File
@@ -52,6 +52,7 @@ public final class Constants {
public static final double teleopOffsetkP = 15;
public static final double autoPositioningkP = 1.0;
public static final double autoPositioningAngleError = 1.0; // 角度误差容忍度，单位为度
public static final double[] bumpY = {2.51,5.556};
public static final double[] trenchY = {0.639,7.43};
@@ -112,6 +113,8 @@ public final class Constants {
// 单位RPS
public static final double positionToleranceRotations = 0.00;
//rotater容差
public static final double IntakeGetterK = 1.0; // intake getter速度与触发器输入的比例
public static final double Getterminspeed = 20; // intake getter的基础速度（即触发器输入为0时的速度，正值表示默认向内转）
}
public static class Climber {
src/main/java/frc/robot/RobotContainer.java
+23 -16
View File
@@ -14,9 +14,11 @@ import edu.wpi.first.wpilibj2.command.button.RobotModeTriggers;
import edu.wpi.first.wpilibj2.command.button.Trigger;
import frc.robot.Constants.RobotStatus;
import frc.robot.controls.DriveControls;
import frc.robot.controls.IntakerControls;
import frc.robot.controls.ShooterControls;
import frc.robot.generated.TunerConstants;
import frc.robot.subsystems.CommandSwerveDrivetrain;
import frc.robot.subsystems.IntakerSubsystem;
import frc.robot.subsystems.LEDSubsystem;
import frc.robot.subsystems.ShooterSubsystem;
import frc.robot.tuning.ConfigTalonFXMotorTuner;
@@ -32,7 +34,8 @@ public class RobotContainer {
/* ====================== */
// 只使用一个 Xbox 手柄（端口 0）
private final CommandXboxController controller = new CommandXboxController(0);
private final CommandXboxController controllerlower = new CommandXboxController(0);
private final CommandXboxController controllerupper = new CommandXboxController(1);
/* ====================== */
/* 子系统 */
@@ -42,6 +45,7 @@ public class RobotContainer {
public final CommandSwerveDrivetrain drivetrain = TunerConstants.createDrivetrain();
public final ClimberSubsystem climberSubsystem = new ClimberSubsystem();
public final ShooterSubsystem shooterSubsystem = new ShooterSubsystem();
public final IntakerSubsystem IntakerSubsystem = new IntakerSubsystem();
// LED 子系统
private final LEDSubsystem leds = new LEDSubsystem(0, 72 ,0); // 总长度=72+72
@@ -51,9 +55,10 @@ public class RobotContainer {
/* 控制封装/日志 */
/* ====================== */
private final DriveControls driveControls = new DriveControls(drivetrain, controller,robotStatusManager );
private final ShooterControls shooterControls = new ShooterControls(shooterSubsystem, controller,robotStatusManager);
private final ClimberControls climberControls = new ClimberControls(climberSubsystem, controller);
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 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);
@@ -106,30 +111,32 @@ public class RobotContainer {
// .onFalse(Commands.runOnce(() -> driveControls.setBoostEnabled(false)));
// Back：重置场地坐标系角度
controller.back().onTrue(drivetrain.runOnce(drivetrain::seedFieldCentric));
controllerlower.back().onTrue(drivetrain.runOnce(drivetrain::seedFieldCentric));
// Start：强行使用metaTag2全场定位
controller.start().whileTrue(drivetrain.run(drivetrain::forceUsingLimelightMT2));
controllerlower.start().whileTrue(drivetrain.run(drivetrain::forceUsingLimelightMT2));
//LM 按下时使用机器人坐标系
controller.leftStick().onTrue(robotStatusManager.setStatusCommand(RobotStatus.AutoAimming));
controller.leftStick().onFalse(robotStatusManager.setStatusCommand(RobotStatus.AllTelop));
//controllerlower.leftStick().onTrue(robotStatusManager.setStatusCommand(RobotStatus.AutoAimming));
//controllerlower.leftStick().onFalse(robotStatusManager.setStatusCommand(RobotStatus.AllTelop));
controller.rightStick().and(controller.leftBumper())
.onTrue(robotStatusManager.setStatusCommand(RobotStatus.CrossingBump))
controllerlower.leftTrigger()
.onTrue(robotStatusManager.setStatusCommand(RobotStatus.CrossingTrench))
.onFalse(robotStatusManager.setStatusCommand(RobotStatus.AllTelop));
controller.rightStick().and(controller.rightBumper())
.onTrue(robotStatusManager.setStatusCommand(RobotStatus.CrossingTrench))
controllerlower.rightTrigger()
.onTrue(robotStatusManager.setStatusCommand(RobotStatus.CrossingBump))
.onFalse(robotStatusManager.setStatusCommand(RobotStatus.AllTelop));
}
private void configueShooter(){
shooterSubsystem.setDefaultCommand(shooterControls.defaultShooterCommand());
controller.povLeft().onTrue(Commands.runOnce(() -> shooterControls.adjustFlywheelSpeedOffset(-1)));
controller.povRight().onTrue(Commands.runOnce(() -> shooterControls.adjustFlywheelSpeedOffset(1)));
controller.povUp().onTrue(Commands.runOnce(() -> shooterControls.adjustBackboardRateOffset(100)));
controller.povDown().onTrue(Commands.runOnce(() -> shooterControls.adjustBackboardRateOffset(-100)));
controllerupper.povLeft().onTrue(Commands.runOnce(() -> shooterControls.adjustFlywheelSpeedOffset(-1)));
controllerupper.povRight().onTrue(Commands.runOnce(() -> shooterControls.adjustFlywheelSpeedOffset(1)));
controllerupper.povUp().onTrue(Commands.runOnce(() -> shooterControls.adjustBackboardRateOffset(100)));
controllerupper.povDown().onTrue(Commands.runOnce(() -> shooterControls.adjustBackboardRateOffset(-100)));
controllerupper.back().onTrue(Commands.runOnce(() -> shooterControls.resetOffsets()));
controllerupper.start().onTrue(Commands.runOnce(() -> shooterControls.resetBackboardCommand()));
}
/* ====================== */
/* LED 绑定 */
src/main/java/frc/robot/controls/DriveControls.java
+16 -49
View File
@@ -126,7 +126,6 @@ public class DriveControls {
vomega = calculateRotationSpeedFromRotationAngle(calculateDistanceAndRotationToPassBall()[1]);
autoControlNetworkTable.getEntry("autoRotationRate").setDouble(vomega);
break;
case AutoAimming:
vomega = calculateRotationSpeedFromRotationAngle(distanceAndRotation[1]);
autoControlNetworkTable.getEntry("autoRotationRate").setDouble(vomega);
@@ -161,15 +160,8 @@ public class DriveControls {
errorB = Math.atan2(Math.sin(errorB), Math.cos(errorB));
// 选误差绝对值更小的那个
double chosenError = (Math.abs(errorA) <= Math.abs(errorB)) ? errorA : errorB;
if (Math.abs(chosenError) < Math.toRadians(2.0)) {
vomega = 0.0;
} else {
vomega = chosenError * Constants.AutoPositioning.TurningkP;
}
// 限制最大角速度
vomega = Math.max(-maxAngularRate, Math.min(maxAngularRate, vomega));
vomega = calculatevomegaFromRotationAngle(chosenError);
autoControlNetworkTable.getEntry("BumpAutoRotationRate").setDouble(vomega);
//vomega = calculateRotationSpeedForBump();
break;
case CrossingTrench:
@@ -188,14 +180,8 @@ public class DriveControls {
double currentRad = currentAngle.getRadians();
double targetRad = Math.toRadians(Constants.AutoPositioning.TrenchtargetAngle);
double TrenchAngleerror = targetRad - currentRad;
if (Math.abs(TrenchAngleerror) < Math.toRadians(1.0)) {
vomega = 0.0;
} else {
vomega = TrenchAngleerror * Constants.AutoPositioning.TurningkP;
}
// 限幅（防止转太快）
vomega = Math.max(-maxAngularRate, Math.min(maxAngularRate, vomega));
vomega = calculatevomegaFromRotationAngle(TrenchAngleerror);
autoControlNetworkTable.getEntry("TrenchAutoRotationRate").setDouble(vomega);
//vomega = calculateRotationSpeedForTrench();
break;
}
@@ -238,33 +224,6 @@ public class DriveControls {
double x2 = target2 - current;
return Math.abs(x1) < Math.abs(x2) ? x1 : x2;
}
public double calculateRotationSpeedForTrench(){
Pose2d currentPose = drivetrain.getState().Pose; // 获取机器人当前的位置和角度
double currentAngle = currentPose.getRotation().getDegrees();
double targetAngle = Constants.AutoPositioning.autoRotationForTrenchTargetDegrees; // 转换为度
double angleDifference = targetAngle - currentAngle;
// Normalize angle difference to the range [-180, 180]
if (angleDifference > 180) {
angleDifference -= 360;
} else if (angleDifference < -180) {
angleDifference += 360;
}
return calculateRotationSpeedFromRotationAngle(angleDifference);
}
public double calculateRotationSpeedForBump(){
Pose2d currentPose = drivetrain.getState().Pose; // 获取机器人当前的位置和角度
double currentAngle = currentPose.getRotation().getDegrees();
double targetAngle = Constants.AutoPositioning.autoRotationForBumpTargetDegrees; // 转换为度
double angleDifference = targetAngle - currentAngle;
// Normalize angle difference to the range [-180, 180]
if (angleDifference > 180) {
angleDifference -= 360;
} else if (angleDifference < -180) {
angleDifference += 360;
}
return calculateRotationSpeedFromRotationAngle(angleDifference);
}
// Calculate distance and rotation (angle difference)
public double[] calculateDistanceAndRotationToHub() {
Pose2d currentPose = drivetrain.getState().Pose; // 获取机器人当前的位置和角度
double targetX = Constants.Field.RedHubPositionX; // 目标 X 坐标
@@ -299,7 +258,6 @@ public class DriveControls {
double targetY1 = Constants.Field.PassingBallPosY1; // 目标 Y 坐标
double targetY2 = Constants.Field.PassingBallPosY2; // 目标 Y 坐标
// 计算目标角度（相对于场地坐标系）
double deltaX = targetX - currentPose.getX();
double deltaY = calculateDifferenceToTwoTarget(currentPose.getY(), targetY1, targetY2);
@@ -324,7 +282,6 @@ public class DriveControls {
return distanceAndRotation;
}
// Calculate rotation speed from rotation angle difference
public double calculateRotationSpeedFromRotationAngle(double angleDifference) {
double calDifference = angleDifference;
@@ -341,8 +298,18 @@ public class DriveControls {
return 0;
}
}
private double calculatevomegaFromRotationAngle(double error) {
double vomega;
if (Math.abs(error) < Math.toRadians(Constants.AutoPositioning.autoPositioningAngleError)) {
vomega = 0.0;
} else {
vomega = error * Constants.AutoPositioning.TurningkP;
}
// 限制最大角速度
vomega = Math.max(-maxAngularRate, Math.min(maxAngularRate, vomega));
return vomega;
}
/**
* 只限制“加速”（幅值变大），不限制“减速”（幅值变小）
src/main/java/frc/robot/controls/IntakerControls.java
+53 -44
View File
@@ -1,9 +1,13 @@
package frc.robot.controls;
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.button.CommandXboxController;
import frc.robot.subsystems.IntakerSubsystem;
import frc.robot.Constants;
public class IntakerControls {
private final IntakerSubsystem intakerSubsystem;
@@ -16,56 +20,61 @@ public class IntakerControls {
this.controller = controller;
}
public void configureBindings() {
//B.A：按住下降 + getter转；松开后 rotater保持当前位置，getter继续转
controller.a()
.whileTrue(
Commands.run(() -> {
// rotater下降：范围会被 subsystem 限制
double cur = intakerSubsystem.getIntakerotaterPosition();
intakerSubsystem.setIntakerRotaterPosition(cur - ROTATER_STEP_PER_CYCLE);
// controller.a()
// .whileTrue(
// Commands.run(() -> {
// // rotater下降：范围会被 subsystem 限制
// double cur = intakerSubsystem.getIntakerotaterPosition();
// intakerSubsystem.setIntakerRotaterPosition(cur - ROTATER_STEP_PER_CYCLE);
// getter 开
intakerSubsystem.setIntakeGetterOn(true);
}, intakerSubsystem)
)
.onFalse(
Commands.runOnce(() -> {
// 松：把当前位置作为target保持
double cur = intakerSubsystem.getIntakerotaterPosition();
intakerSubsystem.setIntakerRotaterPosition(cur);
// // getter 开
// intakerSubsystem.setIntakeGetterOn(true);
// }, intakerSubsystem)
// )
// .onFalse(
// Commands.runOnce(() -> {
// // 松：把当前位置作为target保持
// double cur = intakerSubsystem.getIntakerotaterPosition();
// intakerSubsystem.setIntakerRotaterPosition(cur);
// gtter继续转
intakerSubsystem.setIntakeGetterOn(true);
}, intakerSubsystem)
);
// // gtter继续转
// intakerSubsystem.setIntakeGetterOn(true);
// }, intakerSubsystem)
// );
// // === B.B：按住上升+getter停；松开后rotater保持当前位置
// controller.b()
// .whileTrue(
// Commands.run(() -> {
// // rotater升
// double cur = intakerSubsystem.getIntakerotaterPosition();
// intakerSubsystem.setIntakerRotaterPosition(cur + ROTATER_STEP_PER_CYCLE);
// === B.B：按住上升+getter停；松开后rotater保持当前位置
controller.b()
.whileTrue(
Commands.run(() -> {
// rotater升
double cur = intakerSubsystem.getIntakerotaterPosition();
intakerSubsystem.setIntakerRotaterPosition(cur + ROTATER_STEP_PER_CYCLE);
// getter关
intakerSubsystem.setIntakeGetterOn(false);
}, intakerSubsystem)
)
.onFalse(
Commands.runOnce(() -> {
// 松：保持当前位置
double cur = intakerSubsystem.getIntakerotaterPosition();
intakerSubsystem.setIntakerRotaterPosition(cur);
// getter仍然停止
intakerSubsystem.setIntakeGetterOn(false);
}, intakerSubsystem)
);
}
// // getter关
// intakerSubsystem.setIntakeGetterOn(false);
// }, intakerSubsystem)
// )
// .onFalse(
// Commands.runOnce(() -> {
// // 松：保持当前位置
// double cur = intakerSubsystem.getIntakerotaterPosition();
// intakerSubsystem.setIntakerRotaterPosition(cur);
// // getter仍然停止
// intakerSubsystem.setIntakeGetterOn(false);
// }, intakerSubsystem)
// );
public Command defaultIntakerCommand() {
return Commands.none();
return Commands.run(() -> {
double getterspeed = controller.getLeftTriggerAxis() * Constants.Intaker.IntakeGetterK + Constants.Intaker.Getterminspeed;
if (controller.getLeftTriggerAxis() < 0.1) {
getterspeed = 0;
}
intakerSubsystem.setIntakerGetterSpeed(getterspeed);
}, intakerSubsystem);
}
}
src/main/java/frc/robot/controls/ShooterControls.java
+4 -69
View File
@@ -110,8 +110,6 @@ public class ShooterControls {
break;
}
}, shooterSubsystem);
}
@@ -145,73 +143,6 @@ public class ShooterControls {
shooterSubsystem.backboardPID.reset(0);}, shooterSubsystem);
}
//打表参数
// private static final double[][] FLYWHEEL_TABLE = {
// {1.5, 0.42},
// {2.0, 0.46},
// {2.5, 0.51},
// {3.0, 0.57},
// {3.5, 0.64},
// {4.0, 0.72}
// };
// //线性插值函数
// private static double lerp(double x0, double y0, double x1, double y1, double x) {
// if (Math.abs(x1 - x0) < 1e-9) {
// return y0;
// }
// double t = (x - x0) / (x1 - x0);
// return y0 + t * (y1 - y0);
// }
// private double calculateFlywheelSpeedOnlywithOffset(double defaultSpeed) {
// return defaultSpeed + flywheelSpeedOffset;
// }
// private double calculateBackboardPositionOnlywithOffset(double defaultPosition) {
// return defaultPosition + backboardPositionOffset;
// }
// private double calculateFlywheelSpeed(double distance) {
// // 小于最小距离：用第一个点
// if (distance <= FLYWHEEL_TABLE[0][0]) {
// return FLYWHEEL_TABLE[0][1] + flywheelSpeedOffset;
// }
// // 大于最大距离：用最后一个点
// int last = FLYWHEEL_TABLE.length - 1;
// if (distance >= FLYWHEEL_TABLE[last][0]) {
// return FLYWHEEL_TABLE[last][1] + flywheelSpeedOffset;
// }
// // 查找区间并插值
// for (int i = 0; i < FLYWHEEL_TABLE.length - 1; i++) {
// double d0 = FLYWHEEL_TABLE[i][0];
// double s0 = FLYWHEEL_TABLE[i][1];
// double d1 = FLYWHEEL_TABLE[i + 1][0];
// double s1 = FLYWHEEL_TABLE[i + 1][1];
// if (distance >= d0 && distance <= d1) {
// return lerp(d0, s0, d1, s1, distance) + flywheelSpeedOffset;
// }
// }
// return 0;
// }
// private double calculateBackboardPosition(double distance) {
// double BackboardPosition;
// // 距离阈值（单位自己按distance进行修改）
// final double DISTANCE_NEAR = 2.0; // 近距离阈值
// final double DISTANCE_MID = 4.0; // 中距离阈值
// if (distance < DISTANCE_NEAR) {
// BackboardPosition = Constants.ShooterCalculation.positionNear;
// }
// else if (distance < DISTANCE_MID) {
// BackboardPosition = Constants.ShooterCalculation.positionMid;
// }
// else {
// BackboardPosition = Constants.ShooterCalculation.postionFar;
// }
// BackboardPosition += backboardPositionOffset;
// return BackboardPosition;
// }
public void adjustFlywheelSpeedOffset(double offset) {
flywheelSpeedOffset += offset;
}
@@ -219,4 +150,8 @@ public class ShooterControls {
public void adjustBackboardRateOffset(double offset) {
backboardPositionOffset += offset;
}
public void resetOffsets(){
flywheelSpeedOffset = 0.0;
backboardPositionOffset = 0.0;
}
}
src/main/java/frc/robot/subsystems/ClimberSubsystem.java
+1 -4
View File
@@ -46,12 +46,10 @@ public class ClimberSubsystem extends SubsystemBase {
holdPositionTicks = getPositionTicks();
}
public void initialize() {
climberMotor.set(-0.07);
}
public void zeroEncoder(){
climberMotor.setPosition(0);
stopMotor();
}
@@ -75,7 +73,6 @@ public class ClimberSubsystem extends SubsystemBase {
}
public void climbDownNonStop(){
deltaPositionTicks = Constants.Climber.deltaDownTicks;
climberMotor.setControl(positionHoldRequest.withPosition(targetPositionTicks-deltaPositionTicks));
}
src/main/java/frc/robot/subsystems/IntakerSubsystem.java
+3
View File
@@ -64,4 +64,7 @@ public class IntakerSubsystem extends SubsystemBase{
intakegetter.setControl(velocityRequest.withVelocity(0));
}
}
public void setIntakerGetterSpeed(double speed) {
intakegetter.setControl(velocityRequest.withVelocity(speed));
}
}
src/main/java/frc/robot/subsystems/ShooterSubsystem.java
+3
View File
@@ -146,6 +146,9 @@ public class ShooterSubsystem extends SubsystemBase {
public void conveyorRun(){
setConveyorSpeedByRPS(Constants.Shooter.conveyorSpeed);
}
public void resetbackboardencoder(){
backboardEncoder.reset();
}
@Override
public void periodic() {