141 lines
7.1 KiB
Java
141 lines
7.1 KiB
Java
// Copyright (c) FIRST and other WPILib contributors.
|
|
// Open Source Software; you can modify and/or share it under the terms of
|
|
// the WPILib BSD license file in the root directory of this project.
|
|
|
|
package frc.robot;
|
|
|
|
import static edu.wpi.first.units.Units.*;
|
|
|
|
import com.ctre.phoenix6.swerve.SwerveModule.DriveRequestType;
|
|
import com.ctre.phoenix6.swerve.SwerveRequest;
|
|
import com.pathplanner.lib.auto.AutoBuilder;
|
|
import com.pathplanner.lib.auto.NamedCommands;
|
|
|
|
import edu.wpi.first.cameraserver.CameraServer;
|
|
import edu.wpi.first.math.MathUtil;
|
|
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.button.CommandXboxController;
|
|
import frc.robot.commands.Aspirer;
|
|
import frc.robot.commands.DescendreBalyeuse;
|
|
import frc.robot.commands.DescendreGrimpeur;
|
|
import frc.robot.commands.Inverser;
|
|
import frc.robot.commands.Lancer;
|
|
import frc.robot.commands.LancerAspirer;
|
|
import frc.robot.commands.LancerBaseVitesse;
|
|
import frc.robot.commands.Limelighter;
|
|
import frc.robot.commands.ModeOposer;
|
|
import frc.robot.commands.ModeOposerBalayeuse;
|
|
import frc.robot.commands.ModeOposerDemeleur;
|
|
import frc.robot.commands.MonterBalyeuse;
|
|
import frc.robot.commands.MonterGrimpeur;
|
|
import frc.robot.commands.ModeAuto.AspirerAuto;
|
|
import frc.robot.commands.ModeAuto.BougerDroiteAuto;
|
|
import frc.robot.commands.ModeAuto.BougerGaucheAuto;
|
|
import frc.robot.commands.ModeAuto.GrimperMur;
|
|
import frc.robot.commands.ModeAuto.GrimperReservoir;
|
|
import frc.robot.commands.ModeAuto.LancerAuto;
|
|
import frc.robot.commands.ModeAuto.LimelighterAuto;
|
|
import frc.robot.commands.ModeAuto.RetourMilieuDroite;
|
|
import frc.robot.commands.ModeAuto.RetourMilieuGauche;
|
|
import frc.robot.commands.ModeAuto.TournerVersMur;
|
|
import frc.robot.commands.ModeAuto.TournerVersReservoir;
|
|
import frc.robot.generated.TunerConstants;
|
|
import frc.robot.subsystems.Balayeuse;
|
|
import frc.robot.subsystems.CommandSwerveDrivetrain;
|
|
import frc.robot.subsystems.Grimpeur;
|
|
import frc.robot.subsystems.LEDSubsystem;
|
|
import frc.robot.subsystems.Lanceur;
|
|
import frc.robot.subsystems.LimeLight3;
|
|
import frc.robot.subsystems.Limelight3G;
|
|
|
|
public class RobotContainer {
|
|
private final SendableChooser<Command> autoChooser;
|
|
Balayeuse balayeuse = new Balayeuse();
|
|
Grimpeur grimpeur = new Grimpeur();
|
|
Lanceur lanceur = new Lanceur();
|
|
LimeLight3 limeLight3 = new LimeLight3();
|
|
Limelight3G limeLight3G = new Limelight3G();
|
|
LEDSubsystem ledSubsystem = new LEDSubsystem();
|
|
CommandXboxController manette = new CommandXboxController(0);
|
|
CommandXboxController manette1 = new CommandXboxController(1);
|
|
private double MaxSpeed = TunerConstants.kSpeedAt12Volts.in(MetersPerSecond); // kSpeedAt12Volts desired top speed
|
|
private double MaxAngularRate = RotationsPerSecond.of(0.75).in(RadiansPerSecond); // 3/4 of a rotation per second max angular velocity
|
|
|
|
/* Setting up bindings for necessary control of the swerve drive platform */
|
|
private final SwerveRequest.FieldCentric drive = new SwerveRequest.FieldCentric()
|
|
.withDeadband(MaxSpeed * 0.1).withRotationalDeadband(MaxAngularRate * 0.1) // Add a 10% deadband
|
|
.withDriveRequestType(DriveRequestType.OpenLoopVoltage); // Use open-loop control for drive motors
|
|
|
|
public final CommandSwerveDrivetrain drivetrain = TunerConstants.createDrivetrain();
|
|
|
|
|
|
public RobotContainer() {
|
|
NamedCommands.registerCommand("droite", new BougerDroiteAuto(drivetrain));
|
|
NamedCommands.registerCommand("gauche", new BougerGaucheAuto(drivetrain));
|
|
NamedCommands.registerCommand("GrimperMur", new GrimperMur(limeLight3,drivetrain));
|
|
NamedCommands.registerCommand("GrimperReservoir", new GrimperReservoir(limeLight3G,drivetrain));
|
|
NamedCommands.registerCommand("Lancer", new LancerAuto(lanceur, limeLight3G));
|
|
NamedCommands.registerCommand("RetourMilieuDroite", new RetourMilieuDroite(drivetrain,limeLight3G));
|
|
NamedCommands.registerCommand("RetourMilieuGauche", new RetourMilieuGauche(drivetrain, limeLight3G));
|
|
NamedCommands.registerCommand("Limelighter", new LimelighterAuto(limeLight3G,drivetrain));
|
|
NamedCommands.registerCommand("DescendreBalayeuse", new DescendreBalyeuse(balayeuse));
|
|
NamedCommands.registerCommand("Aspirer", new AspirerAuto(balayeuse));
|
|
NamedCommands.registerCommand("TournerA180", new TournerVersReservoir(drivetrain));
|
|
NamedCommands.registerCommand("TournerAZero", new TournerVersMur(drivetrain));
|
|
NamedCommands.registerCommand("MonterGrimpeur", new MonterGrimpeur(grimpeur));
|
|
NamedCommands.registerCommand("DescendreGrimpeur", new DescendreGrimpeur(grimpeur));
|
|
autoChooser = AutoBuilder.buildAutoChooser();
|
|
SmartDashboard.putData("Auto Chooser", autoChooser);
|
|
|
|
CameraServer.startAutomaticCapture();
|
|
|
|
configureBindings();
|
|
}
|
|
|
|
private void configureBindings() {
|
|
ledSubsystem.setDefaultCommand(ledSubsystem.updateLEDs());
|
|
drivetrain.setDefaultCommand(
|
|
drivetrain.applyRequest(() ->
|
|
drive.withVelocityY(MathUtil.applyDeadband(-manette.getLeftX()*Math.abs(-manette.getLeftX())*MaxSpeed*0.7, 0.05))
|
|
.withVelocityX(MathUtil.applyDeadband(-manette.getLeftY()*Math.abs(-manette.getLeftY())*MaxSpeed*0.7, 0.05))
|
|
.withRotationalRate(MathUtil.applyDeadband(-manette.getRightX()*Math.abs(-manette.getRightX())*MaxAngularRate, 0.05))
|
|
)
|
|
);
|
|
//manette 1
|
|
// manette1.povUp().whileTrue(drivetrain.sysIdQuasistatic(SysIdRoutine.Direction.kForward));
|
|
// manette1.povDown().whileTrue(drivetrain.sysIdQuasistatic(SysIdRoutine.Direction.kReverse));
|
|
// manette1.povRight().whileTrue(drivetrain.sysIdDynamic(SysIdRoutine.Direction.kForward));
|
|
// manette1.povLeft().whileTrue(drivetrain.sysIdDynamic(SysIdRoutine.Direction.kReverse));
|
|
|
|
manette.povUp().whileTrue(new MonterGrimpeur(grimpeur));
|
|
manette.povDown().whileTrue(new DescendreGrimpeur(grimpeur));
|
|
manette.rightTrigger().whileTrue(new Lancer(lanceur, limeLight3G));
|
|
manette.leftTrigger().whileTrue(new Limelighter(limeLight3G, drivetrain));
|
|
manette.leftBumper().whileTrue(new DescendreBalyeuse(balayeuse));
|
|
manette.rightBumper().whileTrue(new Aspirer(balayeuse));
|
|
manette.b().whileTrue(new GrimperReservoir(limeLight3G, drivetrain));
|
|
manette.x().whileTrue(new GrimperMur(limeLight3, drivetrain));
|
|
|
|
|
|
//manette 2
|
|
manette1.rightTrigger().whileTrue(new Aspirer(balayeuse));
|
|
manette1.rightBumper().whileTrue(new MonterBalyeuse(balayeuse));
|
|
manette1.leftTrigger().whileTrue(new LancerBaseVitesse(lanceur));
|
|
manette1.leftBumper().whileTrue(new DescendreBalyeuse(balayeuse));
|
|
manette1.x().whileTrue(new LancerAspirer(lanceur, balayeuse, limeLight3G));
|
|
manette1.b().whileTrue(new ModeOposer(lanceur));
|
|
manette1.a().whileTrue(new ModeOposerDemeleur(lanceur));
|
|
manette1.y().whileTrue(new ModeOposerBalayeuse(balayeuse));
|
|
manette1.povRight().whileTrue(new BougerDroiteAuto(drivetrain));
|
|
manette1.povLeft().whileTrue(new BougerGaucheAuto(drivetrain));
|
|
manette1.start().whileTrue(new Inverser(drivetrain));
|
|
}
|
|
|
|
public Command getAutonomousCommand() {
|
|
return autoChooser.getSelected();
|
|
// return getAutonomousCommand();
|
|
}
|
|
}
|