Modes Autos complets
This commit is contained in:
@ -4,17 +4,81 @@
|
||||
|
||||
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.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.button.CommandXboxController;
|
||||
import edu.wpi.first.wpilibj2.command.sysid.SysIdRoutine.Direction;
|
||||
|
||||
import frc.robot.TunerConstants.TunerConstants;
|
||||
import frc.robot.commands.Grimpe;
|
||||
import frc.robot.subsystems.CommandSwerveDrivetrain;
|
||||
import frc.robot.subsystems.Grimpeur;
|
||||
|
||||
|
||||
public class RobotContainer {
|
||||
public RobotContainer() {
|
||||
configureBindings();
|
||||
}
|
||||
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
|
||||
|
||||
private void configureBindings() {}
|
||||
/* 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 Command getAutonomousCommand() {
|
||||
return Commands.print("No autonomous command configured");
|
||||
}
|
||||
private final Telemetry logger = new Telemetry(MaxSpeed);
|
||||
|
||||
private final CommandXboxController joystick = new CommandXboxController(0);
|
||||
|
||||
public final CommandSwerveDrivetrain drivetrain = TunerConstants.createDrivetrain();
|
||||
|
||||
private final SendableChooser<Command> autoChooser;
|
||||
|
||||
|
||||
|
||||
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(-joystick.getLeftY() * MaxSpeed) // Drive forward with negative Y (forward)
|
||||
.withVelocityY(-joystick.getLeftX() * MaxSpeed) // Drive left with negative X (left)
|
||||
.withRotationalRate(-joystick.getRightX() * MaxAngularRate) // Drive counterclockwise with negative X (left)
|
||||
)
|
||||
);
|
||||
|
||||
// Run SysId routines when holding back/start and X/Y.
|
||||
// Note that each routine should be run exactly once in a single log.
|
||||
joystick.back().and(joystick.y()).whileTrue(drivetrain.sysIdDynamic(Direction.kForward));
|
||||
joystick.back().and(joystick.x()).whileTrue(drivetrain.sysIdDynamic(Direction.kReverse));
|
||||
joystick.start().and(joystick.y()).whileTrue(drivetrain.sysIdQuasistatic(Direction.kForward));
|
||||
joystick.start().and(joystick.x()).whileTrue(drivetrain.sysIdQuasistatic(Direction.kReverse));
|
||||
|
||||
// reset the field-centric heading on left bumper press
|
||||
joystick.leftBumper().onTrue(drivetrain.runOnce(() -> drivetrain.seedFieldCentric()));
|
||||
|
||||
drivetrain.registerTelemetry(logger::telemeterize);
|
||||
}
|
||||
|
||||
public Command getAutonomousCommand() {
|
||||
return autoChooser.getSelected();
|
||||
}
|
||||
}
|
||||
|
Reference in New Issue
Block a user