100 lines
4.6 KiB
Java
100 lines
4.6 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 com.ctre.phoenix6.Utils;
|
|
import com.ctre.phoenix6.mechanisms.swerve.SwerveRequest;
|
|
import com.ctre.phoenix6.mechanisms.swerve.SwerveModule.DriveRequestType;
|
|
|
|
import edu.wpi.first.math.MathUtil;
|
|
import edu.wpi.first.math.geometry.Pose2d;
|
|
import edu.wpi.first.math.geometry.Rotation2d;
|
|
import edu.wpi.first.math.geometry.Translation2d;
|
|
import edu.wpi.first.wpilibj.shuffleboard.Shuffleboard;
|
|
import edu.wpi.first.wpilibj.shuffleboard.ShuffleboardTab;
|
|
import edu.wpi.first.wpilibj.smartdashboard.SendableChooser;
|
|
import edu.wpi.first.wpilibj2.command.Command;
|
|
import edu.wpi.first.wpilibj2.command.RunCommand;
|
|
import edu.wpi.first.wpilibj2.command.button.CommandXboxController;
|
|
import frc.robot.Commands.Desaccumuler;
|
|
import frc.robot.Commands.FollowAprilTag;
|
|
import frc.robot.Commands.LEDactive;
|
|
import frc.robot.Commands.Lancer;
|
|
import frc.robot.Subsystems.Accumulateur;
|
|
import frc.robot.Subsystems.CommandSwerveDrivetrain;
|
|
import frc.robot.Subsystems.LED;
|
|
//import frc.robot.Subsystems.Drive;
|
|
import frc.robot.Subsystems.Lanceur;
|
|
import frc.robot.Subsystems.Limelight3G;
|
|
import frc.robot.generated.TunerConstants;
|
|
//import com.pathplanner.lib.auto.AutoBuilder;
|
|
//import com.pathplanner.lib.auto.NamedCommands;
|
|
public class RobotContainer {
|
|
CommandXboxController manette = new CommandXboxController(0);
|
|
private double MaxSpeed = TunerConstants.kSpeedAt12VoltsMps; // kSpeedAt12VoltsMps desired top speed
|
|
private double MaxAngularRate = 1.5 * Math.PI; // 3/4 of a rotation per second max angular velocity
|
|
|
|
/* Setting up bindings for necessary control of the swerve drive platform */ // My joystick
|
|
private final CommandSwerveDrivetrain drivetrain = TunerConstants.DriveTrain; // My drivetrain
|
|
|
|
private final SwerveRequest.FieldCentric drive = new SwerveRequest.FieldCentric()
|
|
.withDeadband(MaxSpeed * 0.15).withRotationalDeadband(MaxAngularRate * 0.15) // Add a 10% deadband
|
|
.withDriveRequestType(DriveRequestType.OpenLoopVoltage); // I want field-centric
|
|
// driving in open loop
|
|
private final SwerveRequest.PointWheelsAt point = new SwerveRequest.PointWheelsAt();
|
|
|
|
private final Telemetry logger = new Telemetry(MaxSpeed);
|
|
private void configureBindings() {
|
|
drivetrain.setDefaultCommand( // Drivetrain will execute this command periodically
|
|
drivetrain.applyRequest(() -> drive.withVelocityX(-manette.getLeftY() * MaxSpeed) // Drive forward with
|
|
// negative Y (forward)
|
|
.withVelocityY(-manette.getLeftX() * MaxSpeed) // Drive left with negative X (left)
|
|
.withRotationalRate(-manette.getRightX() * MaxAngularRate) // Drive counterclockwise with negative X (left)
|
|
));
|
|
|
|
manette.b().whileTrue(drivetrain
|
|
.applyRequest(() -> point.withModuleDirection(new Rotation2d(-manette.getLeftY(), -manette.getLeftX()))));
|
|
|
|
// reset the field-centric heading on left bumper press
|
|
manette.leftBumper().onTrue(drivetrain.runOnce(() -> drivetrain.seedFieldRelative()));
|
|
|
|
if (Utils.isSimulation()) {
|
|
drivetrain.seedFieldRelative(new Pose2d(new Translation2d(), Rotation2d.fromDegrees(90)));
|
|
}
|
|
drivetrain.registerTelemetry(logger::telemeterize);
|
|
}
|
|
|
|
// private final SendableChooser<Command> autoChooser;
|
|
Lanceur lanceur= new Lanceur();
|
|
Accumulateur accumulateur = new Accumulateur();
|
|
Limelight3G limelight3G = new Limelight3G();
|
|
LED led = new LED();
|
|
//Drive drive = new Drive();
|
|
ShuffleboardTab dashboard = Shuffleboard.getTab("dashboard");
|
|
|
|
public RobotContainer() {
|
|
//NamedCommands.registerCommand("lancer", new Lancer(lanceur, accumulateur));
|
|
//NamedCommands.registerCommand("Desaccumuler", new Desaccumuler(accumulateur));
|
|
//autoChooser = AutoBuilder.buildAutoChooser();
|
|
dashboard.addCamera("limelight3G", "limelight3G","limelight.local:5800")
|
|
.withSize(3,4)
|
|
.withPosition(5,0);
|
|
dashboard.addCamera("limelight3", "limelight3","limelight.local:5800")
|
|
.withSize(3,4)
|
|
.withPosition(2,0);
|
|
|
|
|
|
manette.x().whileTrue(new Lancer(lanceur,accumulateur));
|
|
manette.leftBumper().toggleOnTrue(new FollowAprilTag(limelight3G, lanceur));
|
|
manette.a().whileTrue(new Desaccumuler(accumulateur));
|
|
manette.y().whileTrue(new LEDactive(accumulateur, led));
|
|
}
|
|
public Command getAutonomousCommand() {
|
|
return null;
|
|
// return autoChooser.getSelected();
|
|
}
|
|
} |