mirror of
https://github.com/Team4388/2022NoWayHome.git
synced 2026-06-09 00:38:05 -06:00
Merge branch 'testRoboReveal' into Climber
This commit is contained in:
Ryan Manley
@@ -29,12 +29,16 @@ import java.util.regex.Matcher;
|
||||
import java.util.regex.Pattern;
|
||||
import java.util.stream.Collectors;
|
||||
|
||||
import com.ctre.phoenix.motorcontrol.NeutralMode;
|
||||
import com.ctre.phoenix.motorcontrol.can.WPI_TalonFX;
|
||||
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 org.ejml.dense.row.decomposition.hessenberg.TridiagonalDecompositionHouseholderOrig_DDRM;
|
||||
|
||||
import edu.wpi.first.math.Pair;
|
||||
import edu.wpi.first.math.controller.PIDController;
|
||||
import edu.wpi.first.math.controller.ProfiledPIDController;
|
||||
@@ -51,6 +55,7 @@ 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.NotifierCommand;
|
||||
import edu.wpi.first.wpilibj2.command.ParallelCommandGroup;
|
||||
import edu.wpi.first.wpilibj2.command.RunCommand;
|
||||
import edu.wpi.first.wpilibj2.command.SequentialCommandGroup;
|
||||
import edu.wpi.first.wpilibj2.command.button.JoystickButton;
|
||||
@@ -62,7 +67,20 @@ import frc4388.robot.subsystems.Claws.ClawType;
|
||||
import frc4388.robot.commands.AimToCenter;
|
||||
import frc4388.robot.commands.Shoot;
|
||||
import frc4388.robot.commands.TrackTarget;
|
||||
import frc4388.robot.Constants.OIConstants;
|
||||
import frc4388.robot.Constants.ShooterConstants;
|
||||
import frc4388.robot.Constants.StorageConstants;
|
||||
import frc4388.robot.Constants.SwerveDriveConstants;
|
||||
import frc4388.robot.commands.ButtonBoxCommands.RunMiddleSwitch;
|
||||
// import frc4388.robot.commands.ButtonBoxCommands.TurretManual;
|
||||
import frc4388.robot.commands.ExtenderIntakeCommands.ExtenderIntakeGroup;
|
||||
import frc4388.robot.commands.ShooterCommands.AimToCenter;
|
||||
import frc4388.robot.commands.ShooterCommands.Shoot;
|
||||
import frc4388.robot.commands.ShooterCommands.TrackTarget;
|
||||
import frc4388.robot.commands.StorageCommands.ManageStorage;
|
||||
import frc4388.robot.subsystems.BoomBoom;
|
||||
import frc4388.robot.subsystems.Climber;
|
||||
import frc4388.robot.subsystems.Extender;
|
||||
import frc4388.robot.subsystems.Hood;
|
||||
import frc4388.robot.subsystems.Intake;
|
||||
import frc4388.robot.subsystems.LED;
|
||||
@@ -78,6 +96,7 @@ import frc4388.utility.ListeningSendableChooser;
|
||||
import frc4388.utility.PathPlannerUtil;
|
||||
import frc4388.utility.Vector2D;
|
||||
import frc4388.utility.PathPlannerUtil.Path.Waypoint;
|
||||
import frc4388.utility.controller.ButtonBox;
|
||||
import frc4388.utility.controller.DeadbandedXboxController;
|
||||
|
||||
/**
|
||||
@@ -89,32 +108,38 @@ import frc4388.utility.controller.DeadbandedXboxController;
|
||||
*/
|
||||
public class RobotContainer {
|
||||
private static final Logger LOGGER = Logger.getLogger(RobotContainer.class.getSimpleName());
|
||||
/* RobotMap */
|
||||
private final RobotMap m_robotMap = new RobotMap();
|
||||
|
||||
// RobotMap
|
||||
public final RobotMap m_robotMap = new RobotMap();
|
||||
|
||||
/* Subsystems */
|
||||
private final ClimberRewrite m_robotClimber = new ClimberRewrite(m_robotMap.shoulder, m_robotMap.elbow, m_robotMap.gyro, false);
|
||||
|
||||
private final Claws m_robotClaws = new Claws(m_robotMap.leftClaw, m_robotMap.rightClaw);
|
||||
|
||||
// public final SwerveDrive m_robotSwerveDrive = new SwerveDrive(m_robotMap.leftFront, m_robotMap.leftBack,
|
||||
// m_robotMap.rightFront, m_robotMap.rightBack, m_robotMap.gyro);
|
||||
// Subsystems
|
||||
public final SwerveDrive m_robotSwerveDrive = new SwerveDrive(m_robotMap.leftFront, m_robotMap.leftBack, m_robotMap.rightFront, m_robotMap.rightBack, m_robotMap.gyro);
|
||||
public final Serializer m_robotSerializer = new Serializer(m_robotMap.serializerBelt, /*m_robotMap.serializerShooterBelt,*/ m_robotMap.serializerBeam);
|
||||
public final Intake m_robotIntake = new Intake(m_robotMap.intakeMotor);
|
||||
public final Extender m_robotExtender = new Extender(m_robotMap.extenderMotor);
|
||||
|
||||
public final Storage m_robotStorage = new Storage(m_robotMap.storageMotor);
|
||||
// private final LED m_robotLED = new LED(m_robotMap.LEDController);
|
||||
public final BoomBoom m_robotBoomBoom = new BoomBoom(m_robotMap.shooterFalconLeft, m_robotMap.shooterFalconRight);
|
||||
public final Hood m_robotHood = new Hood(m_robotMap.angleAdjusterMotor);
|
||||
public final Turret m_robotTurret = new Turret(m_robotMap.shooterTurret);
|
||||
public final VisionOdometry m_robotVisionOdometry = new VisionOdometry(m_robotSwerveDrive, m_robotTurret);
|
||||
|
||||
// private final Serializer m_robotSerializer = new Serializer(m_robotMap.serializerBelt, /*m_robotMap.serializerShooterBelt,*/ m_robotMap.serializerBeam);
|
||||
// private final Intake m_robotIntake = new Intake(m_robotMap.intakeMotor, m_robotMap.extenderMotor, m_robotSerializer);
|
||||
// private final Storage m_robotStorage = new Storage(m_robotMap.storageMotor, m_robotMap.beamIntake, m_robotMap.beamShooter);
|
||||
// //private final LED m_robotLED = new LED(m_robotMap.LEDController);
|
||||
// private final BoomBoom m_robotBoomBoom = new BoomBoom(m_robotMap.shooterFalconLeft, m_robotMap.shooterFalconRight);
|
||||
// private final Hood m_robotHood = new Hood(m_robotMap.angleAdjusterMotor);
|
||||
// private final Turret m_robotTurret = new Turret(m_robotMap.shooterTurret);
|
||||
// private final VisionOdometry m_robotVisionOdometry = new VisionOdometry(m_robotSwerveDrive, m_robotTurret);
|
||||
|
||||
|
||||
/* Controllers */
|
||||
private final WPI_TalonFX testShoulderMotor = new WPI_TalonFX(30);
|
||||
private final WPI_TalonFX testElbowMotor = new WPI_TalonFX(31);
|
||||
//public final Climber m_robotClimber = new Climber(testShoulderMotor, testElbowMotor);
|
||||
|
||||
// Controllers
|
||||
private final XboxController m_driverXbox = new DeadbandedXboxController(OIConstants.XBOX_DRIVER_ID);
|
||||
private final XboxController m_operatorXbox = new DeadbandedXboxController(OIConstants.XBOX_OPERATOR_ID);
|
||||
private final ButtonBox m_buttonBox = new ButtonBox(OIConstants.BUTTON_BOX_ID);
|
||||
|
||||
/* Autonomous */
|
||||
// Autonomous
|
||||
private PathPlannerTrajectory loadedPathTrajectory = null;
|
||||
// private final ListeningSendableChooser<File> autoChooser = new ListeningSendableChooser<>(this::loadPath);
|
||||
private final List<Waypoint> pathPoints = new ArrayList<>();
|
||||
@@ -128,6 +153,8 @@ public class RobotContainer {
|
||||
// Function that removes the ".path" from the end of a string.
|
||||
private static final Function<CharSequence, String> PATH_EXTENSION_REMOVER = ((Function<CharSequence, Matcher>) Pattern
|
||||
.compile(".path")::matcher).andThen(m -> m.replaceFirst(""));
|
||||
|
||||
public static boolean softLimits = true;
|
||||
|
||||
/**
|
||||
* The container for the robot. Contains subsystems, OI devices, and commands.
|
||||
@@ -147,39 +174,52 @@ public class RobotContainer {
|
||||
// new RunCommand(() -> m_robotClimber.controlWithInput(getOperatorController().getLeftX(),
|
||||
// getOperatorController().getLeftY()), m_robotClimber).withName("Climber controlWithInput defaultCommand"));
|
||||
|
||||
// Turret default command%
|
||||
//m_robotTurret.setDefaultCommand(new AimToCenter(m_robotTurret, m_robotSwerveDrive, m_robotVisionOdometry));
|
||||
// m_robotTurret.setDefaultCommand(new RunCommand(() -> m_robotTurret.runTurretWithInput(getOperatorController().getLeftX()), m_robotTurret));
|
||||
|
||||
// //Swerve Drive
|
||||
// m_robotSwerveDrive.setDefaultCommand(
|
||||
// new RunCommand(() -> m_robotSwerveDrive.driveWithInput(
|
||||
// getDriverController().getLeftX(),
|
||||
// getDriverController().getLeftY(),
|
||||
// getDriverController().getRightX(),
|
||||
// getDriverController().getRightY(),
|
||||
// true),
|
||||
// m_robotSwerveDrive).withName("Swerve driveWithInput defaultCommand"));
|
||||
// //Intake with Triggers
|
||||
// m_robotIntake.setDefaultCommand(
|
||||
// new RunCommand(() -> m_robotIntake.runWithTriggers(
|
||||
// getOperatorController().getLeftTriggerAxis(),
|
||||
// getOperatorController().getRightTriggerAxis()),
|
||||
// m_robotIntake).withName("Intake runWithTriggers defaultCommand"));
|
||||
// //Storage Management
|
||||
// Swerve Drive with Input
|
||||
m_robotSwerveDrive.setDefaultCommand(
|
||||
new RunCommand(() -> m_robotSwerveDrive.driveWithInput(
|
||||
getDriverController().getLeftX(),
|
||||
getDriverController().getLeftY(),
|
||||
//getDriverController().getRightX(),
|
||||
getDriverController().getRightX(),
|
||||
getDriverController().getRightY(),
|
||||
true),
|
||||
m_robotSwerveDrive).withName("Swerve driveWithInput defaultCommand"));
|
||||
// Intake with Triggers
|
||||
m_robotIntake.setDefaultCommand(
|
||||
new RunCommand(() -> m_robotIntake.runWithTriggers(
|
||||
getOperatorController().getLeftTriggerAxis(),
|
||||
getOperatorController().getRightTriggerAxis()),
|
||||
m_robotIntake).withName("Intake runWithTriggers defaultCommand"));
|
||||
|
||||
// m_robotStorage.setDefaultCommand(
|
||||
// new RunCommand(() -> m_robotStorage.manageStorage(),
|
||||
// m_robotStorage).withName("Storage manageStorage defaultCommand"));
|
||||
// //Serializer Management
|
||||
// m_robotSerializer.setDefaultCommand(
|
||||
// new RunCommand(() -> m_robotSerializer.setSerializerStateWithBeam(),
|
||||
// m_robotSerializer).withName("Serializer setSerializerStateWithBeam defaultCommand"));
|
||||
// new ManageStorage(m_robotStorage,
|
||||
// m_robotBoomBoom,
|
||||
// m_robotTurret).withName("Storage ManageStorage defaultCommand"));
|
||||
|
||||
// m_robotClimber.setDefaultCommand(
|
||||
// new RunCommand(() -> m_robotClimber.runBothMotorsWithInputs(getDriverController().getLeftY() * 0.1, getDriverController().getRightY() * 0.1), m_robotClimber)
|
||||
// );
|
||||
// Storage Management
|
||||
/*m_robotStorage.setDefaultCommand(
|
||||
new RunCommand(() -> m_robotStorage.manageStorage(),
|
||||
m_robotStorage).withName("Storage manageStorage defaultCommand"));*/
|
||||
// Serializer Management
|
||||
m_robotSerializer.setDefaultCommand(
|
||||
new RunCommand(() -> m_robotSerializer.setSerializer(getOperatorController().getLeftTriggerAxis() * 0.8),//m_robotSerializer.setSerializerStateWithBeam(),
|
||||
m_robotSerializer).withName("Serializer setSerializerStateWithBeam defaultCommand"));
|
||||
// Turret Manual
|
||||
m_robotTurret.setDefaultCommand(
|
||||
new RunCommand(() -> m_robotTurret.runTurretWithInput(getOperatorController().getLeftX()),
|
||||
m_robotTurret).withName("Turret runTurretWithInput defaultCommand"));
|
||||
|
||||
m_robotHood.setDefaultCommand(
|
||||
new RunCommand(() -> m_robotHood.runHood(getOperatorController().getRightY()), m_robotHood));
|
||||
// m_robotTurret.setDefaultCommand(
|
||||
// new AimToCenter(m_robotTurret, m_robotSwerveDrive, m_robotVisionOdometry));
|
||||
|
||||
// continually sends updates to the Blinkin LED controller to keep the lights on
|
||||
// m_robotLED
|
||||
// .setDefaultCommand(new RunCommand(m_robotLED::updateLED,
|
||||
// m_robotLED).withName("LED update defaultCommand"));
|
||||
// m_robotLED.setDefaultCommand(new RunCommand(m_robotLED::updateLED, m_robotLED).withName("LED update defaultCommand"));
|
||||
|
||||
// autoInit();
|
||||
// recordInit();
|
||||
}
|
||||
@@ -193,88 +233,112 @@ public class RobotContainer {
|
||||
private void configureButtonBindings() {
|
||||
|
||||
/* Driver Buttons */
|
||||
// "XboxController.Button.kBack" was undefined yet, 7 works just fine
|
||||
// new JoystickButton(getDriverController(), XboxController.Button.kBack.value)
|
||||
// .whenPressed(m_robotSwerveDrive::resetGyro);
|
||||
|
||||
// new JoystickButton(getDriverController(), XboxController.Button.kLeftBumper.value)
|
||||
// // new XboxControllerRawButton(m_driverXbox,
|
||||
// // XboxControllerRaw.LEFT_BUMPER_BUTTON)
|
||||
// .whenPressed(() -> m_robotSwerveDrive.highSpeed(false));
|
||||
// Start > Calibrate Odometry
|
||||
new JoystickButton(getDriverController(), XboxController.Button.kBack.value)
|
||||
.whenPressed(() -> resetOdometry(new Pose2d(0, 0, new Rotation2d(0))));
|
||||
// Start > Calibrate Odometry
|
||||
new JoystickButton(getDriverController(), XboxController.Button.kStart.value)
|
||||
.whenPressed(m_robotSwerveDrive::resetGyro);
|
||||
// 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));
|
||||
|
||||
|
||||
// new JoystickButton(getDriverController(), XboxController.Button.kRightBumper.value)
|
||||
// // new XboxControllerRawButton(m_driverXbox,
|
||||
// // XboxControllerRaw.RIGHT_BUMPER_BUTTON)
|
||||
// .whenPressed(() -> m_robotSwerveDrive.highSpeed(true));
|
||||
|
||||
// new JoystickButton(getDriverController(), XboxController.Button.kA.value)
|
||||
// .whenPressed(() -> resetOdometry(new Pose2d(0, 0, new Rotation2d(0))));
|
||||
|
||||
// new JoystickButton(getDriverController(), XboxController.Button.kX.value) //Temp
|
||||
// .whenPressed(() -> m_robotMap.leftFront.reset())
|
||||
// .whenPressed(() -> m_robotMap.rightFront.reset())
|
||||
// .whenPressed(() -> m_robotMap.leftBack.reset())
|
||||
// .whenPressed(() -> m_robotMap.rightBack.reset());
|
||||
|
||||
/* Operator Buttons */
|
||||
// run claws
|
||||
// new JoystickButton(getOperatorController(), XboxController.Button.kY.value)
|
||||
// .whileHeld(new RunCommand(() -> m_robotClaws.runClaw(ClawType.LEFT, 0.2)))
|
||||
// .whenReleased(new RunCommand(() -> m_robotClaws.runClaw(ClawType.LEFT, 0.0)));
|
||||
|
||||
// new JoystickButton(getOperatorController(), XboxController.Button.kX.value)
|
||||
// .whileHeld(new RunCommand(() -> m_robotClaws.runClaw(ClawType.LEFT, -0.2)))
|
||||
// .whenReleased(new RunCommand(() -> m_robotClaws.runClaw(ClawType.LEFT, 0.0)));
|
||||
|
||||
// new JoystickButton(getOperatorController(), XboxController.Button.kB.value)
|
||||
// .whileHeld(new RunCommand(() -> m_robotClaws.runClaw(ClawType.RIGHT, 0.2)))
|
||||
// .whenReleased(new RunCommand(() -> m_robotClaws.runClaw(ClawType.RIGHT, 0.0)));
|
||||
|
||||
// new JoystickButton(getOperatorController(), XboxController.Button.kA.value)
|
||||
// .whileHeld(new RunCommand(() -> m_robotClaws.runClaw(ClawType.RIGHT, -0.2)))
|
||||
// .whenReleased(new RunCommand(() -> m_robotClaws.runClaw(ClawType.RIGHT, 0.0)));
|
||||
|
||||
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.kY.value)
|
||||
// .whenPressed(new InstantCommand(() -> m_robotVisionOdometry.setLEDs(false), m_robotVisionOdometry));
|
||||
|
||||
|
||||
|
||||
|
||||
new JoystickButton(getOperatorController(), XboxController.Button.kLeftBumper.value)
|
||||
.whenPressed(new RunCommand(() -> m_robotStorage.runStorage(0.9), m_robotStorage))
|
||||
.whenReleased(new RunCommand(() -> m_robotStorage.runStorage(0.0), m_robotStorage));
|
||||
|
||||
new JoystickButton(getOperatorController(), XboxController.Button.kRightBumper.value)
|
||||
.whileHeld(new RunCommand(() -> m_robotStorage.runStorage(-0.9), m_robotStorage))
|
||||
.whenReleased(new RunCommand(() -> m_robotStorage.runStorage(0.0), m_robotStorage));
|
||||
|
||||
//Toggles extender in and out
|
||||
new JoystickButton(getOperatorController(), XboxController.Button.kY.value)
|
||||
.whenPressed(new ExtenderIntakeGroup(m_robotIntake, m_robotExtender));
|
||||
|
||||
new JoystickButton(getOperatorController(), XboxController.Button.kA.value)
|
||||
.whileHeld(new RunCommand(() -> m_robotTurret.gotoZero(), m_robotTurret))
|
||||
.whenReleased(new RunCommand(() -> m_robotTurret.m_boomBoomRotateMotor.set(0), m_robotTurret));
|
||||
|
||||
// Right Bumper > Storage In
|
||||
// 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 Out (note: neccessary?)
|
||||
// new JoystickButton(getOperatorController(), XboxController.Button.kLeftBumper.value)
|
||||
// .whileHeld(new RunCommand(() -> m_robotStorage.runStorage(-StorageConstants.STORAGE_SPEED)))
|
||||
// .whenReleased(new RunCommand(() -> m_robotStorage.runStorage(0.0)));
|
||||
|
||||
// A > Shoot with Odo
|
||||
/*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)
|
||||
// .whileHeld(new RunCommand(() -> m_robotTurret.gotoMidpoint(), m_robotTurret));
|
||||
|
||||
//B > Shoot with Lime
|
||||
new JoystickButton(getOperatorController(), XboxController.Button.kB.value)
|
||||
.whileHeld(new TrackTarget(m_robotTurret, m_robotBoomBoom, m_robotHood, m_robotVisionOdometry))
|
||||
.whenReleased(new InstantCommand(() -> m_robotVisionOdometry.setLEDs(false)))
|
||||
.whenReleased(new RunCommand(() -> m_robotBoomBoom.runDrumShooterVelocityPID(0)));
|
||||
// .whileHeld%
|
||||
|
||||
|
||||
/* Button Box Buttons */
|
||||
|
||||
new JoystickButton(getButtonBox(), ButtonBox.Button.kLeftSwitch.value)
|
||||
.whenPressed(new InstantCommand(() -> m_robotTurret.setTurretSoftLimits(false), m_robotTurret))
|
||||
.whenPressed(new InstantCommand(() -> m_robotHood.setHoodSoftLimits(false), m_robotHood))
|
||||
.whenPressed(new InstantCommand(() -> m_robotExtender.setExtenderSoftLimits(false), m_robotExtender))
|
||||
|
||||
.whenReleased(new InstantCommand(() -> m_robotTurret.setTurretSoftLimits(true), m_robotTurret))
|
||||
.whenReleased(new InstantCommand(() -> m_robotHood.setHoodSoftLimits(true), 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));
|
||||
|
||||
// new JoystickButton(getButtonBox(), ButtonBox.Button.kMiddleSwitch.value)
|
||||
// .whileHeld(new TurretManual(m_robotTurret));
|
||||
|
||||
// control turret manual mode
|
||||
// new JoystickButton(getButtonBox(), ButtonBox.Button.kRightSwitch.value)
|
||||
// .whileHeld(new RunCommand(() -> TurretManual.setManual(true)))
|
||||
// .whenReleased(new RunCommand(() -> TurretManual.setManual(false)));
|
||||
|
||||
|
||||
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));
|
||||
|
||||
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));
|
||||
}
|
||||
|
||||
/*
|
||||
* // activates "BoomBoom"
|
||||
* new JoystickButton(getOperatorController(), XboxController.Button.kA.value)
|
||||
* .whenPressed(new Shoot(m_robotSwerveDrive, m_robotBoomBoom, m_robotTurret,
|
||||
* m_robotHood));
|
||||
*/
|
||||
|
||||
//Extender
|
||||
// new JoystickButton(getOperatorController(), XboxController.Button.kX.value)
|
||||
// .whenPressed(() -> m_robotIntake.runExtender(true));
|
||||
|
||||
// new JoystickButton(getOperatorController(), XboxController.Button.kY.value)
|
||||
// .whenPressed(() -> m_robotIntake.runExtender(false));
|
||||
|
||||
|
||||
|
||||
// //Storage
|
||||
// new JoystickButton(getOperatorController(), XboxController.Button.kRightBumper.value)
|
||||
// .whenPressed(() -> m_robotStorage.runStorage(StorageConstants.STORAGE_SPEED))
|
||||
// .whenReleased(() -> m_robotStorage.runStorage(0.0));
|
||||
|
||||
|
||||
// new JoystickButton(getOperatorController(), XboxController.Button.kLeftBumper.value)
|
||||
// .whenPressed(() -> m_robotStorage.runStorage(-StorageConstants.STORAGE_SPEED))
|
||||
// .whenReleased(() -> m_robotStorage.runStorage(0.0));
|
||||
|
||||
// //Shooter
|
||||
// new JoystickButton(getOperatorController(), XboxController.Button.kA.value)
|
||||
// .whenPressed(new Shoot(m_robotSwerveDrive, m_robotBoomBoom, m_robotTurret, m_robotHood));
|
||||
|
||||
// new JoystickButton(getOperatorController(), XboxController.Button.kB.value)
|
||||
// .whenPressed(new TrackTarget(m_robotTurret, m_robotBoomBoom, m_robotHood, m_robotSwerveDrive, m_robotVisionOdometry));
|
||||
// }
|
||||
|
||||
/**
|
||||
* Use this to pass the autonomous command to the main {@link Robot} class.
|
||||
*
|
||||
@@ -308,6 +372,18 @@ public class RobotContainer {
|
||||
return m_driverXbox;
|
||||
}
|
||||
|
||||
public XboxController getOperatorController() {
|
||||
return m_operatorXbox;
|
||||
}
|
||||
|
||||
public ButtonBox getButtonBox() {
|
||||
return m_buttonBox;
|
||||
}
|
||||
|
||||
public static void setSoftLimits(boolean set) {
|
||||
softLimits = set;
|
||||
}
|
||||
|
||||
/**
|
||||
* Get odometry.
|
||||
*
|
||||
@@ -326,10 +402,6 @@ public class RobotContainer {
|
||||
// m_robotSwerveDrive.resetOdometry(pose);
|
||||
// }
|
||||
|
||||
public XboxController getOperatorController() {
|
||||
return m_operatorXbox;
|
||||
}
|
||||
|
||||
/**
|
||||
* Creates a WatchKey for the path planner directory and registers it with the
|
||||
* WatchService.
|
||||
@@ -362,144 +434,145 @@ public class RobotContainer {
|
||||
* Creates a button on the SmartDashboard that will record the path of the
|
||||
* robot.
|
||||
*/
|
||||
// public void recordInit() {
|
||||
// SmartDashboard.putData("Recording",
|
||||
// new RunCommand(this::recordPeriodic) {
|
||||
// @Override
|
||||
// public void end(boolean interupted) {
|
||||
// new InstantCommand(RobotContainer.this::saveRecording) {
|
||||
// @Override
|
||||
// public boolean runsWhenDisabled() {
|
||||
// return true;
|
||||
// }
|
||||
// }.withName("Save Recording").schedule();
|
||||
// }
|
||||
// }.withName("Record Path (Cancel to Save)"));
|
||||
// }
|
||||
|
||||
// /**
|
||||
// * Called when a file is created, modified, or deleted.
|
||||
// * Adds newly created .path files to the SendableChooser.
|
||||
// * Reloads the path if the currently selected file is modified.
|
||||
// *
|
||||
// * @param watchKey The WatchKey that is being observed.
|
||||
// */
|
||||
// private void updateAutoChooser(WatchKey watchKey) {
|
||||
// List<WatchEvent<?>> watchEvents = watchKey.pollEvents();
|
||||
// if (!watchEvents.isEmpty()) {
|
||||
// List<WatchEvent<?>> pathWatchEvents = watchEvents.stream()
|
||||
// .filter(e -> e.kind().type().isAssignableFrom(Path.class)).collect(Collectors.toList());
|
||||
// for (WatchEvent<?> pathWatchEvent : pathWatchEvents) {
|
||||
// Path watchEventPath = (Path) pathWatchEvent.context();
|
||||
// File watchEventFile = watchEventPath.toFile();
|
||||
// String watchEventFileName = watchEventFile.getName();
|
||||
// if (watchEventFileName.endsWith(".path")) {
|
||||
// if (pathWatchEvent.kind().equals(StandardWatchEventKinds.ENTRY_CREATE)) {
|
||||
// LOGGER.log(Level.WARNING, "PathPlanner file {0} created. Options added to SendableChooser.",
|
||||
// watchEventFileName);
|
||||
// autoChooser.addOption(watchEventFile.getName(), watchEventFile);
|
||||
// } else if (pathWatchEvent.kind().equals(StandardWatchEventKinds.ENTRY_MODIFY)) {
|
||||
// LOGGER.log(Level.WARNING, "PathPlanner file {0} modified.", watchEventFileName);
|
||||
// if (watchEventFileName.equals(autoChooser.getSelected().getName())) {
|
||||
// LOGGER.log(Level.SEVERE, "PathPlanner file {0} already selected. Reloading path.", watchEventFileName);
|
||||
// loadPath(watchEventFileName);
|
||||
// }
|
||||
// } else if (pathWatchEvent.kind().equals(StandardWatchEventKinds.ENTRY_DELETE)) {
|
||||
// LOGGER.log(Level.SEVERE,
|
||||
// "PathPlanner file {0} deleted. Removing options from SendableChooser not yet implemented.",
|
||||
// watchEventFileName);
|
||||
// }
|
||||
// }
|
||||
// }
|
||||
// }
|
||||
// if (!watchKey.reset())
|
||||
// LOGGER.severe("File watch key invalid.");
|
||||
// }
|
||||
public void recordInit() {
|
||||
SmartDashboard.putData("Recording",
|
||||
new RunCommand(this::recordPeriodic) {
|
||||
@Override
|
||||
public void end(boolean interupted) {
|
||||
new InstantCommand(RobotContainer.this::saveRecording) {
|
||||
@Override
|
||||
public boolean runsWhenDisabled() {
|
||||
return true;
|
||||
}
|
||||
}.withName("Save Recording").schedule();
|
||||
}
|
||||
}.withName("Record Path (Cancel to Save)"));
|
||||
}
|
||||
|
||||
// private void loadPath(String pathName) {
|
||||
// LOGGER.warning("Loading path " + pathName);
|
||||
// loadedPathTrajectory = null;
|
||||
// loadedPathTrajectory = PathPlanner.loadPath(PATH_EXTENSION_REMOVER.apply(Objects.requireNonNullElse(pathName, "")),
|
||||
// SwerveDriveConstants.PATH_MAX_VELOCITY, SwerveDriveConstants.PATH_MAX_ACCELERATION);
|
||||
// LOGGER.info("Done loading");
|
||||
// }
|
||||
/**
|
||||
* Called when a file is created, modified, or deleted.
|
||||
* Adds newly created .path files to the SendableChooser.
|
||||
* Reloads the path if the currently selected file is modified.
|
||||
*
|
||||
* @param watchKey The WatchKey that is being observed.
|
||||
*/
|
||||
private void updateAutoChooser(WatchKey watchKey) {
|
||||
List<WatchEvent<?>> watchEvents = watchKey.pollEvents();
|
||||
if (!watchEvents.isEmpty()) {
|
||||
List<WatchEvent<?>> pathWatchEvents = watchEvents.stream()
|
||||
.filter(e -> e.kind().type().isAssignableFrom(Path.class)).collect(Collectors.toList());
|
||||
for (WatchEvent<?> pathWatchEvent : pathWatchEvents) {
|
||||
Path watchEventPath = (Path) pathWatchEvent.context();
|
||||
File watchEventFile = watchEventPath.toFile();
|
||||
String watchEventFileName = watchEventFile.getName();
|
||||
if (watchEventFileName.endsWith(".path")) {
|
||||
if (pathWatchEvent.kind().equals(StandardWatchEventKinds.ENTRY_CREATE)) {
|
||||
LOGGER.log(Level.WARNING, "PathPlanner file {0} created. Options added to SendableChooser.",
|
||||
watchEventFileName);
|
||||
autoChooser.addOption(watchEventFile.getName(), watchEventFile);
|
||||
} else if (pathWatchEvent.kind().equals(StandardWatchEventKinds.ENTRY_MODIFY)) {
|
||||
LOGGER.log(Level.WARNING, "PathPlanner file {0} modified.", watchEventFileName);
|
||||
if (watchEventFileName.equals(autoChooser.getSelected().getName())) {
|
||||
LOGGER.log(Level.SEVERE, "PathPlanner file {0} already selected. Reloading path.", watchEventFileName);
|
||||
loadPath(watchEventFileName);
|
||||
}
|
||||
} else if (pathWatchEvent.kind().equals(StandardWatchEventKinds.ENTRY_DELETE)) {
|
||||
LOGGER.log(Level.SEVERE,
|
||||
"PathPlanner file {0} deleted. Removing options from SendableChooser not yet implemented.",
|
||||
watchEventFileName);
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
if (!watchKey.reset())
|
||||
LOGGER.severe("File watch key invalid.");
|
||||
}
|
||||
|
||||
// private void saveRecording() {
|
||||
// // IMPORTANT: Had to chown the pathplanner folder in order to save autos.
|
||||
// File outputFile = PATHPLANNER_DIRECTORY
|
||||
// .resolve(ZonedDateTime.now(SYSTEM_CLOCK).format(RECORDING_FILE_NAME_FORMATTER)).toFile();
|
||||
// LOGGER.log(Level.WARNING, "Creating path {0}.", outputFile.getPath());
|
||||
// if (!pathPoints.isEmpty() && Boolean.TRUE.equals(Errors.log().getWithDefault(outputFile::createNewFile, false))) {
|
||||
// // TODO: Change to use measured maximum velocity and acceleration.
|
||||
// var path = createPath(null, null, false);
|
||||
// if (RobotBase.isReal())
|
||||
// path.write(outputFile);
|
||||
// StringWriter writer = new StringWriter();
|
||||
// path.write(writer);
|
||||
// recordingNetworkTable.getEntry(outputFile.getName()).setString(writer.toString());
|
||||
// autoChooser.setDefaultOption(outputFile.getName(), outputFile);
|
||||
// LOGGER.log(Level.INFO, "Recorded path to {0}.", outputFile.getPath());
|
||||
// } else
|
||||
// LOGGER.log(Level.SEVERE, "Unable to record path to {0}", outputFile.getPath());
|
||||
// }
|
||||
private void loadPath(String pathName) {
|
||||
LOGGER.warning("Loading path " + pathName);
|
||||
loadedPathTrajectory = null;
|
||||
loadedPathTrajectory = PathPlanner.loadPath(PATH_EXTENSION_REMOVER.apply(Objects.requireNonNullElse(pathName, "")),
|
||||
SwerveDriveConstants.PATH_MAX_VELOCITY, SwerveDriveConstants.PATH_MAX_ACCELERATION);
|
||||
LOGGER.info("Done loading");
|
||||
}
|
||||
|
||||
// // public void recordPeriodic() {
|
||||
// // Translation2d position = m_robotSwerveDrive.m_poseEstimator.getEstimatedPosition().getTranslation();
|
||||
// // Rotation2d rotation = m_robotSwerveDrive.m_gyro.getRotation2d();
|
||||
// // // FIXME: Chassis speeds are created from joystick inputs and do not reflect
|
||||
// // // actual robot velocity.
|
||||
// // Translation2d velocity = new Translation2d(m_robotSwerveDrive.chassisSpeeds.vxMetersPerSecond,
|
||||
// // m_robotSwerveDrive.chassisSpeeds.vyMetersPerSecond);
|
||||
// // Waypoint waypoint = new Waypoint(position, position, position, rotation.getDegrees(), false,
|
||||
// // SwerveDriveConstants.PATH_RECORD_VELOCITY ? velocity.getNorm() : null, false);
|
||||
// // pathPoints.add(waypoint);
|
||||
// // }
|
||||
private void saveRecording() {
|
||||
// IMPORTANT: Had to chown the pathplanner folder in order to save autos.
|
||||
File outputFile = PATHPLANNER_DIRECTORY
|
||||
.resolve(ZonedDateTime.now(SYSTEM_CLOCK).format(RECORDING_FILE_NAME_FORMATTER)).toFile();
|
||||
LOGGER.log(Level.WARNING, "Creating path {0}.", outputFile.getPath());
|
||||
if (!pathPoints.isEmpty() && Boolean.TRUE.equals(Errors.log().getWithDefault(outputFile::createNewFile, false))) {
|
||||
// TODO: Change to use measured maximum velocity and acceleration.
|
||||
var path = createPath(null, null, false);
|
||||
if (RobotBase.isReal())
|
||||
path.write(outputFile);
|
||||
StringWriter writer = new StringWriter();
|
||||
path.write(writer);
|
||||
recordingNetworkTable.getEntry(outputFile.getName()).setString(writer.toString());
|
||||
autoChooser.setDefaultOption(outputFile.getName(), outputFile);
|
||||
LOGGER.log(Level.INFO, "Recorded path to {0}.", outputFile.getPath());
|
||||
} else
|
||||
LOGGER.log(Level.SEVERE, "Unable to record path to {0}", outputFile.getPath());
|
||||
}
|
||||
|
||||
// public PathPlannerUtil.Path createPath(Double maxVelocity, Double maxAcceleration, Boolean isReversed) {
|
||||
// // Remove points whose angles to neighboring points are less than 10 degrees
|
||||
// // apart.
|
||||
// int j = 0;
|
||||
// for (int i = 1; i < pathPoints.size() - 1; i++) {
|
||||
// var prev = pathPoints.get(j).anchorPoint.orElseThrow();
|
||||
// var current = pathPoints.get(i).anchorPoint.orElseThrow();
|
||||
// var next = pathPoints.get(i + 1).anchorPoint.orElseThrow();
|
||||
// var fromPrevious = current.minus(prev);
|
||||
// var toNext = next.minus(current);
|
||||
// var angleFromPrevious = new Rotation2d(fromPrevious.getX(), fromPrevious.getY());
|
||||
// var angleToNext = new Rotation2d(toNext.getX(), toNext.getY());
|
||||
// if (Math.abs(angleFromPrevious.minus(angleToNext).getDegrees()) < SwerveDriveConstants.MIN_WAYPOINT_ANGLE
|
||||
// || (next.getDistance(prev) < SwerveDriveConstants.MIN_WAYPOINT_DISTANCE
|
||||
// && pathPoints.get(i).velOverride.map(v -> v < SwerveDriveConstants.MIN_WAYPOINT_VELOCITY).orElse(false)))
|
||||
// pathPoints.set(i, null);
|
||||
// else
|
||||
// j = i;
|
||||
// }
|
||||
// pathPoints.removeIf(Objects::isNull);
|
||||
// // Make control points
|
||||
// pathPoints.get(0).nextControl = Optional.of(makeControlPoints(null, pathPoints.get(0).anchorPoint.orElseThrow(),
|
||||
// pathPoints.get(1).anchorPoint.orElseThrow()).getSecond());
|
||||
// for (int i = 1; i < pathPoints.size() - 1; i++) {
|
||||
// var controls = makeControlPoints(pathPoints.get(i - 1).anchorPoint.orElseThrow(),
|
||||
// pathPoints.get(i).anchorPoint.orElseThrow(), pathPoints.get(i + 1).anchorPoint.orElseThrow());
|
||||
// pathPoints.get(i).prevControl = Optional.of(controls.getFirst());
|
||||
// pathPoints.get(i).nextControl = Optional.of(controls.getSecond());
|
||||
// }
|
||||
// pathPoints.get(pathPoints.size() - 1).prevControl = Optional
|
||||
// .of(makeControlPoints(pathPoints.get(pathPoints.size() - 2).anchorPoint.orElseThrow(),
|
||||
// pathPoints.get(pathPoints.size() - 1).anchorPoint.orElseThrow(), null).getFirst());
|
||||
// // Create the path
|
||||
// PathPlannerUtil.Path path = new PathPlannerUtil.Path();
|
||||
// path.waypoints = Optional.ofNullable(pathPoints.toArray(PathPlannerUtil.Path.Waypoint[]::new));
|
||||
// path.maxVelocity = Optional.ofNullable(maxVelocity);
|
||||
// path.maxAcceleration = Optional.ofNullable(maxAcceleration);
|
||||
// path.isReversed = Optional.ofNullable(isReversed);
|
||||
// pathPoints.clear();
|
||||
// return path;
|
||||
// }
|
||||
public void recordPeriodic() {
|
||||
Translation2d position = m_robotSwerveDrive.m_poseEstimator.getEstimatedPosition().getTranslation();
|
||||
Rotation2d rotation = m_robotSwerveDrive.m_gyro.getRotation2d();
|
||||
// FIXME: Chassis speeds are created from joystick inputs and do not reflect
|
||||
// actual robot velocity.
|
||||
Translation2d velocity = new Translation2d(m_robotSwerveDrive.getChassisSpeeds()[0],
|
||||
m_robotSwerveDrive.getChassisSpeeds()[1]);
|
||||
Waypoint waypoint = new Waypoint(position, position, position, rotation.getDegrees(), false,
|
||||
SwerveDriveConstants.PATH_RECORD_VELOCITY ? velocity.getNorm() : null, false);
|
||||
pathPoints.add(waypoint);
|
||||
}
|
||||
|
||||
// private static Pair<Translation2d, Translation2d> makeControlPoints(Translation2d prev, Translation2d current,
|
||||
// Translation2d next) {
|
||||
// var line = Objects.requireNonNullElse(next, current).minus(Objects.requireNonNullElse(prev, current)).div(4);
|
||||
// return Pair.of(current.minus(line), current.plus(line));
|
||||
// }
|
||||
public PathPlannerUtil.Path createPath(Double maxVelocity, Double maxAcceleration, Boolean isReversed) {
|
||||
// Remove points whose angles to neighboring points are less than 10 degrees
|
||||
// apart.
|
||||
int j = 0;
|
||||
for (int i = 1; i < pathPoints.size() - 1; i++) {
|
||||
var prev = pathPoints.get(j).anchorPoint.orElseThrow();
|
||||
var current = pathPoints.get(i).anchorPoint.orElseThrow();
|
||||
var next = pathPoints.get(i + 1).anchorPoint.orElseThrow();
|
||||
var fromPrevious = current.minus(prev);
|
||||
var toNext = next.minus(current);
|
||||
var angleFromPrevious = new Rotation2d(fromPrevious.getX(), fromPrevious.getY());
|
||||
var angleToNext = new Rotation2d(toNext.getX(), toNext.getY());
|
||||
if (Math.abs(angleFromPrevious.minus(angleToNext).getDegrees()) < SwerveDriveConstants.MIN_WAYPOINT_ANGLE
|
||||
|| (next.getDistance(prev) < SwerveDriveConstants.MIN_WAYPOINT_DISTANCE
|
||||
&& pathPoints.get(i).velOverride.map(v -> v < SwerveDriveConstants.MIN_WAYPOINT_VELOCITY).orElse(false)))
|
||||
pathPoints.set(i, null);
|
||||
else
|
||||
j = i;
|
||||
}
|
||||
pathPoints.removeIf(Objects::isNull);
|
||||
// Make control points
|
||||
pathPoints.get(0).nextControl = Optional.of(makeControlPoints(null, pathPoints.get(0).anchorPoint.orElseThrow(),
|
||||
pathPoints.get(1).anchorPoint.orElseThrow()).getSecond());
|
||||
for (int i = 1; i < pathPoints.size() - 1; i++) {
|
||||
var controls = makeControlPoints(pathPoints.get(i - 1).anchorPoint.orElseThrow(),
|
||||
pathPoints.get(i).anchorPoint.orElseThrow(), pathPoints.get(i + 1).anchorPoint.orElseThrow());
|
||||
pathPoints.get(i).prevControl = Optional.of(controls.getFirst());
|
||||
pathPoints.get(i).nextControl = Optional.of(controls.getSecond());
|
||||
}
|
||||
pathPoints.get(pathPoints.size() - 1).prevControl = Optional
|
||||
.of(makeControlPoints(pathPoints.get(pathPoints.size() - 2).anchorPoint.orElseThrow(),
|
||||
pathPoints.get(pathPoints.size() - 1).anchorPoint.orElseThrow(), null).getFirst());
|
||||
// Create the path
|
||||
PathPlannerUtil.Path path = new PathPlannerUtil.Path();
|
||||
path.waypoints = Optional.ofNullable(pathPoints.toArray(PathPlannerUtil.Path.Waypoint[]::new));
|
||||
path.maxVelocity = Optional.ofNullable(maxVelocity);
|
||||
path.maxAcceleration = Optional.ofNullable(maxAcceleration);
|
||||
path.isReversed = Optional.ofNullable(isReversed);
|
||||
pathPoints.clear();
|
||||
return path;
|
||||
}
|
||||
|
||||
private static Pair<Translation2d, Translation2d> makeControlPoints(Translation2d prev, Translation2d current,
|
||||
Translation2d next) {
|
||||
var line = Objects.requireNonNullElse(next, current).minus(Objects.requireNonNullElse(prev, current)).div(4);
|
||||
return Pair.of(current.minus(line), current.plus(line));
|
||||
}
|
||||
}
|
||||
|
||||
Reference in New Issue
Block a user