102 lines
4.7 KiB
Java
102 lines
4.7 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.mechanisms.swerve.LegacySwerveRequest.PointWheelsAt;
|
|
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.math.geometry.Pose2d;
|
|
import edu.wpi.first.math.geometry.Rotation2d;
|
|
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.ParallelCommandGroup;
|
|
import edu.wpi.first.wpilibj2.command.SequentialCommandGroup;
|
|
import edu.wpi.first.wpilibj2.command.button.CommandXboxController;
|
|
import edu.wpi.first.wpilibj2.command.sysid.SysIdRoutine.Direction;
|
|
|
|
import frc.robot.TunerConstants.TunerConstants;
|
|
import frc.robot.commands.AprilTag3;
|
|
import frc.robot.commands.AprilTag3G;
|
|
import frc.robot.commands.Forme3;
|
|
import frc.robot.commands.RainBow;
|
|
import frc.robot.subsystems.Bougie;
|
|
import frc.robot.subsystems.CommandSwerveDrivetrain;
|
|
import frc.robot.subsystems.Grimpeur;
|
|
import frc.robot.subsystems.Limelight3;
|
|
import frc.robot.subsystems.Limelight3G;
|
|
|
|
|
|
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);
|
|
|
|
public final CommandSwerveDrivetrain drivetrain = TunerConstants.createDrivetrain();
|
|
|
|
private final SendableChooser<Command> autoChooser;
|
|
Bougie bougie = new Bougie();
|
|
Limelight3G limelight3g = new Limelight3G();
|
|
Limelight3 limelight3 = new Limelight3();
|
|
Pose2d pose = new Pose2d();
|
|
private final Pigeon2 gyro = new Pigeon2(13); // ID du Pigeon 2
|
|
public double getAngle() {
|
|
return gyro.getYaw().getValueAsDouble(); // Retourne l'angle actuel du robot
|
|
}
|
|
public RobotContainer() {
|
|
autoChooser = AutoBuilder.buildAutoChooser("New Auto");
|
|
SmartDashboard.putData("Auto Mode", autoChooser);
|
|
configureBindings();
|
|
}
|
|
|
|
private void configureBindings() {
|
|
// Note that X is defined as forward according to WPILib convention,
|
|
// and Y is defined as to the left according to WPILib convention.
|
|
drivetrain.setDefaultCommand(
|
|
// Drivetrain will execute this command periodically
|
|
drivetrain.applyRequest(() ->
|
|
drive.withVelocityX(MathUtil.applyDeadband(-manette1.getLeftY()*MaxSpeed, 0.2)) // Drive forward with negative Y (forward)
|
|
.withVelocityY(MathUtil.applyDeadband(-manette1.getLeftX()*MaxSpeed, 0.2)) // Drive left with negative X (left)
|
|
.withRotationalRate(MathUtil.applyDeadband(-manette1.getRightX()*MaxAngularRate, 0.15)) // Drive counterclockwise with negative X (left)
|
|
)
|
|
);
|
|
|
|
|
|
// reset the field-centric heading on left bumper press
|
|
manette1.start().onTrue(drivetrain.runOnce(() -> drivetrain.seedFieldCentric()));
|
|
manette1.leftTrigger().whileTrue(new AprilTag3(limelight3,drivetrain,manette1::getLeftX,manette1::getLeftY));
|
|
manette1.rightTrigger().whileTrue(new Forme3(limelight3,drivetrain,manette1::getLeftX,manette1::getLeftY));
|
|
manette2.leftTrigger().whileTrue(new AprilTag3G(limelight3g,drivetrain,manette1::getLeftX,manette1::getLeftY));
|
|
drivetrain.registerTelemetry(logger::telemeterize);
|
|
}
|
|
|
|
|
|
public Command getAutonomousCommand() {
|
|
return new SequentialCommandGroup(autoChooser.getSelected(),
|
|
new RainBow(bougie));
|
|
}
|
|
|
|
}
|