From a5a360222e87b190c7c0a7face0717caaea467f5 Mon Sep 17 00:00:00 2001 From: Alyssum-chenyiyi <13162501939@163.com> Date: Fri, 27 Feb 2026 23:38:42 +0800 Subject: [PATCH] 0227 MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit 加了passball状态等 --- src/main/java/frc/robot/Constants.java | 5 ++- src/main/java/frc/robot/RobotContainer.java | 1 - .../frc/robot/controls/DriveControls.java | 35 ++++++++++++++++ .../frc/robot/controls/ShooterControls.java | 42 +++++++++++++++++-- .../robot/subsystems/ShooterSubsystem.java | 6 +++ 5 files changed, 83 insertions(+), 6 deletions(-) diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java index 791a6e8..b061e0f 100644 --- a/src/main/java/frc/robot/Constants.java +++ b/src/main/java/frc/robot/Constants.java @@ -8,6 +8,7 @@ public final class Constants { public enum RobotStatus{ Stopped, AllTelop, + PassingBall, AutoAimming, Climbing, CrossingBump, @@ -86,7 +87,9 @@ public final class Constants { 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 = 0; + public static final double PassingBallPosY2 = 0; } public class Intaker { diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 67e1dfb..5984ec7 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -126,7 +126,6 @@ public class RobotContainer { 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))); diff --git a/src/main/java/frc/robot/controls/DriveControls.java b/src/main/java/frc/robot/controls/DriveControls.java index a5d0380..e66a026 100644 --- a/src/main/java/frc/robot/controls/DriveControls.java +++ b/src/main/java/frc/robot/controls/DriveControls.java @@ -122,6 +122,11 @@ public class DriveControls { vy = 0; vomega = 0; break; + case PassingBall: + vomega = calculateRotationSpeedFromRotationAngle(calculateDistanceAndRotationToPassBall()[1]); + autoControlNetworkTable.getEntry("autoRotationRate").setDouble(vomega); + break; + case AutoAimming: vomega = calculateRotationSpeedFromRotationAngle(distanceAndRotation[1]); autoControlNetworkTable.getEntry("autoRotationRate").setDouble(vomega); @@ -288,6 +293,36 @@ public class DriveControls { // 返回距离和角度差 return distanceAndRotation; } + public double[] calculateDistanceAndRotationToPassBall() { + Pose2d currentPose = drivetrain.getState().Pose; // 获取机器人当前的位置和角度 + double targetX = Constants.Field.PassingBallPosX; // 目标 X 坐标 + double targetY1 = Constants.Field.PassingBallPosY1; // 目标 Y 坐标 + double targetY2 = Constants.Field.PassingBallPosY2; // 目标 Y 坐标 + + + // 计算目标角度（相对于场地坐标系） + double deltaX = targetX - currentPose.getX(); + double deltaY = calculateDifferenceToTwoTarget(currentPose.getY(), targetY1, targetY2); + double targetAngle = Math.toDegrees(Math.atan2(deltaY, deltaX)); // 转换为度 + + // 当前机器人角度 + double currentAngle = currentPose.getRotation().getDegrees(); + double angleDifference = targetAngle - currentAngle; + // Normalize angle difference to the range [-180, 180] + if (angleDifference > 180) { + angleDifference -= 360; + } else if (angleDifference < -180) { + angleDifference += 360; + } + + // 计算距离 + double distance = Math.sqrt(deltaX * deltaX + deltaY * deltaY); + // 计算距离和旋转角度 + double[] distanceAndRotation = new double[]{distance, angleDifference}; + + // 返回距离和角度差 + return distanceAndRotation; + } // Calculate rotation speed from rotation angle difference public double calculateRotationSpeedFromRotationAngle(double angleDifference) { diff --git a/src/main/java/frc/robot/controls/ShooterControls.java b/src/main/java/frc/robot/controls/ShooterControls.java index cd48a33..d8491fc 100644 --- a/src/main/java/frc/robot/controls/ShooterControls.java +++ b/src/main/java/frc/robot/controls/ShooterControls.java @@ -7,6 +7,8 @@ import edu.wpi.first.wpilibj2.command.Commands; import edu.wpi.first.wpilibj2.command.button.CommandXboxController; import frc.robot.Constants; import frc.robot.RobotStatusManager; +import frc.robot.Constants.Intaker; +import frc.robot.subsystems.IntakerSubsystem; import frc.robot.subsystems.ShooterSubsystem; public class ShooterControls { @@ -16,6 +18,9 @@ public class ShooterControls { private double flywheelSpeedOffset = 0.0; private double backboardPositionOffset = 0.0; private NetworkTable shooterControlTable; + private double conveyorSpeed = 0.0; + double flywheelSpeed = 0; + double backboardPosition = 0; private final RobotStatusManager robotStatusManager; public ShooterControls(ShooterSubsystem shooterSubsystem, CommandXboxController controller,RobotStatusManager rsm) { this.shooterSubsystem = shooterSubsystem; @@ -23,7 +28,7 @@ public class ShooterControls { this.robotStatusManager = rsm; shooterControlTable = NetworkTableInstance.getDefault().getTable("ShooterControl"); } - + public Command defaultShooterCommand() { return Commands.run(() -> { switch (robotStatusManager.getStatus()) { @@ -31,10 +36,36 @@ public class ShooterControls { shooterSubsystem.stopMotors(); break; case AllTelop: + case PassingBall: + flywheelSpeed = 0;//todo:实测传球的时候的速度 + flywheelSpeed += flywheelSpeedOffset; + backboardPosition = 0;//todo:实测传球的时候的背板位置 + backboardPosition += backboardPositionOffset; + conveyorSpeed = Constants.Shooter.conveyorSpeed; + if (controller.getRightTriggerAxis() < 0.1) { + flywheelSpeed = 0; + conveyorSpeed = 0; + } + if(shooterSubsystem.flywheelMotorLeft.getVelocity().getValueAsDouble()