mirror of
https://github.com/Team4388/2022NoWayHome.git
synced 2026-06-09 00:38:05 -06:00
Merge remote-tracking branch 'origin/testRoboReveal' into cleanup
This commit is contained in:
nathanrsxtn
@@ -4,24 +4,42 @@
|
||||
|
||||
package frc4388.robot;
|
||||
|
||||
import java.io.File;
|
||||
import java.io.IOException;
|
||||
import java.io.StringWriter;
|
||||
import java.nio.file.FileSystems;
|
||||
import java.nio.file.StandardWatchEventKinds;
|
||||
import java.nio.file.WatchEvent;
|
||||
import java.nio.file.WatchKey;
|
||||
import java.time.ZonedDateTime;
|
||||
import java.util.ArrayList;
|
||||
import java.util.Arrays;
|
||||
import java.util.Comparator;
|
||||
import java.util.List;
|
||||
import java.util.Objects;
|
||||
import java.util.Optional;
|
||||
import java.util.logging.Logger;
|
||||
import java.util.stream.Collectors;
|
||||
|
||||
import com.diffplug.common.base.Errors;
|
||||
import com.pathplanner.lib.PathPlanner;
|
||||
import com.pathplanner.lib.PathPlannerTrajectory;
|
||||
import com.pathplanner.lib.PathPlannerTrajectory.PathPlannerState;
|
||||
import com.pathplanner.lib.commands.PPSwerveControllerCommand;
|
||||
|
||||
import edu.wpi.first.math.Pair;
|
||||
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.wpilibj.GenericHID;
|
||||
import edu.wpi.first.wpilibj.RobotBase;
|
||||
import edu.wpi.first.wpilibj.XboxController;
|
||||
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.PrintCommand;
|
||||
import edu.wpi.first.wpilibj2.command.NotifierCommand;
|
||||
import edu.wpi.first.wpilibj2.command.RunCommand;
|
||||
import edu.wpi.first.wpilibj2.command.SequentialCommandGroup;
|
||||
import edu.wpi.first.wpilibj2.command.button.JoystickButton;
|
||||
@@ -29,9 +47,8 @@ import frc4388.robot.Constants.OIConstants;
|
||||
import frc4388.robot.Constants.StorageConstants;
|
||||
import frc4388.robot.Constants.SwerveDriveConstants;
|
||||
import frc4388.robot.commands.PathRecorder;
|
||||
// import frc4388.robot.commands.ButtonBoxCommands.TurretManual;
|
||||
import frc4388.robot.commands.DriveCommands.DriveWithInputForTime;
|
||||
import frc4388.robot.commands.ExtenderIntakeCommands.ExtenderIntakeGroup;
|
||||
import frc4388.robot.commands.ShooterCommands.Shoot;
|
||||
import frc4388.robot.commands.ShooterCommands.TrackTarget;
|
||||
import frc4388.robot.subsystems.BoomBoom;
|
||||
import frc4388.robot.subsystems.Claws;
|
||||
@@ -44,8 +61,9 @@ import frc4388.robot.subsystems.Storage;
|
||||
import frc4388.robot.subsystems.SwerveDrive;
|
||||
import frc4388.robot.subsystems.Turret;
|
||||
import frc4388.robot.subsystems.VisionOdometry;
|
||||
import frc4388.utility.PathPlannerUtil;
|
||||
import frc4388.utility.controller.ButtonBox;
|
||||
import frc4388.utility.controller.DeadbandedXboxController;
|
||||
import frc4388.utility.controller.DeadbandedRawXboxController;
|
||||
|
||||
/**
|
||||
* This class is where the bulk of the robot should be declared. Since
|
||||
@@ -78,8 +96,8 @@ public class RobotContainer {
|
||||
/* Autonomous */
|
||||
private final PathRecorder m_pathChooser = new PathRecorder(m_robotSwerveDrive);
|
||||
// Controllers
|
||||
private final static XboxController m_driverXbox = new DeadbandedXboxController(OIConstants.XBOX_DRIVER_ID);
|
||||
private final static XboxController m_operatorXbox = new DeadbandedXboxController(OIConstants.XBOX_OPERATOR_ID);
|
||||
private final static XboxController m_driverXbox = new DeadbandedRawXboxController(OIConstants.XBOX_DRIVER_ID);
|
||||
private final static XboxController m_operatorXbox = new DeadbandedRawXboxController(OIConstants.XBOX_OPERATOR_ID);
|
||||
private final ButtonBox m_buttonBox = new ButtonBox(OIConstants.BUTTON_BOX_ID);
|
||||
|
||||
public static boolean softLimits = true;
|
||||
@@ -153,9 +171,11 @@ public class RobotContainer {
|
||||
if (RobotContainer.currentControlMode.equals(ControlMode.SHOOTER)) { m_robotClimber.setMotors(0.0); }
|
||||
if (RobotContainer.currentControlMode.equals(ControlMode.CLIMBER)) { m_robotClimber.setMotors(getOperatorController().getRightY()); }
|
||||
}, m_robotClimber));
|
||||
|
||||
m_robotBoomBoom.setDefaultCommand(
|
||||
new RunCommand(() -> m_robotBoomBoom.runDrumShooter(0.45), m_robotBoomBoom)
|
||||
);
|
||||
|
||||
// autoInit();
|
||||
// recordInit();
|
||||
}
|
||||
|
||||
/**
|
||||
@@ -165,104 +185,117 @@ public class RobotContainer {
|
||||
* passing it to a {@link edu.wpi.first.wpilibj2.command.button.JoystickButton}.
|
||||
*/
|
||||
private void configureButtonBindings() {
|
||||
// Iterate over all Xbox controller buttons.
|
||||
for (XboxController.Button binding : XboxController.Button.values()) {
|
||||
/* ------------------------------------ Driver ------------------------------------ */
|
||||
JoystickButton button = new JoystickButton(getDriverController(), binding.value);
|
||||
if (binding == XboxController.Button.kLeftBumper)
|
||||
/* Left Bumper > Shift Down */ button.whenPressed(() -> m_robotSwerveDrive.highSpeed(false));
|
||||
else if (binding == XboxController.Button.kRightBumper)
|
||||
/* Right Bumper > Shift Up */ button.whenPressed(() -> m_robotSwerveDrive.highSpeed(false));
|
||||
else if (binding == XboxController.Button.kLeftStick)
|
||||
/* Left Stick > Unbound */ button.whenPressed(new PrintCommand("Unbound"));
|
||||
else if (binding == XboxController.Button.kRightStick)
|
||||
/* Right Stick > Unbound */ button.whenPressed(new PrintCommand("Unbound"));
|
||||
else if (binding == XboxController.Button.kA)
|
||||
/* A > Unbound */ button.whenPressed(new PrintCommand("Unbound"));
|
||||
else if (binding == XboxController.Button.kB)
|
||||
/* B > Unbound */ button.whenPressed(new PrintCommand("Unbound"));
|
||||
else if (binding == XboxController.Button.kX)
|
||||
/* X > Unbound */ button.whenPressed(new PrintCommand("Unbound"));
|
||||
else if (binding == XboxController.Button.kY)
|
||||
/* Y > Unbound */ button.whenPressed(new PrintCommand("Unbound"));
|
||||
else if (binding == XboxController.Button.kBack)
|
||||
/* Back > Calibrate Odometry */ button.whenPressed(() -> resetOdometry(new Pose2d(0, 0, new Rotation2d(0))));
|
||||
else if (binding == XboxController.Button.kStart)
|
||||
/* Start > Calibrate Odometry */ button.whenPressed(m_robotSwerveDrive::resetGyro);
|
||||
//! Driver Buttons
|
||||
|
||||
/* ------------------------------------ Operator ------------------------------------ */
|
||||
button = new JoystickButton(getDriverController(), binding.value);
|
||||
if (binding == XboxController.Button.kLeftBumper)
|
||||
/* Left Bumper > Storage In */
|
||||
button.whileHeld(new RunCommand(() -> m_robotStorage.runStorage(StorageConstants.STORAGE_SPEED)))
|
||||
.whenReleased(new RunCommand(() -> m_robotStorage.runStorage(0.0)));
|
||||
else if (binding == XboxController.Button.kRightBumper)
|
||||
/* Right Bumper > Storage Out */
|
||||
button.whileHeld(new RunCommand(() -> m_robotStorage.runStorage(-StorageConstants.STORAGE_SPEED)))
|
||||
.whenReleased(new RunCommand(() -> m_robotStorage.runStorage(0.0)));
|
||||
else if (binding == XboxController.Button.kLeftStick)
|
||||
/* Left Stick > Unbound */ button.whenPressed(new PrintCommand("Unbound"));
|
||||
else if (binding == XboxController.Button.kRightStick)
|
||||
/* Right Stick > Unbound */ button.whenPressed(new PrintCommand("Unbound"));
|
||||
else if (binding == XboxController.Button.kA)
|
||||
/* A > Spit Out Ball */
|
||||
button.whileHeld(new RunCommand(m_robotTurret::gotoMidpoint, m_robotTurret))
|
||||
.whileHeld(new RunCommand(() -> m_robotBoomBoom.runDrumShooter(0.25)));
|
||||
else if (binding == XboxController.Button.kB)
|
||||
/* B > Toggle Claws */ button.whenPressed(new InstantCommand(m_robotClaws::toggleClaws, m_robotClaws));
|
||||
else if (binding == XboxController.Button.kX)
|
||||
/* X > TEST BUTTON */ button.whileHeld(new TrackTarget(m_robotSwerveDrive, m_robotTurret, m_robotBoomBoom, m_robotHood, m_robotVisionOdometry));
|
||||
///* X > Toggles extender in and out */ button.whenPressed(new ExtenderIntakeGroup(m_robotIntake, m_robotExtender));
|
||||
else if (binding == XboxController.Button.kY)
|
||||
/* Y > TEST BUTTON */ button.whenPressed(new Shoot(m_robotSwerveDrive, m_robotBoomBoom, m_robotTurret, m_robotHood, m_robotVisionOdometry, false, false));
|
||||
///* Y > Full aim command */ button.whileHeld(new Seek(m_robotSwerveDrive, m_robotBoomBoom, m_robotTurret, m_robotHood, m_robotVisionOdometry));
|
||||
else if (binding == XboxController.Button.kBack)
|
||||
/* Back > Unbound */ button.whenPressed(new PrintCommand("Unbound"));
|
||||
else if (binding == XboxController.Button.kStart)
|
||||
/* Start > Unbound */ button.whenPressed(new PrintCommand("Unbound"));
|
||||
}
|
||||
// Iterate over all button box buttons.
|
||||
for (ButtonBox.Button binding : ButtonBox.Button.values()) {
|
||||
/* ---------------------------------- Button Box ---------------------------------- */
|
||||
JoystickButton button = new JoystickButton(getButtonBox(), binding.value);
|
||||
if (binding == ButtonBox.Button.kRightSwitch)
|
||||
/* Right Switch > Unbound */ button.whenPressed(new PrintCommand("Unbound"));
|
||||
else if (binding == ButtonBox.Button.kMiddleSwitch)
|
||||
/* Middle Switch > Climber and Shooter mode switching */
|
||||
button.whenPressed(new InstantCommand(() -> currentControlMode = ControlMode.CLIMBER))
|
||||
.whenReleased(new InstantCommand(() -> currentControlMode = ControlMode.SHOOTER));
|
||||
else if (binding == ButtonBox.Button.kLeftSwitch)
|
||||
/* Left Switch > Disables soft limits on press, release resets encoders (all for turret, hood, climber, and extender) */
|
||||
button.whenPressed(new InstantCommand(() -> m_robotTurret.setTurretSoftLimits(false), m_robotTurret))
|
||||
.whenPressed(new InstantCommand(() -> m_robotTurret.calibrationSpeed = 0.3, m_robotTurret))
|
||||
|
||||
.whenPressed(new InstantCommand(() -> m_robotHood.setHoodSoftLimits(false), m_robotHood))
|
||||
.whenPressed(new InstantCommand(() -> m_robotHood.calibrationSpeed = 0.3, m_robotHood))
|
||||
|
||||
.whenPressed(new InstantCommand(() -> m_robotExtender.setExtenderSoftLimits(false), m_robotExtender))
|
||||
|
||||
.whenReleased(new InstantCommand(() -> m_robotTurret.setTurretSoftLimits(true), m_robotTurret))
|
||||
.whenReleased(new InstantCommand(() -> m_robotTurret.calibrationSpeed = 1.0, m_robotTurret))
|
||||
|
||||
.whenReleased(new InstantCommand(() -> m_robotHood.setHoodSoftLimits(true), m_robotHood))
|
||||
.whenReleased(new InstantCommand(() -> m_robotHood.calibrationSpeed = 1.0, m_robotHood))
|
||||
// Start > Calibrate Odometry
|
||||
new JoystickButton(getDriverController(), XboxController.Button.kBack.value)
|
||||
.whenPressed(() -> resetOdometry(new Pose2d(0, 0, new Rotation2d(0))));
|
||||
|
||||
.whenReleased(new InstantCommand(() -> m_robotExtender.setExtenderSoftLimits(true), m_robotExtender))
|
||||
// Start > Calibrate Odometry
|
||||
new JoystickButton(getDriverController(), XboxController.Button.kStart.value)
|
||||
.whenPressed(m_robotSwerveDrive::resetGyro);
|
||||
|
||||
.whenReleased(new InstantCommand(() -> m_robotTurret.m_boomBoomRotateEncoder.setPosition(0), m_robotTurret))
|
||||
.whenReleased(new InstantCommand(() -> m_robotHood.m_angleEncoder.setPosition(0), m_robotHood))
|
||||
.whenReleased(new InstantCommand(() -> m_robotExtender.setEncoder(0), m_robotExtender))
|
||||
.whenReleased(new InstantCommand(() -> ExtenderIntakeGroup.setDirectionToOut(), m_robotIntake, m_robotExtender))
|
||||
.whenReleased(new InstantCommand(() -> m_robotClimber.setEncoders(0), m_robotClimber));
|
||||
else if (binding == ButtonBox.Button.kRightButton)
|
||||
/* Right Button > Extender Out */
|
||||
button.whileHeld(new RunCommand(() -> m_robotExtender.runExtender(1.0), m_robotExtender))
|
||||
.whenReleased(new RunCommand(() -> m_robotExtender.runExtender(0.0), m_robotExtender));
|
||||
else if (binding == ButtonBox.Button.kLeftButton)
|
||||
/* Left Button > Extender In */
|
||||
button.whileHeld(new RunCommand(() -> m_robotExtender.runExtender(-1.0), m_robotExtender))
|
||||
.whenReleased(new RunCommand(() -> m_robotExtender.runExtender(0.0), m_robotExtender));
|
||||
}
|
||||
// Left Bumper > Shift Down
|
||||
new JoystickButton(getDriverController(), XboxController.Button.kLeftBumper.value)
|
||||
.whenPressed(() -> m_robotSwerveDrive.highSpeed(false));
|
||||
|
||||
// Right Bumper > Shift Up
|
||||
new JoystickButton(getDriverController(), XboxController.Button.kRightBumper.value)
|
||||
.whenPressed(() -> m_robotSwerveDrive.highSpeed(true));
|
||||
|
||||
|
||||
|
||||
//! Operator Buttons
|
||||
|
||||
// Right Bumper > Storage Out
|
||||
new JoystickButton(getOperatorController(), XboxController.Button.kRightBumper.value)
|
||||
.whileHeld(new RunCommand(() -> m_robotStorage.runStorage(-StorageConstants.STORAGE_SPEED)))
|
||||
.whenReleased(new RunCommand(() -> m_robotStorage.runStorage(0.0)));
|
||||
|
||||
// Left Bumper > Storage In
|
||||
new JoystickButton(getOperatorController(), XboxController.Button.kLeftBumper.value)
|
||||
.whileHeld(new RunCommand(() -> m_robotStorage.runStorage(StorageConstants.STORAGE_SPEED)))
|
||||
.whenReleased(new RunCommand(() -> m_robotStorage.runStorage(0.0)));
|
||||
|
||||
// B > Toggle claws
|
||||
new JoystickButton(getOperatorController(), XboxController.Button.kB.value)
|
||||
.whenPressed(new InstantCommand(() -> m_robotClaws.toggleClaws(), m_robotClaws));
|
||||
|
||||
// X > Toggles extender in and out
|
||||
new JoystickButton(getOperatorController(), XboxController.Button.kX.value)
|
||||
.whenPressed(new ExtenderIntakeGroup(m_robotIntake, m_robotExtender));
|
||||
|
||||
// A > Spit Out Ball
|
||||
new JoystickButton(getOperatorController(), XboxController.Button.kA.value)
|
||||
.whileHeld(new RunCommand(() -> m_robotTurret.gotoMidpoint(), m_robotTurret))
|
||||
.whileHeld(new RunCommand(() -> m_robotBoomBoom.runDrumShooter(0.25)));
|
||||
|
||||
// Y > Full aim command
|
||||
// new JoystickButton(getOperatorController(), XboxController.Button.kY.value)
|
||||
// .whileHeld(new Seek(m_robotSwerveDrive, m_robotBoomBoom, m_robotTurret, m_robotHood, m_robotVisionOdometry));
|
||||
|
||||
|
||||
//! Test Buttons
|
||||
// new JoystickButton(getOperatorController(), XboxController.Button.kY.value)
|
||||
// .whenPressed(new Shoot(m_robotSwerveDrive, m_robotBoomBoom, m_robotTurret, m_robotHood, m_robotVisionOdometry, false, false));
|
||||
|
||||
new JoystickButton(getOperatorController(), XboxController.Button.kY.value)
|
||||
.whileHeld(new TrackTarget(m_robotTurret, m_robotBoomBoom, m_robotHood, m_robotVisionOdometry, false));
|
||||
|
||||
// new JoystickButton(getOperatorController(), XboxController.Button.kY.value)
|
||||
// .whileHeld(new RunCommand(() -> m_robotClaws.setOpen(true)));
|
||||
|
||||
// new JoystickButton(getOperatorController(), XboxController.Button.kB.value)
|
||||
// .whileHeld(new RunCommand(() -> m_robotClaws.setOpen(false)));
|
||||
|
||||
// new JoystickButton(getOperatorController(), XboxController.Button.kA.value)
|
||||
// .whenPressed(new Shoot(m_robotSwerveDrive, m_robotBoomBoom, m_robotTurret, m_robotHood));
|
||||
|
||||
// new JoystickButton(getOperatorController(), XboxController.Button.kA.value)
|
||||
// .whenPressed(new RunCommand(() -> m_robotBoomBoom.runDrumShooter(0.25)))
|
||||
// .whenReleased(new RunCommand(() -> m_robotBoomBoom.runDrumShooter(0.0)));
|
||||
|
||||
|
||||
|
||||
//! Button Box Buttons
|
||||
// Left Switch > Disables soft limits on press, release resets encoders (all for turret, hood, climber, and extender)
|
||||
new JoystickButton(getButtonBox(), ButtonBox.Button.kLeftSwitch.value)
|
||||
.whenPressed(new InstantCommand(() -> m_robotTurret.setTurretSoftLimits(false), m_robotTurret))
|
||||
.whenPressed(new InstantCommand(() -> m_robotTurret.calibrationSpeed = 0.3, m_robotTurret))
|
||||
|
||||
.whenPressed(new InstantCommand(() -> m_robotHood.setHoodSoftLimits(false), m_robotHood))
|
||||
.whenPressed(new InstantCommand(() -> m_robotHood.calibrationSpeed = 0.3, m_robotHood))
|
||||
|
||||
.whenPressed(new InstantCommand(() -> m_robotExtender.setExtenderSoftLimits(false), m_robotExtender))
|
||||
|
||||
.whenReleased(new InstantCommand(() -> m_robotTurret.setTurretSoftLimits(true), m_robotTurret))
|
||||
.whenReleased(new InstantCommand(() -> m_robotTurret.calibrationSpeed = 1.0, m_robotTurret))
|
||||
|
||||
.whenReleased(new InstantCommand(() -> m_robotHood.setHoodSoftLimits(true), m_robotHood))
|
||||
.whenReleased(new InstantCommand(() -> m_robotHood.calibrationSpeed = 1.0, m_robotHood))
|
||||
|
||||
.whenReleased(new InstantCommand(() -> m_robotExtender.setExtenderSoftLimits(true), m_robotExtender))
|
||||
|
||||
.whenReleased(new InstantCommand(() -> m_robotTurret.m_boomBoomRotateEncoder.setPosition(0), m_robotTurret))
|
||||
.whenReleased(new InstantCommand(() -> m_robotHood.m_angleEncoder.setPosition(0), m_robotHood))
|
||||
.whenReleased(new InstantCommand(() -> m_robotExtender.setEncoder(0), m_robotExtender))
|
||||
.whenReleased(new InstantCommand(() -> ExtenderIntakeGroup.setDirectionToOut(), m_robotIntake, m_robotExtender))
|
||||
.whenReleased(new InstantCommand(() -> m_robotClimber.setEncoders(0), m_robotClimber));
|
||||
|
||||
// Middle Switch > Climber and Shooter mode switching
|
||||
new JoystickButton(getButtonBox(), ButtonBox.Button.kMiddleSwitch.value)
|
||||
.whenPressed(new InstantCommand(() -> this.currentControlMode = ControlMode.CLIMBER))
|
||||
.whenReleased(new InstantCommand(() -> this.currentControlMode = ControlMode.SHOOTER));
|
||||
|
||||
// Left Button > Extender In
|
||||
new JoystickButton(getButtonBox(), ButtonBox.Button.kLeftButton.value)
|
||||
.whileHeld(new RunCommand(() -> m_robotExtender.runExtender(-1.0), m_robotExtender))
|
||||
.whenReleased(new RunCommand(() -> m_robotExtender.runExtender(0.0), m_robotExtender));
|
||||
|
||||
// Left Button > Extender Out
|
||||
new JoystickButton(getButtonBox(), ButtonBox.Button.kRightButton.value)
|
||||
.whileHeld(new RunCommand(() -> m_robotExtender.runExtender(1.0), m_robotExtender))
|
||||
.whenReleased(new RunCommand(() -> m_robotExtender.runExtender(0.0), m_robotExtender));
|
||||
}
|
||||
|
||||
/**
|
||||
@@ -288,8 +321,11 @@ public class RobotContainer {
|
||||
if (inputs[i] instanceof String) {
|
||||
|
||||
PathPlannerTrajectory traj = PathPlanner.loadPath(inputs[i].toString(), maxVel, maxAccel);
|
||||
PathPlannerState initState = (PathPlannerState) traj.sample(0);
|
||||
commands.add(new InstantCommand(() -> m_robotSwerveDrive.resetOdometry(new Pose2d(initState.poseMeters.getTranslation(), initState.holonomicRotation))));
|
||||
PathPlannerState initState = traj.getInitialState();
|
||||
|
||||
Pose2d initPose = new Pose2d(initState.poseMeters.getTranslation(), initState.holonomicRotation);
|
||||
|
||||
commands.add(new InstantCommand(() -> m_robotSwerveDrive.resetOdometry(initPose), m_robotSwerveDrive));
|
||||
commands.add(new PPSwerveControllerCommand(
|
||||
traj,
|
||||
m_robotSwerveDrive::getOdometry,
|
||||
@@ -300,12 +336,13 @@ public class RobotContainer {
|
||||
m_robotSwerveDrive::setModuleStates,
|
||||
m_robotSwerveDrive));
|
||||
}
|
||||
|
||||
if (inputs[i] instanceof Command) {
|
||||
commands.add((Command) inputs[i]);
|
||||
}
|
||||
}
|
||||
|
||||
commands.add(new InstantCommand(() -> m_robotSwerveDrive.stopModules()));
|
||||
commands.add(new InstantCommand(() -> m_robotSwerveDrive.driveWithInput(0, 0, 0, 0, true), m_robotSwerveDrive));
|
||||
Command[] ret = new Command[commands.size()];
|
||||
ret = commands.toArray(ret);
|
||||
return ret;
|
||||
@@ -346,8 +383,38 @@ public class RobotContainer {
|
||||
// * null,
|
||||
// * new SequentialCommandGroup(buildAuto(5.0, 5.0, "Path1", "Path2", "Path3")),
|
||||
// * new RunCommand(() -> m_robotIntake.runAtOutput(0.5))));
|
||||
|
||||
// return new SequentialCommandGroup(buildAuto(1.0, 1.0, new InstantCommand(() -> m_robotSwerveDrive.m_gyro.reset(), m_robotSwerveDrive),
|
||||
// // new InstantCommand(() -> this.resetOdometry(new Pose2d())),
|
||||
// new InstantCommand(() -> m_robotSwerveDrive.setModuleRotationsToAngle(0.0), m_robotSwerveDrive),
|
||||
// "Diamond"));
|
||||
|
||||
return new SequentialCommandGroup(buildAuto(1.0, 1.0, new InstantCommand(() -> m_robotSwerveDrive.m_gyro.reset()), "Move Forward")); // test command
|
||||
// * assume turret is already pointed towards target.
|
||||
// return new SequentialCommandGroup( new InstantCommand(() -> m_robotSwerveDrive.resetGyro(), m_robotSwerveDrive),
|
||||
// new DriveWithInputForTime(m_robotSwerveDrive, new double[] {0.5, 0.5, 0.0, 0.0}, 1.0),
|
||||
// new ParallelRaceGroup(
|
||||
// new TrackTarget(m_robotTurret, m_robotBoomBoom, m_robotHood, m_robotVisionOdometry, true),
|
||||
// new RunCommand(() -> m_robotStorage.runStorage(StorageConstants.STORAGE_SPEED), m_robotStorage)
|
||||
// ));
|
||||
|
||||
// return new RunCommandForTime(new RunCommand(() -> m_robotSwerveDrive.driveWithInput(0, 0, 0, true), m_robotSwerveDrive), 1.0, true);
|
||||
|
||||
return new SequentialCommandGroup( new InstantCommand(() -> m_robotSwerveDrive.resetGyro(), m_robotSwerveDrive),
|
||||
new DriveWithInputForTime(m_robotSwerveDrive, new double[] {0.0, 0.3, 0.0, 0.0}, 1.0));//,
|
||||
// new TrackTarget(m_robotTurret, m_robotBoomBoom, m_robotHood, m_robotVisionOdometry, true),
|
||||
// new ParallelCommandGroup(
|
||||
// new TrackTarget(m_robotTurret, m_robotBoomBoom, m_robotHood, m_robotVisionOdometry, true),
|
||||
// new RunCommandForTime(new RunCommand(() -> m_robotStorage.runStorage(StorageConstants.STORAGE_SPEED), m_robotStorage), 1.0)
|
||||
//));
|
||||
|
||||
// * aim with RotateUntilTarget
|
||||
// return new SequentialCommandGroup( new InstantCommand(() -> m_robotSwerveDrive.resetGyro(), m_robotSwerveDrive),
|
||||
// new DriveWithInputForTime(m_robotSwerveDrive, new double[] {0.5, 0.5, 0.0, 0.0}, 1.0),
|
||||
// new RotateUntilTarget(m_robotSwerveDrive, m_robotVisionOdometry, 0.5),
|
||||
// new ParallelRaceGroup(
|
||||
// new TrackTarget(m_robotTurret, m_robotBoomBoom, m_robotHood, m_robotVisionOdometry, true),
|
||||
// new RunCommand(() -> m_robotStorage.runStorage(StorageConstants.STORAGE_SPEED), m_robotStorage)
|
||||
// ));
|
||||
}
|
||||
|
||||
public static XboxController getDriverController() {
|
||||
|
||||
Reference in New Issue
Block a user