163 lines
7.4 KiB
Java
163 lines
7.4 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.hardware.Pigeon2;
|
|
import com.ctre.phoenix6.swerve.SwerveRequest;
|
|
import com.pathplanner.lib.auto.AutoBuilder;
|
|
import com.pathplanner.lib.auto.NamedCommands;
|
|
import com.pathplanner.lib.commands.PathPlannerAuto;
|
|
import com.pathplanner.lib.util.FlippingUtil;
|
|
import edu.wpi.first.math.MathUtil;
|
|
import edu.wpi.first.math.geometry.Pose2d;
|
|
import edu.wpi.first.wpilibj.DriverStation;
|
|
import edu.wpi.first.wpilibj.DriverStation.Alliance;
|
|
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.RunCommand;
|
|
import edu.wpi.first.wpilibj2.command.SequentialCommandGroup;
|
|
import edu.wpi.first.wpilibj2.command.button.CommandXboxController;
|
|
import frc.robot.TunerConstants.TunerConstants;
|
|
import frc.robot.commands.Algue1Test;
|
|
import frc.robot.commands.Algue2Test;
|
|
import frc.robot.commands.AlgueExpire;
|
|
import frc.robot.commands.Algue_inspire;
|
|
import frc.robot.commands.AprilTag3;
|
|
import frc.robot.commands.AprilTag3G;
|
|
import frc.robot.commands.CorailAspir;
|
|
import frc.robot.commands.CorailTest;
|
|
import frc.robot.commands.CoralAlgueInspire;
|
|
import frc.robot.commands.CoralExpire;
|
|
import frc.robot.commands.Depart;
|
|
import frc.robot.commands.ElevateurManuel;
|
|
import frc.robot.commands.Forme3;
|
|
import frc.robot.commands.L2;
|
|
import frc.robot.commands.L3;
|
|
import frc.robot.commands.L4;
|
|
import frc.robot.commands.PinceManuel;
|
|
import frc.robot.commands.PinceManuel2;
|
|
import frc.robot.commands.RainBow;
|
|
import frc.robot.commands.StationPince;
|
|
import frc.robot.commands.reset;
|
|
import frc.robot.subsystems.Bougie;
|
|
import frc.robot.subsystems.CommandSwerveDrivetrain;
|
|
import frc.robot.subsystems.Elevateur;
|
|
import frc.robot.subsystems.Grimpeur;
|
|
import frc.robot.subsystems.Limelight3;
|
|
import frc.robot.subsystems.Limelight3G;
|
|
import frc.robot.subsystems.Pince;
|
|
|
|
public class RobotContainer {
|
|
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
|
|
|
|
private final Telemetry logger = new Telemetry(MaxSpeed);
|
|
private final CommandXboxController manette1 = new CommandXboxController(0);
|
|
private final CommandXboxController manette2 = new CommandXboxController(1);
|
|
private final Pigeon2 gyro = new Pigeon2(13); // ID du Pigeon 2
|
|
public final CommandSwerveDrivetrain drivetrain = TunerConstants.createDrivetrain();
|
|
private final SendableChooser<Command> autoChooser;
|
|
public double getAngle() {
|
|
return gyro.getYaw().getValueAsDouble(); // Retourne l'angle actuel du robot
|
|
}
|
|
|
|
Elevateur elevateur = new Elevateur();
|
|
Pince pince = new Pince();
|
|
ElevateurManuel elevateurManuel = new ElevateurManuel(elevateur, manette2::getLeftY);
|
|
PinceManuel pinceManuel = new PinceManuel(pince,manette2::getRightY);
|
|
PinceManuel2 pinceManuel2 = new PinceManuel2(pince);
|
|
Bougie bougie = new Bougie();
|
|
Limelight3G limelight3g = new Limelight3G();
|
|
Limelight3 limelight3 = new Limelight3();
|
|
Pose2d pose = new Pose2d();
|
|
Grimpeur Grimpeur = new Grimpeur();
|
|
|
|
public RobotContainer() {
|
|
autoChooser = AutoBuilder.buildAutoChooser("New Auto");
|
|
SmartDashboard.putData("Auto Mode", autoChooser);
|
|
configureBindings();
|
|
NamedCommands.registerCommand("AprilTag", new AprilTag3G(limelight3g, drivetrain, null, null));
|
|
NamedCommands.registerCommand("Station",new StationPince(pince, elevateur));
|
|
NamedCommands.registerCommand("L4", new L4(elevateur, pince));
|
|
NamedCommands.registerCommand("L3", new L3(elevateur, pince));
|
|
NamedCommands.registerCommand("CoralExpire",new CoralExpire(pince,bougie));
|
|
NamedCommands.registerCommand("CoraletAlgue", new CoralAlgueInspire(pince,bougie));
|
|
}
|
|
private void configureBindings() {
|
|
|
|
drivetrain.setDefaultCommand(
|
|
// Drivetrain will execute this command periodically
|
|
drivetrain.applyRequest(() ->
|
|
drive.withVelocityX(MathUtil.applyDeadband(Math.pow(-manette1.getLeftX(), 2)*MaxSpeed, 0.1)) // Drive forward with negative Y (forward)
|
|
.withVelocityY(MathUtil.applyDeadband(Math.pow(-manette1.getLeftY(), 2)*MaxSpeed, 0.10000)) // Drive left with negative X (left)
|
|
.withRotationalRate(MathUtil.applyDeadband(Math.pow(-manette1.getRightX(), 2)*manette1.getRightX()*MaxAngularRate, 0.1)) // Drive counterclockwise with negative X (left)
|
|
)
|
|
);
|
|
// Elevateur manuel
|
|
drivetrain.registerTelemetry(logger::telemeterize);
|
|
elevateur.setDefaultCommand(new RunCommand(()->{
|
|
elevateur.vitesse(MathUtil.applyDeadband(manette2.getLeftY(), 0.1));
|
|
}, elevateur));
|
|
|
|
//Pince manuel
|
|
pince.setDefaultCommand(new RunCommand(()->{
|
|
pince.pivote(MathUtil.applyDeadband(manette2.getRightY(), 0.1));
|
|
}, pince));
|
|
|
|
|
|
// manette1
|
|
|
|
// reset the field-centric heading on start press
|
|
manette1.start().onTrue(drivetrain.runOnce(() -> drivetrain.seedFieldCentric()));
|
|
|
|
//elevateur
|
|
manette1.a().whileTrue(new Depart(elevateur, pince));
|
|
manette1.b().whileTrue(new L2(elevateur,pince));
|
|
manette1.x().whileTrue(new L3(elevateur, pince));
|
|
manette1.y().whileTrue(new L4(elevateur, pince));
|
|
|
|
//pince
|
|
manette1.rightTrigger().whileTrue(new CoralAlgueInspire(pince, bougie));
|
|
manette1.rightBumper().whileTrue(new StationPince(pince, elevateur));
|
|
manette1.leftTrigger().whileTrue(new CoralExpire(pince, bougie));
|
|
|
|
//manette2
|
|
|
|
//pince
|
|
manette2.a().whileTrue(new CorailAspir(pince));
|
|
manette2.start().whileTrue(new reset(elevateur, pince));
|
|
manette2.b().whileTrue(new Algue_inspire(pince));
|
|
manette2.start().whileTrue(new reset(elevateur,pince));
|
|
manette2.leftTrigger().whileTrue(new AlgueExpire(pince, bougie));
|
|
|
|
//limelight
|
|
manette2.leftBumper().whileTrue(new AprilTag3(limelight3,drivetrain,manette1::getLeftX,manette1::getLeftY));
|
|
manette2.rightBumper().whileTrue(new Forme3(limelight3,drivetrain,manette1::getLeftX,manette1::getLeftY));
|
|
}
|
|
|
|
// manette2.leftTrigger().whileTrue(new AprilTag3G(limelight3g,drivetrain,manette1::getLeftX,manette1::getLeftY));
|
|
|
|
public Command getAutonomousCommand() {
|
|
return new SequentialCommandGroup(Commands.runOnce(()->{
|
|
boolean flip = DriverStation.getAlliance().orElse(Alliance.Blue) == Alliance.Red;
|
|
if(flip){
|
|
drivetrain.resetPose(FlippingUtil.flipFieldPose(((PathPlannerAuto)autoChooser.getSelected()).getStartingPose()));
|
|
}
|
|
else{
|
|
drivetrain.resetPose(((PathPlannerAuto)autoChooser.getSelected()).getStartingPose());
|
|
}
|
|
}),autoChooser.getSelected(), new RainBow(bougie));
|
|
}
|
|
} |