230 lines
9.7 KiB
Java
230 lines
9.7 KiB
Java
package frc.robot;
|
||
|
||
import com.ctre.phoenix6.SignalLogger;
|
||
import com.pathplanner.lib.auto.AutoBuilder;
|
||
|
||
import edu.wpi.first.math.geometry.Pose2d;
|
||
import edu.wpi.first.wpilibj.DriverStation;
|
||
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.RunCommand;
|
||
import edu.wpi.first.wpilibj2.command.button.CommandXboxController;
|
||
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;
|
||
import frc.robot.tuning.ConfigTalonFXSMotorTuner;
|
||
import frc.robot.tuning.DriveGainsTuner;
|
||
import frc.robot.utils.LedBindings;
|
||
import frc.robot.controls.ClimberControls;
|
||
import frc.robot.controls.Autocommand;
|
||
import frc.robot.subsystems.ClimberSubsystem;
|
||
public class RobotContainer {
|
||
|
||
/* ====================== */
|
||
/* 手柄 */
|
||
/* ====================== */
|
||
|
||
// Demo mode uses one Xbox controller on port 0.
|
||
private final CommandXboxController controllerlower = new CommandXboxController(0);
|
||
private final CommandXboxController controllerupper = controllerlower;
|
||
|
||
/* ====================== */
|
||
/* 子系统 */
|
||
/* ====================== */
|
||
|
||
// Phoenix TunerX 生成的底盘
|
||
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
|
||
|
||
private final RobotStatusManager robotStatusManager = new RobotStatusManager();
|
||
/* ====================== */
|
||
/* 控制封装/日志 */
|
||
/* ====================== */
|
||
|
||
public 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, 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);
|
||
private ConfigTalonFXSMotorTuner configBackboardTuner = new ConfigTalonFXSMotorTuner(shooterSubsystem.backboardMotor, "backboard", Constants.Shooter.backboardSlot0Configs);
|
||
|
||
private final SendableChooser<Command> autoChooser;
|
||
|
||
private final Trigger enableTrigger = new Trigger(DriverStation::isEnabled);
|
||
|
||
|
||
|
||
public RobotContainer() {
|
||
|
||
Pose2d startingPose = Constants.StartingPoints.Red.Point2;
|
||
drivetrain.resetPose(startingPose);
|
||
configueSwerve();
|
||
configueShooter();
|
||
// configueClimber();
|
||
bingdingLED();
|
||
configueIntaker();
|
||
// Autocommand.preNameCommands(
|
||
// climberSubsystem,
|
||
// IntakerSubsystem,
|
||
// drivetrain,
|
||
// shooterSubsystem,
|
||
// driveControls,
|
||
// shooterControls
|
||
// );
|
||
|
||
Autocommand.preNameCommands(
|
||
IntakerSubsystem,
|
||
drivetrain,
|
||
shooterSubsystem,
|
||
driveControls,
|
||
shooterControls
|
||
);
|
||
SignalLogger.enableAutoLogging(false);
|
||
autoChooser = AutoBuilder.buildAutoChooser();
|
||
SmartDashboard.putData("Auto Chooser", autoChooser);
|
||
}
|
||
|
||
/* ====================== */
|
||
/* 底盘配置 */
|
||
/* ====================== */
|
||
|
||
private void configueSwerve() {
|
||
enableTrigger.onTrue(driveControls.getAllianceColorCommand());
|
||
Pose2d startingPose = Constants.StartingPoints.Red.Point2;
|
||
drivetrain.resetPose(startingPose);
|
||
|
||
// 默认驾驶命令
|
||
/*
|
||
* LeftX 底盘左右
|
||
* LeftY 底盘前后 Bump/Trench偏移未完成
|
||
* RightX Aiming偏移 AllTelop模式下 底盘旋转
|
||
*
|
||
*/
|
||
drivetrain.setDefaultCommand(driveControls.defaultDriveCommand());
|
||
|
||
// Disabled 时进入 idle（防止模块乱动）
|
||
RobotModeTriggers.disabled().whileTrue(driveControls.idleCommand());
|
||
|
||
if (!Constants.DemoMode.ENABLED) {
|
||
|
||
|
||
//左保险：按住刹车
|
||
controllerlower.leftBumper()
|
||
.onTrue(Commands.runOnce(driveControls::emergencyStop))
|
||
.whileTrue(driveControls.brakeWhileHeld());
|
||
|
||
// 右保险：按住加速（Boost）
|
||
controllerlower.rightBumper()
|
||
.onTrue(Commands.runOnce(() -> driveControls.setBoostEnabled(true)))
|
||
.onFalse(Commands.runOnce(() -> driveControls.setBoostEnabled(false)));
|
||
|
||
// Back：重置场地坐标系角度
|
||
controllerlower.back().onTrue(drivetrain.runOnce(drivetrain::seedFieldCentric));
|
||
|
||
// Start：强行使用metaTag2全场定位
|
||
controllerlower.start().whileTrue(drivetrain.run(drivetrain::forceUsingLimelightmt2));
|
||
|
||
controllerlower.a().whileTrue(drivetrain.run(()->{drivetrain.forceUsingLimelightmt2WithllName(Constants.Limelight.LIMELIGHT_NAME_Shooter);
|
||
System.out.println("Try using shooter for metatag2");
|
||
}));
|
||
controllerlower.b().whileTrue(drivetrain.run(()->{drivetrain.forceUsingLimelightmt2WithllName(Constants.Limelight.LIMELIGHT_NAME_Intaker);
|
||
System.out.println("Try using intaker for metatag2");
|
||
}));
|
||
//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))
|
||
.onFalse(robotStatusManager.setStatusCommand(RobotStatus.AllTelop));
|
||
|
||
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(){
|
||
shooterSubsystem.setDefaultCommand(shooterControls.defaultShooterCommand());
|
||
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()));
|
||
if (Constants.DemoMode.ENABLED) {
|
||
controllerupper.rightTrigger()
|
||
.onTrue(robotStatusManager.setStatusCommand(RobotStatus.AutoAimming))
|
||
.onFalse(robotStatusManager.setStatusCommand(RobotStatus.AllTelop));
|
||
} else {
|
||
controllerupper.start().onTrue(shooterControls.resetBackboardCommand());
|
||
}
|
||
|
||
}
|
||
|
||
private void bingdingLED() {
|
||
LedBindings.bindModeIndicators(leds);
|
||
}
|
||
|
||
public Command getAutonomousCommand() {
|
||
if (Constants.DemoMode.ENABLED) {
|
||
return Commands.none();
|
||
}
|
||
return autoChooser.getSelected();
|
||
}
|
||
|
||
public void tunerPeriodic() {
|
||
driveGainsTuner.periodic();
|
||
configFlywheelTuner.periodic(() -> shooterSubsystem.applyLeftConfigurationToRight());
|
||
configConveyorTuner.periodic(null);
|
||
configBackboardTuner.periodic();
|
||
shooterSubsystem.setRightFollowLeft();
|
||
}
|
||
|
||
// private void configueClimber() {
|
||
// climberSubsystem.setDefaultCommand(climberControls.defaultClimberCommand());
|
||
// }
|
||
|
||
private void configueIntaker() {
|
||
IntakerSubsystem.setDefaultCommand(intakerControls.defaultIntakerCommand());
|
||
//controllerupper.a().onTrue(intakerControls.resetIntakerRotaterEncoderCommand(IntakerSubsystem));
|
||
}
|
||
|
||
// public ClimberSubsystem getClimberSubsystem() {
|
||
// return climberSubsystem;
|
||
// }
|
||
|
||
}
|