Team4388/2023WayOfTheRobot
1
0
Fork 0
mirror of https://github.com/Team4388/2023WayOfTheRobot.git synced 2026-06-09 00:37:59 -06:00
Code Issues Packages Projects Releases Wiki Activity

Merge branch 'master' into vision-april-lime

Browse Source
This commit is contained in:
Aarav Shah
2023-02-27 17:37:08 -07:00
committed by GitHub
parent 92243d0942 61f5911e48
commit deaf36df0a
14 changed files with 957 additions and 90 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/main/java/frc4388/robot/RobotContainer.java
+92 -48
View File
@@ -7,15 +7,42 @@
package frc4388.robot;
import java.util.ArrayList;
import java.util.List;
import org.opencv.objdetect.HOGDescriptor;
import edu.wpi.first.math.controller.HolonomicDriveController;
import edu.wpi.first.math.controller.PIDController;
import edu.wpi.first.math.controller.ProfiledPIDController;
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.math.trajectory.Trajectory;
import edu.wpi.first.math.trajectory.TrajectoryConfig;
import edu.wpi.first.math.trajectory.TrajectoryGenerator;
import edu.wpi.first.math.trajectory.Trajectory.State;
import edu.wpi.first.wpilibj.Joystick;
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.InstantCommand;
import edu.wpi.first.wpilibj2.command.RunCommand;
import edu.wpi.first.wpilibj2.command.SequentialCommandGroup;
import edu.wpi.first.wpilibj2.command.SwerveControllerCommand;
import edu.wpi.first.wpilibj2.command.button.JoystickButton;
import frc4388.robot.Constants.*;
import frc4388.robot.Constants.SwerveDriveConstants.AutoConstants;
import frc4388.robot.Constants.SwerveDriveConstants.PIDConstants;
import frc4388.robot.commands.AutoBalance;
import frc4388.robot.commands.DriveWithInput;
import frc4388.robot.subsystems.SwerveDrive;
import frc4388.utility.controller.IHandController;
import frc4388.robot.commands.JoystickPlayback;
import frc4388.robot.commands.JoystickRecorder;
import frc4388.robot.commands.PlaybackChooser;
import frc4388.robot.subsystems.SwerveDrive;
import frc4388.utility.controller.DeadbandedXboxController;
import frc4388.utility.controller.XboxController;
/**
@@ -30,31 +57,60 @@ public class RobotContainer {
public final RobotMap m_robotMap = new RobotMap();
/* Subsystems */
public final SwerveDrive m_robotSwerveDrive = new SwerveDrive(m_robotMap.leftFront, m_robotMap.rightFront, m_robotMap.leftBack, m_robotMap.rightBack);//, m_robotMap.gyro);
// private final LED m_robotLED = new LED(m_robotMap.LEDController);
public final SwerveDrive m_robotSwerveDrive = new SwerveDrive(m_robotMap.leftFront,
m_robotMap.rightFront,
m_robotMap.leftBack,
m_robotMap.rightBack,
m_robotMap.gyro);
/* Controllers */
private final XboxController m_driverXbox = new XboxController(OIConstants.XBOX_DRIVER_ID);
private final XboxController m_operatorXbox = new XboxController(OIConstants.XBOX_OPERATOR_ID);
private final DeadbandedXboxController m_driverXbox = new DeadbandedXboxController(OIConstants.XBOX_DRIVER_ID);
private final DeadbandedXboxController m_operatorXbox = new DeadbandedXboxController(OIConstants.XBOX_OPERATOR_ID);
/* Autos */
public SendableChooser<Command> chooser = new SendableChooser<>();
private Command noAuto = new InstantCommand();
// private Command balance = new AutoBalance(m_robotMap.gyro, m_robotSwerveDrive);
// private Command blue1Path = new JoystickPlayback(m_robotSwerveDrive, "Blue1Path.txt");
// private Command blue1PathWithBalance = new JoystickPlayback(m_robotSwerveDrive, "Blue1Path.txt").andThen(new AutoBalance(m_robotMap.gyro, m_robotSwerveDrive));
// private Command red1Path = new JoystickPlayback(m_robotSwerveDrive, "Blue1Path.txt", -1);
// private Command red1PathWithBalance = new JoystickPlayback(m_robotSwerveDrive, "Blue1Path.txt", -1).andThen(new AutoBalance(m_robotMap.gyro, m_robotSwerveDrive));
// private Command taxi = new JoystickPlayback(m_robotSwerveDrive, "Taxi.txt");
private PlaybackChooser playbackChooser;
/**
* The container for the robot. Contains subsystems, OI devices, and commands.
*/
public RobotContainer() {
configureButtonBindings();
/* Default Commands */
m_robotSwerveDrive.setDefaultCommand(new DriveWithInput(m_robotSwerveDrive,
() -> getDriverController().getLeftXAxis(),
() -> getDriverController().getLeftYAxis(),
() -> getDriverController().getRightXAxis(),
false));
// * Default Commands
m_robotSwerveDrive.setDefaultCommand(new RunCommand(() -> {
m_robotSwerveDrive.driveWithInput(getDeadbandedDriverController().getLeft(),
getDeadbandedDriverController().getRight(),
true);
}, m_robotSwerveDrive)
.withName("SwerveDrive DefaultCommand"));
// m_robotLED.setDefaultCommand(new RunCommand(() -> m_robotLED.updateLED(), m_robotLED));
// * Auto Commands
// chooser.setDefaultOption("NoAuto", noAuto);
// chooser.addOption("Blue1Path", blue1Path);
// chooser.addOption("Blue1PathWithBalance", blue1PathWithBalance);
// chooser.addOption("Red1Path", red1Path);
// chooser.addOption("Red1PathWithBalance", red1PathWithBalance);
// chooser.addOption("Taxi", taxi);
playbackChooser = new PlaybackChooser(m_robotSwerveDrive)
.addOption("Balance", new AutoBalance(m_robotMap.gyro, m_robotSwerveDrive))
.buildDisplay();
}
@@ -65,20 +121,27 @@ public class RobotContainer {
* passing it to a {@link edu.wpi.first.wpilibj2.command.button.JoystickButton}.
*/
private void configureButtonBindings() {
/* Driver Buttons */
// * Driver Buttons
new JoystickButton(getDeadbandedDriverController(), XboxController.A_BUTTON)
.onTrue(new InstantCommand(() -> m_robotSwerveDrive.resetGyro(), m_robotSwerveDrive));
new JoystickButton(getDriverJoystick(), XboxController.A_BUTTON)
.onTrue(new InstantCommand(() -> m_robotMap.gyro.reset()));
// .onFalse()
/* Operator Buttons */
// new JoystickButton(getDeadbandedDriverController(), XboxController.X_BUTTON)
// .onTrue(new InstantCommand(() -> m_robotSwerveDrive.resetOdometry(), m_robotSwerveDrive));
// // .onFalse()
new JoystickButton(getDriverJoystick(), XboxController.Y_BUTTON)
.onTrue(new AutoBalance(m_robotMap.gyro, m_robotSwerveDrive));
// interrupt button
new JoystickButton(getOperatorJoystick(), XboxController.X_BUTTON)
.onTrue(new InstantCommand());
new JoystickButton(getDeadbandedDriverController(), XboxController.RIGHT_BUMPER_BUTTON)
.whileTrue(new JoystickRecorder(m_robotSwerveDrive,
() -> getDeadbandedDriverController().getLeftX(),
() -> getDeadbandedDriverController().getLeftY(),
() -> getDeadbandedDriverController().getRightX(),
() -> getDeadbandedDriverController().getRightY(),
"Blue1Path.txt"))
.onFalse(new InstantCommand());
// * Operator Buttons
}
/**
@@ -87,35 +150,16 @@ public class RobotContainer {
* @return the command to run in autonomous
*/
public Command getAutonomousCommand() {
return new InstantCommand();
// return chooser.getSelected();
return playbackChooser.getCommand();
}
/**
* Add your docs here.
*/
public IHandController getDriverController() {
return m_driverXbox;
public DeadbandedXboxController getDeadbandedDriverController() {
return this.m_driverXbox;
}
/**
* Add your docs here.
*/
public IHandController getOperatorController() {
return m_operatorXbox;
public DeadbandedXboxController getDeadbandedOperatorController() {
return this.m_operatorXbox;
}
/**
* Add your docs here.
*/
public Joystick getOperatorJoystick() {
return m_operatorXbox.getJoyStick();
}
/**
* Add your docs here.
*/
public Joystick getDriverJoystick() {
return m_driverXbox.getJoyStick();
}
}