mirror of
https://github.com/Team4388/2026KPopRobotHunters.git
synced 2026-06-09 00:38:03 -06:00
Initial commit
This commit is contained in:
mirage54321
@@ -0,0 +1,221 @@
|
||||
/*----------------------------------------------------------------------------*/
|
||||
/* 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(m_robotMap.leftCamera, m_robotMap.rightCamera);
|
||||
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;
|
||||
}
|
||||
}
|
||||
Reference in New Issue
Block a user