diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 3054905..510bfba 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -85,6 +85,7 @@ public class RobotContainer { /* ====================== */ private void configueSwerve() { + enableTrigger.onTrue(driveControls.getAllianceColorCommand()); Pose2d startingPose = Constants.StartingPoints.Red.Point2; drivetrain.resetPose(startingPose); diff --git a/src/main/java/frc/robot/controls/Autocommand.java b/src/main/java/frc/robot/controls/Autocommand.java index afe72ec..c008e1b 100644 --- a/src/main/java/frc/robot/controls/Autocommand.java +++ b/src/main/java/frc/robot/controls/Autocommand.java @@ -48,14 +48,14 @@ import frc.robot.Constants.RobotStatus; return new InstantCommand(() -> { intaker.IntakerDownNonStop(); // 吸球机构下降 }, intaker).withTimeout(0.5).andThen(new InstantCommand(() -> { - intaker.stopIntakerotater();; // 吸球机构保持当前位置 + intaker.stopIntakerotater();; // 停止旋转 }, intaker)); } public static Command intaker_up(IntakerSubsystem intaker) { return new InstantCommand(() -> { intaker.IntakerUpNonStop(); // 吸球机构上升 }, intaker).withTimeout(0.5).andThen(new InstantCommand(() -> { - intaker.stopIntakerotater();; // 吸球机构保持当前位置 + intaker.stopIntakerotater();; // 停止旋转 }, intaker)); } diff --git a/src/main/java/frc/robot/controls/DriveControls.java b/src/main/java/frc/robot/controls/DriveControls.java index 6141a2b..cf3d087 100644 --- a/src/main/java/frc/robot/controls/DriveControls.java +++ b/src/main/java/frc/robot/controls/DriveControls.java @@ -11,6 +11,8 @@ import edu.wpi.first.networktables.NetworkTable; import edu.wpi.first.networktables.NetworkTableInstance; 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 frc.robot.Constants; import frc.robot.RobotStatusManager; @@ -19,7 +21,13 @@ import frc.robot.Constants.RobotStatus; import frc.robot.generated.TunerConstants; import frc.robot.subsystems.CommandSwerveDrivetrain; +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; private final CommandXboxController driver; @@ -362,5 +370,26 @@ public class DriveControls { public double getMaxAngularRate() { return maxAngularRate; } + public void setTeamColors() { + Optional ally = DriverStation.getAlliance(); // 获取队伍颜色 + if (ally.isPresent()) { + // 根据队伍颜色设置布尔值 + if (ally.get() == Alliance.Red) { + isRedAlliance = true; // 红队 + } else if (ally.get() == Alliance.Blue) { + isRedAlliance = false; // 蓝队 + } + } else { + isRedAlliance = true; + } + if (isRedAlliance) { + System.out.println("Robot is on the Red Alliance."); + } else { + System.out.println("Robot is on the Blue Alliance."); + } + } + public Command getAllianceColorCommand() { + return Commands.runOnce(this::setTeamColors); +} }