mirror of
https://github.com/Team4388/2026KPopRobotHunters.git
synced 2026-06-09 08:48:05 -06:00
C4llSqin
222 lines
9.2 KiB
Java
222 lines
9.2 KiB
Java
/*----------------------------------------------------------------------------*/
|
|
/* Copyright (c) 2018-2019 FIRST. All Rights Reserved. */
|
|
/* Open Source Software - may be modified and shared by FRC teams. The code */
|
|
/* must be accompanied by the FIRST BSD license file in the root directory of */
|
|
/* the project. */
|
|
/*----------------------------------------------------------------------------*/
|
|
|
|
package frc4388.robot;
|
|
|
|
// Drive Systems
|
|
import edu.wpi.first.wpilibj.DriverStation;
|
|
|
|
import java.io.File;
|
|
import edu.wpi.first.math.geometry.Rotation2d;
|
|
import edu.wpi.first.math.geometry.Translation2d;
|
|
import edu.wpi.first.math.util.Units;
|
|
import edu.wpi.first.wpilibj.GenericHID;
|
|
import edu.wpi.first.wpilibj.RobotBase;
|
|
import edu.wpi.first.wpilibj.smartdashboard.SendableChooser;
|
|
import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
|
|
import frc4388.utility.controller.XboxController;
|
|
import frc4388.utility.controller.ButtonBox;
|
|
import frc4388.utility.controller.DeadbandedXboxController;
|
|
import edu.wpi.first.wpilibj2.command.button.JoystickButton;
|
|
import edu.wpi.first.wpilibj2.command.button.Trigger;
|
|
|
|
// Commands
|
|
import edu.wpi.first.wpilibj2.command.Command;
|
|
import edu.wpi.first.wpilibj2.command.InstantCommand;
|
|
import edu.wpi.first.wpilibj2.command.ParallelRaceGroup;
|
|
import edu.wpi.first.wpilibj2.command.RunCommand;
|
|
import edu.wpi.first.wpilibj2.command.SequentialCommandGroup;
|
|
import edu.wpi.first.wpilibj2.command.WaitCommand;
|
|
import edu.wpi.first.wpilibj2.command.Commands;
|
|
import edu.wpi.first.wpilibj2.command.ConditionalCommand;
|
|
// Autos
|
|
import frc4388.utility.controller.VirtualController;
|
|
import frc4388.robot.commands.MoveForTimeCommand;
|
|
import frc4388.robot.commands.MoveUntilSuply;
|
|
// import frc4388.robot.commands.alignment.DriveToReef;
|
|
import frc4388.robot.commands.alignment.DriveUntilLiDAR;
|
|
import frc4388.robot.commands.alignment.LidarAlign;
|
|
// import frc4388.robot.commands.wait.waitElevatorRefrence;
|
|
// import frc4388.robot.commands.wait.waitEndefectorRefrence;
|
|
// import frc4388.robot.commands.wait.waitFeedCoral;
|
|
import frc4388.robot.commands.wait.waitSupplier;
|
|
import frc4388.robot.constants.Constants;
|
|
import frc4388.robot.constants.Constants.AutoConstants;
|
|
import frc4388.robot.constants.Constants.LiDARConstants;
|
|
import frc4388.robot.constants.Constants.OIConstants;
|
|
import frc4388.robot.constants.Constants.SimConstants.Mode;
|
|
|
|
import com.pathplanner.lib.auto.NamedCommands;
|
|
import com.pathplanner.lib.commands.PathPlannerAuto;
|
|
|
|
// Subsystems
|
|
import frc4388.robot.subsystems.LED;
|
|
// import frc4388.robot.subsystems.elevator.Elevator;
|
|
// import frc4388.robot.subsystems.elevator.Elevator.CoordinationState;
|
|
import frc4388.robot.subsystems.lidar.LiDAR;
|
|
import frc4388.robot.subsystems.swerve.SwerveDrive;
|
|
import frc4388.robot.subsystems.vision.Vision;
|
|
// Utilites
|
|
import frc4388.utility.DeferredBlock;
|
|
import frc4388.utility.compute.TimesNegativeOne;
|
|
// import frc4388.utility.compute.ReefPositionHelper.Side;
|
|
|
|
/**
|
|
* This class is where the bulk of the robot should be declared. Since
|
|
* Command-based is a "declarative" paradigm, very little robot logic should
|
|
* actually be handled in the {@link Robot} periodic methods (other than the
|
|
* scheduler calls). Instead, the structure of the robot (2including subsystems,
|
|
* commands, and button mappings) should be declared here.
|
|
*/
|
|
public class RobotContainer {
|
|
/* RobotMap */
|
|
|
|
public final RobotMap m_robotMap = new RobotMap(Mode.REAL);
|
|
|
|
/* Subsystems */
|
|
public final LED m_robotLED = new LED();
|
|
public final Vision m_vision = new Vision();
|
|
// public final Elevator m_robotElevator = new Elevator(m_robotMap.elevatorIO, m_robotLED);
|
|
public final SwerveDrive m_robotSwerveDrive = new SwerveDrive(m_robotMap.swerveDrivetrain, m_vision);
|
|
// public final SwerveDrive m_robotSwerveDrive = new SwerveDrive(m_robotMap.swerveDrivetrain);
|
|
|
|
// public final LiDAR reefLidar = new LiDAR(m_robotMap.reefLidar, "Reef");
|
|
// public final LiDAR reverseLidar = new LiDAR(m_robotMap.reverseLidar, "Reverse");
|
|
|
|
|
|
/* Controllers */
|
|
private final DeadbandedXboxController m_driverXbox = new DeadbandedXboxController(OIConstants.XBOX_DRIVER_ID);
|
|
private final DeadbandedXboxController m_operatorXbox = new DeadbandedXboxController(OIConstants.XBOX_OPERATOR_ID);
|
|
private final ButtonBox m_buttonBox = new ButtonBox(OIConstants.BUTTONBOX_ID);
|
|
|
|
// public List<Subsystem> subsystems = new ArrayList<>();
|
|
|
|
// ! Teleop Commands
|
|
public void stop() {
|
|
new InstantCommand(()->{}, m_robotSwerveDrive).schedule();
|
|
m_robotSwerveDrive.stopModules();
|
|
Constants.AutoConstants.Y_OFFSET_TRIM.set(0);
|
|
}
|
|
|
|
// ! /* Autos */
|
|
private SendableChooser<String> autoChooser;
|
|
private Command autoCommand;
|
|
|
|
|
|
/**
|
|
* This method is used to replcate {@link Trigger Triggers} for {@link VirtualController Virtual Controllers}. <p/>
|
|
* Please use {@link RobotContainer#DualJoystickButton} in {@link RobotContainer#configureButtonBindings} for standard buttons.
|
|
*/
|
|
private void configureVirtualButtonBindings() {
|
|
|
|
// ? /* Driver Buttons */
|
|
|
|
/* Notice: the following buttons have not been replicated
|
|
* Swerve Drive Slow and Fast mode Gear Shifts : Fast mode is known to cause drift, so we disable that feature in Autoplayback
|
|
* Swerve Drive Rotation Gear Shifts : Same reason as Slow and Fast mode.
|
|
* Auto Recording controls : We don't want an Null Ouroboros for an auto.
|
|
*/
|
|
|
|
// ? /* Operator Buttons */
|
|
|
|
/* Notice: the following buttons have not been replicated
|
|
* Override Intake Position Encoder : It's an emergancy overide, for when the position of intake when the robot boots, the intake is not inside the robot.
|
|
* We don't need it in an auto.
|
|
* Climbing controls : We don't need to climb in auto.
|
|
*/
|
|
|
|
// ? Notice: the Programer Buttons are not to be replicated because they are designed for debuging the robot, and do not need to be replicated in auto.
|
|
|
|
}
|
|
|
|
/**
|
|
* Use this to pass the autonomous command to the main {@link Robot} class.
|
|
*
|
|
* @return the command to run in autonomous
|
|
*/
|
|
public Command getAutonomousCommand() {
|
|
|
|
|
|
//return autoPlayback;
|
|
//return new GotoPositionCommand(m_robotSwerveDrive, m_vision)
|
|
//return autoChooser.getSelected();
|
|
// try{
|
|
// // // Load the path you want to follow using its name in the GUI
|
|
// return autoCommand;
|
|
// } catch (Exception e) {
|
|
// DriverStation.reportError("Path planner error: " + e.getMessage(), e.getStackTrace());
|
|
return autoCommand;
|
|
// }
|
|
// return new PathPlannerAuto("Line-up-no-arm");
|
|
// zach told me to do the below comment
|
|
//return new GotoPositionCommand(m_robotSwerveDrive, m_vision);
|
|
// return new GotoPositionCommand(m_robotSwerveDrive, m_vision, AutoConstants.targetpos);
|
|
}
|
|
|
|
public boolean autoChooserUpdated = false;
|
|
public void makeAutoChooser() {
|
|
autoChooser = new SendableChooser<String>();
|
|
|
|
File dir;
|
|
|
|
if(RobotBase.isReal()) {
|
|
dir = new File("/home/lvuser/deploy/pathplanner/autos/");
|
|
} else {
|
|
// dir = new File("C:\\Users\\Ridgebotics\\Documents\\GitHub\\2025RidgeScape\\src\\main\\deploy\\pathplanner\\autos\\");
|
|
dir = new File("/home/astatin3/Documents/GitHub/2025RidgeScape/src/main/deploy/pathplanner/autos");
|
|
}
|
|
|
|
String[] autos = dir.list();
|
|
|
|
if(autos == null) return;
|
|
|
|
for (String auto : autos) {
|
|
if (auto.endsWith(".auto"))
|
|
autoChooser.addOption(auto.replaceAll(".auto", ""), auto.replaceAll(".auto", ""));
|
|
// System.out.println(auto);
|
|
}
|
|
|
|
autoChooser.onChange((filename) -> {
|
|
autoChooserUpdated = true;
|
|
if (filename.equals("Taxi")) {
|
|
autoCommand = new SequentialCommandGroup(
|
|
new MoveForTimeCommand(m_robotSwerveDrive,
|
|
new Translation2d(0, -1),
|
|
new Translation2d(), 1000, true
|
|
), new InstantCommand(()-> {m_robotSwerveDrive.softStop();} , m_robotSwerveDrive));
|
|
} else {
|
|
autoCommand = new PathPlannerAuto(filename);
|
|
}
|
|
System.out.println("Robot Auto Changed " + filename);
|
|
});
|
|
// SmartDashboard.putData(autoChooser);
|
|
|
|
}
|
|
|
|
/**
|
|
* A button binding for two controllers, preferably an {@link DeadbandedXboxController Xbox Controller} and {@link VirtualController Virtual Xbox Controller}
|
|
* @param joystickA A controller
|
|
* @param joystickB A controller
|
|
* @param buttonNumber The button to bind to
|
|
*/
|
|
public Trigger DualJoystickButton(GenericHID joystickA, GenericHID joystickB, int buttonNumber) {
|
|
return new Trigger(() -> (joystickA.getRawButton(buttonNumber) || joystickB.getRawButton(buttonNumber)));
|
|
}
|
|
|
|
public DeadbandedXboxController getDeadbandedDriverController() {
|
|
return this.m_driverXbox;
|
|
}
|
|
|
|
public DeadbandedXboxController getDeadbandedOperatorController() {
|
|
return this.m_operatorXbox;
|
|
}
|
|
|
|
public ButtonBox getButtonBox() {
|
|
return this.m_buttonBox;
|
|
}
|
|
}
|