95 lines
3.9 KiB
Java
95 lines
3.9 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.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.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.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.RainBow;
|
|
import frc.robot.subsystems.Bougie;
|
|
import frc.robot.subsystems.CommandSwerveDrivetrain;
|
|
import frc.robot.subsystems.Grimpeur;
|
|
|
|
|
|
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(0);
|
|
|
|
public final CommandSwerveDrivetrain drivetrain = TunerConstants.createDrivetrain();
|
|
|
|
private final SendableChooser<Command> autoChooser;
|
|
Bougie bougie = new Bougie();
|
|
|
|
|
|
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(), 0.5)) // Drive forward with negative Y (forward)
|
|
.withVelocityY(MathUtil.applyDeadband(-manette1.getLeftX(), 0.5)) // Drive left with negative X (left)
|
|
.withRotationalRate(MathUtil.applyDeadband(-manette1.getRightX()*MaxAngularRate, 0.5)) // Drive counterclockwise with negative X (left)
|
|
)
|
|
);
|
|
|
|
|
|
// reset the field-centric heading on left bumper press
|
|
manette1.start().onTrue(drivetrain.runOnce(() -> drivetrain.seedFieldCentric()));
|
|
|
|
drivetrain.registerTelemetry(logger::telemeterize);
|
|
}
|
|
|
|
|
|
public Command getAutonomousCommand() {
|
|
return
|
|
new
|
|
SequentialCommandGroup(
|
|
autoChooser
|
|
.getSelected().andThen(
|
|
new
|
|
RainBow
|
|
(
|
|
bougie
|
|
)
|
|
));
|
|
}
|
|
}
|