diff --git a/src/main/java/frc4388/robot/RobotContainer.java b/src/main/java/frc4388/robot/RobotContainer.java index dd47223..dd76e20 100644 --- a/src/main/java/frc4388/robot/RobotContainer.java +++ b/src/main/java/frc4388/robot/RobotContainer.java @@ -30,18 +30,18 @@ import edu.wpi.first.wpilibj2.command.WaitCommand; import edu.wpi.first.wpilibj2.command.WaitUntilCommand; import edu.wpi.first.wpilibj2.command.button.JoystickButton; import edu.wpi.first.wpilibj2.command.button.Trigger; +import frc4388.robot.commands.Swerve.StayInPosition; import frc4388.robot.commands.alignment.AutoAlign; import frc4388.robot.constants.Constants; import frc4388.robot.constants.Constants.OIConstants; import frc4388.robot.constants.Constants.SimConstants.Mode; -// Subsystems -import frc4388.robot.subsystems.LED; -import frc4388.robot.subsystems.Lidar; import frc4388.robot.subsystems.intake.Intake; import frc4388.robot.subsystems.intake.Intake.IntakeMode; +import frc4388.robot.subsystems.led.LED; import frc4388.robot.subsystems.shooter.Shooter; import frc4388.robot.subsystems.shooter.ShooterConstants; import frc4388.robot.subsystems.swerve.SwerveDrive; +import frc4388.robot.subsystems.vision.Lidar; import frc4388.robot.subsystems.vision.Vision; import frc4388.utility.DeferredBlock; import frc4388.utility.compute.FieldPositions; @@ -84,148 +84,159 @@ public class RobotContainer { // private final ButtonBox m_buttonBox = new ButtonBox(OIConstants.BUTTONBOX_ID); // public List 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 autoChooser; - private Command autoCommand; - - - private Command IntakeExtended = new SequentialCommandGroup( - new InstantCommand(() -> m_robotIntake.setMode(IntakeMode.Extended), m_robotIntake) - ); - - // private Command LidarIntake = new SequentialCommandGroup( - // // Right now this will just go to the closest ball constantly updating - need to make it so it locks on one ball - // // RobotIntakeDown, - // // new WaitCommand(1), - // // new InstantCommand(() -> System.out.println("Closest Ball: " + m_lidar.getClosestBall())), - // new AutoAlign(m_robotSwerveDrive, m_vision, m_lidar, true) - - // // new RunCommand( - // // () -> m_robotSwerveDrive.driveWithInput( - // // m_lidar.getClosestBall(), - // // new Translation2d(m_lidar.getLatestBallAngle().getCos() * 0.1, 0), - // // false - // // ), - // // m_robotSwerveDrive - // // ) - // // .withTimeout(5.0) - // // .andThen(new InstantCommand(() -> m_robotSwerveDrive.softStop(), m_robotSwerveDrive)) - // ); - - private Command RobotRev = new SequentialCommandGroup( - new InstantCommand(() -> m_robotShooter.spinUpShooting(), m_robotShooter), - IntakeExtended, - new InstantCommand(() -> m_robotIntake.setMode(IntakeMode.RollerStop), m_robotIntake) - ); - - private Command IntakeRetracted = new SequentialCommandGroup( - new InstantCommand(() -> m_robotIntake.setMode(IntakeMode.Retracted), m_robotIntake) - ); - - private Command RobotShoot = new SequentialCommandGroup( - // TEST NEW AUTO ALIGN - //new AutoAlign(m_robotSwerveDrive, m_vision, new Pose2d(FieldPositions.HUB_POSITION, new Rotation2d(0)), false), - new WaitUntilCommand(m_robotShooter::isShooterUpToSpeed), - new InstantCommand(()-> m_robotShooter.allowShooting(), m_robotShooter), - new WaitCommand(2), - IntakeRetracted, - new WaitCommand(5), - new InstantCommand(() -> m_robotShooter.denyShooting(), m_robotShooter), - new InstantCommand(()->m_robotShooter.spinUpIdle(), m_robotShooter) - ); - + private final StayInPosition m_stayInPosition = new StayInPosition(m_robotSwerveDrive); - public RobotContainer() { - - configureButtonBindings(); - - // Called on first robot enable - DeferredBlock.addBlock(() -> { - m_robotSwerveDrive.resetGyro(); - }, false); - - // Called on every robot enable - DeferredBlock.addBlock(() -> { - TimesNegativeOne.update(); - FieldPositions.update(); - - m_robotIntake.io.updateGains(); - m_robotShooter.io.updateGains(); - }, true); - - NamedCommands.registerCommand("Robot Rev Up", RobotRev); - NamedCommands.registerCommand("Intake Retracted", IntakeRetracted); - NamedCommands.registerCommand("Robot Shoot", RobotShoot); - // NamedCommands.registerCommand("Lidar Intake", LidarIntake); - NamedCommands.registerCommand("Intake Extended", IntakeExtended); - - - DriverStation.silenceJoystickConnectionWarning(true); - - // Drive normally - m_robotSwerveDrive.setDefaultCommand(new RunCommand(() -> { - m_robotSwerveDrive.driveWithInput( - getDeadbandedDriverController().getLeft(), - getDeadbandedDriverController().getRight(),true); - - }, m_robotSwerveDrive) - .withName("SwerveDrive DefaultCommand")); - - m_robotSwerveDrive.setToSlow(); - - makeAutoChooser(); - SmartDashboard.putData("Auto Chooser", autoChooser); - - } + private Pose2d currentPose = new Pose2d(0, 0, new Rotation2d()); + // ! Teleop Commands + public void stop() { + new InstantCommand(()->{}, m_robotSwerveDrive).schedule(); + m_robotSwerveDrive.stopModules(); + Constants.AutoConstants.Y_OFFSET_TRIM.set(0); + } - - - - /** - * Use this method to define your button->command mappings. Buttons can be - * created by instantiating a {@link GenericHID} or one of its subclasses - * ({@link edu.wpi.first.wpilibj.Joystick} or {@link XboxController}), and then - * passing it to a {@link edu.wpi.first.wpilibj2.command.button.JoystickButton}. - */ - private void configureButtonBindings() { - - //Driver controls - new JoystickButton(getDeadbandedDriverController(), XboxController.A_BUTTON) - .onTrue(new InstantCommand(() -> m_robotSwerveDrive.resetGyro())); - - new JoystickButton(getDeadbandedDriverController(), XboxController.RIGHT_BUMPER_BUTTON) - .onTrue(new InstantCommand(() -> m_robotSwerveDrive.shiftUp())); - - new JoystickButton(getDeadbandedDriverController(), XboxController.LEFT_BUMPER_BUTTON) - .onTrue(new InstantCommand(() -> m_robotSwerveDrive.shiftDown())); - + // ! /* Autos */ + private SendableChooser autoChooser; + private Command autoCommand; + + + private Command IntakeExtended = new SequentialCommandGroup( + new InstantCommand(() -> m_robotIntake.setMode(IntakeMode.Extended), m_robotIntake) + ); + + // private Command LidarIntake = new SequentialCommandGroup( + // // Right now this will just go to the closest ball constantly updating - need to make it so it locks on one ball + // // RobotIntakeDown, + // // new WaitCommand(1), + // // new InstantCommand(() -> System.out.println("Closest Ball: " + m_lidar.getClosestBall())), + // new AutoAlign(m_robotSwerveDrive, m_vision, m_lidar, true) + + // // new RunCommand( + // // () -> m_robotSwerveDrive.driveWithInput( + // // m_lidar.getClosestBall(), + // // new Translation2d(m_lidar.getLatestBallAngle().getCos() * 0.1, 0), + // // false + // // ), + // // m_robotSwerveDrive + // // ) + // // .withTimeout(5.0) + // // .andThen(new InstantCommand(() -> m_robotSwerveDrive.softStop(), m_robotSwerveDrive)) + // ); + + private Command RobotRev = new SequentialCommandGroup( + new InstantCommand(() -> m_robotShooter.spinUpShooting(), m_robotShooter), + IntakeExtended, + new InstantCommand(() -> m_robotIntake.setMode(IntakeMode.RollerStop), m_robotIntake) + ); + + private Command IntakeRetracted = new SequentialCommandGroup( + new InstantCommand(() -> m_robotIntake.setMode(IntakeMode.Retracted), m_robotIntake) + ); + + private Command RobotShoot = new SequentialCommandGroup( + // TEST NEW AUTO ALIGN + //new AutoAlign(m_robotSwerveDrive, m_vision, new Pose2d(FieldPositions.HUB_POSITION, new Rotation2d(0)), false), + new WaitUntilCommand(m_robotShooter::isShooterUpToSpeed), + new InstantCommand(()-> m_robotShooter.allowShooting(), m_robotShooter), + new WaitCommand(2), + IntakeRetracted, + new WaitCommand(5), + new InstantCommand(() -> m_robotShooter.denyShooting(), m_robotShooter), + new InstantCommand(()->m_robotShooter.spinUpIdle(), m_robotShooter) + ); - new JoystickButton(getDeadbandedDriverController(), XboxController.BACK_BUTTON) - .onTrue(new InstantCommand(() -> { + public RobotContainer() { + + configureButtonBindings(); + + // Called on first robot enable + DeferredBlock.addBlock(() -> { + m_robotSwerveDrive.resetGyro(); + }, false); + + // Called on every robot enable + DeferredBlock.addBlock(() -> { + TimesNegativeOne.update(); + FieldPositions.update(); + m_robotIntake.io.updateGains(); m_robotShooter.io.updateGains(); - })); - + }, true); + + NamedCommands.registerCommand("Robot Rev Up", RobotRev); + NamedCommands.registerCommand("Intake Retracted", IntakeRetracted); + NamedCommands.registerCommand("Robot Shoot", RobotShoot); + // NamedCommands.registerCommand("Lidar Intake", LidarIntake); + NamedCommands.registerCommand("Intake Extended", IntakeExtended); + + + DriverStation.silenceJoystickConnectionWarning(true); + + // Drive normally + m_robotSwerveDrive.setDefaultCommand(new RunCommand(() -> { + m_robotSwerveDrive.driveWithInput( + getDeadbandedDriverController().getLeft(), + getDeadbandedDriverController().getRight(),true); + + }, m_robotSwerveDrive) + .withName("SwerveDrive DefaultCommand")); - // IF the driver is holding the left trigger, intake driving - new Trigger(() -> getDeadbandedDriverController().getLeftTriggerAxis() >= 0.5) - .whileTrue(new RunCommand( - () -> { - m_robotSwerveDrive.driveIntakeOrientation( - getDeadbandedDriverController().getLeft(), - getDeadbandedDriverController().getRight() - - ); - }, m_robotSwerveDrive)) + m_robotSwerveDrive.setToSlow(); + + makeAutoChooser(); + SmartDashboard.putData("Auto Chooser", autoChooser); + + } + + + + + /** + * Use this method to define your button->command mappings. Buttons can be + * created by instantiating a {@link GenericHID} or one of its subclasses + * ({@link edu.wpi.first.wpilibj.Joystick} or {@link XboxController}), and then + * passing it to a {@link edu.wpi.first.wpilibj2.command.button.JoystickButton}. + */ + private void configureButtonBindings() { + + //Driver controls + new JoystickButton(getDeadbandedDriverController(), XboxController.A_BUTTON) + .onTrue(new InstantCommand(() -> m_robotSwerveDrive.resetGyro())); + + new JoystickButton(getDeadbandedDriverController(), XboxController.RIGHT_BUMPER_BUTTON) + .onTrue(new InstantCommand(() -> m_robotSwerveDrive.shiftUp())); + + new JoystickButton(getDeadbandedDriverController(), XboxController.LEFT_BUMPER_BUTTON) + .onTrue(new InstantCommand(() -> m_robotSwerveDrive.shiftDown())); + + + + new JoystickButton(getDeadbandedDriverController(), XboxController.BACK_BUTTON) + .onTrue(new InstantCommand(() -> { + m_robotIntake.io.updateGains(); + m_robotShooter.io.updateGains(); + })); + + + // TEST-> the driver is holding the left trigger, drive slow and rotation up + new Trigger(() -> getDeadbandedDriverController().getLeftTriggerAxis() >= 0.5) + .onTrue(new InstantCommand(() -> { + m_robotSwerveDrive.setToSlow(); + m_robotSwerveDrive.shiftUpRot(); + })) + .onFalse(new InstantCommand(() -> { + m_robotSwerveDrive.setToFast(); + m_robotSwerveDrive.shiftDownRot(); + })); + + //TEST - > Swerve drive pid on position + new JoystickButton(getDeadbandedDriverController(), XboxController.X_BUTTON) + .onTrue(new InstantCommand(() -> { + currentPose = m_robotSwerveDrive.getCurrentPose(); + })) + .whileTrue(new RunCommand(() -> { + m_stayInPosition.goToTargetPose(currentPose); + }, m_robotSwerveDrive)) .onFalse(new InstantCommand(() -> { m_robotSwerveDrive.softStop(); })); diff --git a/src/main/java/frc4388/robot/commands/MoveForTimeCommand.java b/src/main/java/frc4388/robot/commands/MoveForTimeCommand.java deleted file mode 100644 index 3a4e043..0000000 --- a/src/main/java/frc4388/robot/commands/MoveForTimeCommand.java +++ /dev/null @@ -1,49 +0,0 @@ -package frc4388.robot.commands; - -import java.time.Instant; - -import edu.wpi.first.math.geometry.Translation2d; -import edu.wpi.first.wpilibj2.command.Command; -import frc4388.robot.subsystems.swerve.SwerveDrive; - -// Command to repeat a joystick movement for a specific time. -public class MoveForTimeCommand extends Command { - private final SwerveDrive swerveDrive; - private final Translation2d leftStick; - private final Translation2d rightStick; - private final long duration; - private final boolean robotRelative; - - private Instant startTime; - - public MoveForTimeCommand( - SwerveDrive swerveDrive, - Translation2d leftStick, - Translation2d rightStick, - long millis, - boolean robotRelative) { - - addRequirements(swerveDrive); - - this.swerveDrive = swerveDrive; - this.leftStick = leftStick; - this.rightStick = rightStick; - this.duration = millis; - this.robotRelative = robotRelative; - } - - @Override - public void initialize() { - startTime = Instant.now(); - } - - @Override - public void execute() { - swerveDrive.driveWithInput(leftStick, rightStick, !robotRelative); - } - - @Override - public boolean isFinished() { - return Math.abs(startTime.toEpochMilli() - Instant.now().toEpochMilli()) > duration; - } -} \ No newline at end of file diff --git a/src/main/java/frc4388/robot/commands/MoveUntilSuply.java b/src/main/java/frc4388/robot/commands/MoveUntilSuply.java deleted file mode 100644 index c5b5e53..0000000 --- a/src/main/java/frc4388/robot/commands/MoveUntilSuply.java +++ /dev/null @@ -1,45 +0,0 @@ -package frc4388.robot.commands; - -import java.util.function.Supplier; - -import edu.wpi.first.math.geometry.Translation2d; -import edu.wpi.first.wpilibj2.command.Command; -import frc4388.robot.subsystems.swerve.SwerveDrive; - -// Command to repeat a joystick movement for a specific time. -public class MoveUntilSuply extends Command { - private final SwerveDrive swerveDrive; - private final Translation2d leftStick; - private final Translation2d rightStick; - private final Supplier truth; - private final boolean robotRelative; - - public MoveUntilSuply( - SwerveDrive swerveDrive, - Translation2d leftStick, - Translation2d rightStick, - Supplier truth, - boolean robotRelative) { - addRequirements(swerveDrive); - - this.swerveDrive = swerveDrive; - this.leftStick = leftStick; - this.rightStick = rightStick; - this.truth = truth; - this.robotRelative = robotRelative; - } - - @Override - public void initialize() { - } - - @Override - public void execute() { - swerveDrive.driveWithInput(leftStick, rightStick, !robotRelative); - } - - @Override - public boolean isFinished() { - return truth.get(); - } -} \ No newline at end of file diff --git a/src/main/java/frc4388/robot/commands/Swerve/RotateToAngle.java b/src/main/java/frc4388/robot/commands/Swerve/RotateToAngle.java deleted file mode 100644 index a3610d1..0000000 --- a/src/main/java/frc4388/robot/commands/Swerve/RotateToAngle.java +++ /dev/null @@ -1,35 +0,0 @@ -// Copyright (c) FIRST and other WPILib contributors. -// Open Source Software; you can modify and/or share it under the terms of -// the WPILib BSD license file in the root directory of this project. - -package frc4388.robot.commands.Swerve; - -import edu.wpi.first.math.geometry.Translation2d; -import frc4388.robot.commands.PID; -import frc4388.robot.subsystems.swerve.SwerveDrive; - -public class RotateToAngle extends PID { - - SwerveDrive drive; - double targetAngle; - - /** Creates a new RotateToAngle. */ - public RotateToAngle(SwerveDrive drive, double targetAngle) { - super(0.3, 0.0, 0.0, 0.0, 1); - - this.drive = drive; - this.targetAngle = targetAngle; - - addRequirements(drive); - } - - @Override - public double getError() { - return targetAngle - drive.getGyroAngle(); - } - - @Override - public void runWithOutput(double output) { - drive.driveWithInput(new Translation2d(0.0, 0.0), new Translation2d(output / Math.abs(getError()), 0.0), true); - } -} diff --git a/src/main/java/frc4388/robot/commands/Swerve/StayInPosition.java b/src/main/java/frc4388/robot/commands/Swerve/StayInPosition.java new file mode 100644 index 0000000..8d9be18 --- /dev/null +++ b/src/main/java/frc4388/robot/commands/Swerve/StayInPosition.java @@ -0,0 +1,56 @@ +// Copyright (c) FIRST and other WPILib contributors. +// Open Source Software; you can modify and/or share it under the terms of +// the WPILib BSD license file in the root directory of this project. + +package frc4388.robot.commands.Swerve; + +import edu.wpi.first.math.geometry.Pose2d; +import edu.wpi.first.math.geometry.Rotation2d; +import edu.wpi.first.math.geometry.Translation2d; +import frc4388.robot.commands.PID; +import frc4388.robot.subsystems.swerve.SwerveDrive; + +public class StayInPosition extends PID { + + SwerveDrive drive; + + /** Creates a new StayInPosition. */ + public StayInPosition(SwerveDrive drive) { + super(0.3, 0.0, 0.0, 0.0, 1); + + this.drive = drive; + + addRequirements(drive); + } + + public void goToTargetPose(Pose2d targetPose){ + Pose2d currentPose = drive.getCurrentPose(); + double translationX = targetPose.getX() - currentPose.getX(); + double translationY = targetPose.getY() - currentPose.getY(); + Rotation2d deltaRotation = targetPose.getRotation().minus(currentPose.getRotation()); + Translation2d driveTranslation = new Translation2d(translationX, translationY); + + drive.driveFieldAngle(driveTranslation, deltaRotation); + } + + @Override + public double getError() { + return 0; + // return targetAngle - drive.getGyroAngle(); + } + + @Override + public void runWithOutput(double output) { + // drive.driveWithInput(new Pose2d(new Translation2d(0.0, 0.0), new Rotation2d(output / Math.abs(getError())))); + } + + // @Override + // public boolean isFinished() { + // Rotation2d curRot = m_SwerveDrive.getPose2d().getRotation(); + // double ballAngleDeg = m_lidar.getLatestBallAngleDegrees(); + + // // TODO: Tune + // return Math.abs(curRot.getDegrees() +ballAngleDeg) < 5; + // } + +} diff --git a/src/main/java/frc4388/robot/commands/Swerve/neoJoystickPlayback.java b/src/main/java/frc4388/robot/commands/Swerve/neoJoystickPlayback.java deleted file mode 100644 index ba3c6ce..0000000 --- a/src/main/java/frc4388/robot/commands/Swerve/neoJoystickPlayback.java +++ /dev/null @@ -1,197 +0,0 @@ -package frc4388.robot.commands.Swerve; - -import java.io.FileInputStream; -import java.util.ArrayList; -import java.util.function.Supplier; - -import edu.wpi.first.math.geometry.Translation2d; -import edu.wpi.first.wpilibj2.command.Command; -import frc4388.robot.subsystems.swerve.SwerveDrive; -import frc4388.utility.compute.DataUtils; -import frc4388.utility.controller.VirtualController; -import frc4388.utility.structs.UtilityStructs.AutoRecordingControllerFrame; -import frc4388.utility.structs.UtilityStructs.AutoRecordingFrame; - - -/** - * The NEO autonomus playback system, designed based the old {@link JoystickPlayback} System but with {@link VirtualController}s - * @author Zachary Wilke - */ -public class neoJoystickPlayback extends Command { - private final SwerveDrive swerve; - private final VirtualController[] controllers; - private final ArrayList frames = new ArrayList<>(); - private final Supplier filenameGetter; - private String filename; - private int frame_index = 0; - // private long startTime = 0; - // private long playbackTime = 0; - private boolean m_finished = false; // ! There is no better way. - private boolean m_shouldfree = false; // should free memory on ending - - private byte m_numAxes = 0; - private byte m_numPOVs = 0; - private byte m_numControllers = 0; - private short m_numFrames = -1; - - /** - * Creates an new NEO Joystick Playback with specifyed pramiters. - * @param swerve m_robotSwerveDrive - * @param filenameGetter a String Supplier, designed for quickly changing auto names in shuffle board. - * @param controllers an Order-Specific Array of Virtual controllers, index 0 means driver, index 1 means operator, etc. - * @param shouldfree Unloads the auto on compleation or intruption. - * @param instantload Load the auto on object instantiation - */ - public neoJoystickPlayback(SwerveDrive swerve, Supplier filenameGetter, VirtualController[] controllers, boolean shouldfree, boolean instantload) { - this.swerve = swerve; - this.filenameGetter = filenameGetter; - this.controllers = controllers; - this.m_shouldfree = shouldfree; - - if (instantload) loadAuto(); - addRequirements(this.swerve); - } - - /** - * Creates an new NEO Joystick Playback with specifyed pramiters. - * @param swerve m_robotSwerveDrive - * @param filename a String containing the name of the auto file you wish to playback. - * @param controllers an Order-Specific Array of Virtual controllers, index 0 means driver, index 1 means operator, etc. - * @param shouldfree unloads the auto on compleation or intruption. - * @param instantload load the auto on object instantiation - */ - public neoJoystickPlayback(SwerveDrive swerve, String filename, VirtualController[] controllers, boolean shouldfree, boolean instantload) { - this(swerve, () -> filename, controllers, shouldfree, instantload); - } - - /** - * Creates an new NEO Joystick Playback with specifyed pramiters. - * @param swerve m_robotSwerveDrive - * @param filenameGetter a String Supplier, designed for quickly changing auto names in shuffle board. - * @param controllers an Order-Specific Array of Virtual controllers, index 0 means driver, index 1 means operator, etc. - */ - public neoJoystickPlayback(SwerveDrive swerve, Supplier filenameGetter, VirtualController[] controllers) { - this(swerve, filenameGetter, controllers, true, false); - } - - /** - * Creates an new NEO Joystick Playback with specifyed pramiters. - * @param swerve m_robotSwerveDrive - * @param filename a String containing the name of the auto file you wish to playback. - * @param controllers an Order-Specific Array of Virtual controllers, index 0 means driver, index 1 means operator, etc. - */ - public neoJoystickPlayback(SwerveDrive swerve, String filename, VirtualController[] controllers) { - this(swerve, () -> filename, controllers, true, false); - } - - /** - * Load the auto file from disk into memory - * @return Returns true if loading was successful, else wise; return false - * @implNote if the auto is already loaded, it will return true. - */ - public boolean loadAuto() { - filename = filenameGetter.get(); - try (FileInputStream stream = new FileInputStream("/home/lvuser/autos/" + filename)) { - if (m_numFrames != -1 && m_numFrames == frames.size()) { - System.out.println("AUTOPLAYBACK: Auto Already loaded."); - return true; - } - - m_numAxes = stream.readNBytes(1)[0]; - m_numPOVs = stream.readNBytes(1)[0]; - m_numControllers = stream.readNBytes(1)[0]; - m_numFrames = DataUtils.byteArrayToShort(stream.readNBytes(2)); - - if (m_numControllers > controllers.length) { - System.out.println("AUTOPLAYBACK: The auto file `" + filename + "` wants " + m_numControllers - + " virtual controllers but only " + controllers.length + " were given"); - return false; - } - - for (int i = 0; i < m_numFrames; i++) { - AutoRecordingFrame frame = new AutoRecordingFrame(); - for (int j = 0; j < m_numControllers; j++) { - AutoRecordingControllerFrame controllerFrame = new AutoRecordingControllerFrame(); - double[] axes = new double[m_numAxes]; - for (int k = 0; k < m_numAxes; k++) { // we love third level for loops. - axes[k] = DataUtils.byteArrayToDouble(stream.readNBytes(8)); - } - short button = DataUtils.byteArrayToShort(stream.readNBytes(2)); - short[] POV = new short[m_numPOVs]; - for (int k = 0; k < m_numPOVs; k++) { - POV[k] = DataUtils.byteArrayToShort(stream.readNBytes(2)); - } - controllerFrame.axes = axes; - controllerFrame.button = button; - controllerFrame.POV = POV; - frame.controllerFrames[j] = controllerFrame; - } - frame.timeStamp = DataUtils.byteArrayToInt(stream.readNBytes(4)); - frames.add(frame); - } - - System.out.println("AUTOPLAYBACK: Read Auto `" + filename + "` that is " + m_numFrames + " frames long"); - return true; - - } catch (Exception e) { - e.printStackTrace(); - System.out.println("AUTOPLAYBACK: Unable to read auto file `" + filename + '`'); - return false; - } - } - - /** - * Unloads the auto. - */ - public void unloadAuto() { - System.out.println("AUTOPLAYBACK: Auto unloaded"); - frames.clear(); - } - - @Override - public void initialize() { - // startTime = System.currentTimeMillis(); - // playbackTime = 0; - frame_index = 0; - - m_finished = !loadAuto(); - } - - @Override - public void execute() { - if (frame_index >= m_numFrames) m_finished = true; - if (m_finished) return; - - // if (frame_index == 0) { - // startTime = System.currentTimeMillis(); - // playbackTime = 0; - // } else { - // playbackTime = System.currentTimeMillis() - startTime; - // } - - AutoRecordingFrame frame = frames.get(frame_index); - for (int i = 0; i < controllers.length; i++) { - AutoRecordingControllerFrame controllerFrame = frame.controllerFrames[i]; - controllers[i].setFrame(controllerFrame.axes, controllerFrame.button, controllerFrame.POV); - if (i == 0) { - this.swerve.driveWithInput( - new Translation2d(controllers[i].getRawAxis(0), controllers[i].getRawAxis(1)), - new Translation2d(controllers[i].getRawAxis(4), controllers[i].getRawAxis(5)), - true); - } - } - frame_index++; - } - - @Override - public void end(boolean interrupted) { - for (VirtualController controller : controllers) controller.zeroControls(); - swerve.stopModules(); - if (m_shouldfree) unloadAuto(); - } - - @Override - public boolean isFinished() { - return m_finished; - } -} diff --git a/src/main/java/frc4388/robot/commands/Swerve/neoJoystickRecorder.java b/src/main/java/frc4388/robot/commands/Swerve/neoJoystickRecorder.java deleted file mode 100644 index 9aa9283..0000000 --- a/src/main/java/frc4388/robot/commands/Swerve/neoJoystickRecorder.java +++ /dev/null @@ -1,129 +0,0 @@ -package frc4388.robot.commands.Swerve; - -import java.io.FileOutputStream; -import java.util.ArrayList; -import java.util.function.Supplier; - -import edu.wpi.first.math.geometry.Translation2d; -import edu.wpi.first.wpilibj.XboxController; -import edu.wpi.first.wpilibj2.command.Command; -import frc4388.robot.subsystems.swerve.SwerveDrive; -import frc4388.utility.compute.DataUtils; -import frc4388.utility.controller.DeadbandedXboxController; -import frc4388.utility.structs.UtilityStructs.AutoRecordingControllerFrame; -import frc4388.utility.structs.UtilityStructs.AutoRecordingFrame; - -/** - * The NEO autonomus recording system, designed based the old {@link JoystickRecorder} System but with {@link frc4388.utility.controller.VirtualController VirtualController}s - * @author Zachary Wilke - */ -public class neoJoystickRecorder extends Command { - private final SwerveDrive swerve; - private final XboxController[] controllers; - private String filename; - private final Supplier filenameGetter; - private long startTime = -1; - private final ArrayList frames = new ArrayList<>(); - - /** - * Creates an new NEO Joystick Playback with specifyed pramiters. - * @param swerve m_robotSwerveDrive - * @param controllers an Order-Specific Array of Virtual controllers, index 0 means driver, index 1 means operator, etc. - * @param filenameGetter a String Supplier, designed for quickly changing auto names in shuffle board. - */ - public neoJoystickRecorder(SwerveDrive swerve, DeadbandedXboxController[] controllers, Supplier filenameGetter) { - this.swerve = swerve; - this.controllers = controllers; - this.filenameGetter = filenameGetter; - this.filename = ""; - - addRequirements(this.swerve); - } - - /** - * Creates an new NEO Joystick Playback with specifyed pramiters. - * @param swerve m_robotSwerveDrive - * @param controllers an Order-Specific Array of Virtual controllers, index 0 means driver, index 1 means operator, etc. - * @param filename a String containing the name of the auto file you wish to playback. - */ - public neoJoystickRecorder(SwerveDrive swerve, DeadbandedXboxController[] controllers, String filename) { - this(swerve, controllers, () -> filename); - } - - @Override - public void initialize() { - frames.clear(); - - this.startTime = System.currentTimeMillis(); - AutoRecordingFrame frame = new AutoRecordingFrame(); - frame.controllerFrames = new AutoRecordingControllerFrame[] {new AutoRecordingControllerFrame(), new AutoRecordingControllerFrame()}; - frames.add(frame); - this.filename = this.filenameGetter.get(); - } - - - @Override - public void execute() { - System.out.println("AUTORECORD: RECORDING"); - AutoRecordingFrame frame = new AutoRecordingFrame(); - frame.timeStamp = (int) (System.currentTimeMillis() - startTime); - for (int i = 0; i < controllers.length; i++) { - XboxController controller = controllers[i]; - AutoRecordingControllerFrame controllerFrame = new AutoRecordingControllerFrame(); - double[] axes = {controller.getLeftX(), controller.getLeftY(), - controller.getLeftTriggerAxis(), controller.getRightTriggerAxis(), - controller.getRightX(), controller.getRightY()}; - short button = 0; - for (int j = 0; j < 10; j++) - if (controller.getRawButton(j+1)) - button |= 1 << j; - short[] POV = {(short)(controller.getPOV())}; - controllerFrame.axes = axes; - controllerFrame.button = button; - controllerFrame.POV = POV; - frame.controllerFrames[i] = controllerFrame; - } - - frames.add(frame); - - swerve.driveWithInput(new Translation2d(frame.controllerFrames[0].axes[0], frame.controllerFrames[0].axes[1]), - new Translation2d(frame.controllerFrames[0].axes[4], frame.controllerFrames[0].axes[5]), - true); // Really jank way of doing this. - - } - @Override - public void end(boolean interrupted) { - try (FileOutputStream stream = new FileOutputStream("/home/lvuser/autos/" + filename)) { - // header: size of 0x5 - // byte Number of axes per controller - // byte Number of POVs per controller - // byte Number of controllers - // short Number of frames - stream.write(new byte[]{6, 1, (byte) controllers.length}); - stream.write(DataUtils.shortToByteArray((short) frames.size())); - - // frame - // controller frame * number of controllers - // int unix time stamp. - for (AutoRecordingFrame frame : frames) { - // controller frame - // double axis * Number of axes per controller - // short button states - // short POV * Number of POVs per controller - for (AutoRecordingControllerFrame controllerFrame: frame.controllerFrames) { - for (double axis: controllerFrame.axes) { - stream.write(DataUtils.doubleToByteArray(axis)); - } - stream.write(DataUtils.shortToByteArray(controllerFrame.button)); - for (short POV: controllerFrame.POV) { - stream.write(DataUtils.shortToByteArray(POV)); - } - } - stream.write(DataUtils.intToByteArray(frame.timeStamp)); - } - System.out.println("AUTORECORD: Wrote auto `" + filename + "` that is " + frames.size() + " frames long."); - } catch (Exception e) { - e.printStackTrace(); - } - } -} diff --git a/src/main/java/frc4388/robot/commands/WhileTrueCommand.java b/src/main/java/frc4388/robot/commands/WhileTrueCommand.java deleted file mode 100644 index 2a0d045..0000000 --- a/src/main/java/frc4388/robot/commands/WhileTrueCommand.java +++ /dev/null @@ -1,104 +0,0 @@ -package frc4388.robot.commands; - -// Copyright (c) FIRST and other WPILib contributors. -// Open Source Software; you can modify and/or share it under the terms of -// the WPILib BSD license file in the root directory of this project. - -import static edu.wpi.first.util.ErrorMessages.requireNonNullParam; - -import edu.wpi.first.util.sendable.SendableBuilder; -import edu.wpi.first.wpilibj2.command.Command; - -import java.util.function.BooleanSupplier; - -/** - * A command composition that runs one of two commands, depending on the value of the given - * condition when this command is initialized. - * - *

The rules for command compositions apply: command instances that are passed to it cannot be - * added to any other composition or scheduled individually, and the composition requires all - * subsystems its components require. - * - *

This class is provided by the NewCommands VendorDep - */ -public class WhileTrueCommand extends Command { - private final Command m_whileTrue; - private final BooleanSupplier m_condition; - - /** - * Creates a new WhileTrueCommand. - * - * @param whileTrue the command to run while the condition is true - * @param condition the condition to determine which command to run - */ - @SuppressWarnings("this-escape") - public WhileTrueCommand(Command whileTrue, BooleanSupplier condition) { - m_whileTrue = requireNonNullParam(whileTrue, "whileTrue", "WhileTrueCommand"); - m_condition = requireNonNullParam(condition, "condition", "WhileTrueCommand"); - - //CommandScheduler.getInstance().registerComposedCommands(whileTrue); - - // addRequirements(whileTrue.getRequirements()); - } - - @Override - public void initialize() { - if(m_condition.getAsBoolean()) - m_whileTrue.initialize(); - } - - @Override - public void execute() { - m_whileTrue.execute(); - - System.out.println("Loop, " + !m_whileTrue.isFinished() + ", " + m_condition.getAsBoolean()); - - if(!m_whileTrue.isFinished()) - return; - - if(m_condition.getAsBoolean()){ - m_whileTrue.end(false); - m_whileTrue.initialize(); - } - } - - @Override - public void end(boolean interrupted) { - m_whileTrue.end(interrupted); - } - - @Override - public boolean isFinished() { - return !m_condition.getAsBoolean() && m_whileTrue.isFinished(); - } - - @Override - public boolean runsWhenDisabled() { - return m_whileTrue.runsWhenDisabled(); - } - - @Override - public InterruptionBehavior getInterruptionBehavior() { - if (m_whileTrue.getInterruptionBehavior() == InterruptionBehavior.kCancelSelf) { - return InterruptionBehavior.kCancelSelf; - } else { - return InterruptionBehavior.kCancelIncoming; - } - } - - @Override - public void initSendable(SendableBuilder builder) { - super.initSendable(builder); - builder.addStringProperty("whileTrue", m_whileTrue::getName, null); - builder.addStringProperty( - "selected", - () -> { - if (m_whileTrue == null) { - return "null"; - } else { - return m_whileTrue.getName(); - } - }, - null); - } -} diff --git a/src/main/java/frc4388/robot/commands/alignment/AutoAlign.java b/src/main/java/frc4388/robot/commands/alignment/AutoAlign.java index 770280d..5a1d2d3 100644 --- a/src/main/java/frc4388/robot/commands/alignment/AutoAlign.java +++ b/src/main/java/frc4388/robot/commands/alignment/AutoAlign.java @@ -6,8 +6,8 @@ import edu.wpi.first.math.geometry.Translation2d; import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import edu.wpi.first.wpilibj2.command.Command; import frc4388.robot.constants.Constants.AutoConstants; -import frc4388.robot.subsystems.Lidar; import frc4388.robot.subsystems.swerve.SwerveDrive; +import frc4388.robot.subsystems.vision.Lidar; import frc4388.robot.subsystems.vision.Vision; import frc4388.utility.compute.FieldPositions; import frc4388.utility.structs.Gains; diff --git a/src/main/java/frc4388/robot/commands/alignment/RotTo45.java b/src/main/java/frc4388/robot/commands/alignment/RotTo45.java deleted file mode 100644 index 197cf8b..0000000 --- a/src/main/java/frc4388/robot/commands/alignment/RotTo45.java +++ /dev/null @@ -1,37 +0,0 @@ -package frc4388.robot.commands.alignment; - -import edu.wpi.first.math.geometry.Rotation2d; -import edu.wpi.first.math.geometry.Translation2d; -import edu.wpi.first.wpilibj2.command.Command; -import frc4388.robot.subsystems.swerve.SwerveDrive; - -public class RotTo45 extends Command { - - SwerveDrive m_SwerveDrive; - Rotation2d targetAngle; - - - public RotTo45(SwerveDrive swerveDrive) { - m_SwerveDrive = swerveDrive; - - addRequirements(swerveDrive); - } - - @Override - public void initialize() { - targetAngle = new Rotation2d(); - } - - @Override - public void execute() { - m_SwerveDrive.driveRelativeAngle(new Translation2d(), targetAngle); - } - - @Override - public boolean isFinished() { - Rotation2d curRot = m_SwerveDrive.getPose2d().getRotation(); - // TODO: Tune - return Math.abs(curRot.getDegrees() - targetAngle.getDegrees()) < 5; - } - -} diff --git a/src/main/java/frc4388/robot/commands/wait/waitSupplier.java b/src/main/java/frc4388/robot/commands/wait/waitSupplier.java deleted file mode 100644 index a23db69..0000000 --- a/src/main/java/frc4388/robot/commands/wait/waitSupplier.java +++ /dev/null @@ -1,36 +0,0 @@ -// Copyright (c) FIRST and other WPILib contributors. -// Open Source Software; you can modify and/or share it under the terms of -// the WPILib BSD license file in the root directory of this project. - -package frc4388.robot.commands.wait; - -import java.util.function.Supplier; - -import edu.wpi.first.wpilibj2.command.Command; - -/* You should consider using the more terse Command factories API instead https://docs.wpilib.org/en/stable/docs/software/commandbased/organizing-command-based.html#defining-commands */ -public class waitSupplier extends Command { - /** Creates a new waitSupplier. */ - private final Supplier truth; - public waitSupplier(Supplier truth) { - this.truth = truth; - } - - // Called when the command is initially scheduled. - @Override - public void initialize() {} - - // Called every time the scheduler runs while the command is scheduled. - @Override - public void execute() {} - - // Called once the command ends or is interrupted. - @Override - public void end(boolean interrupted) {} - - // Returns true when the command should end. - @Override - public boolean isFinished() { - return truth.get(); - } -} diff --git a/src/main/java/frc4388/robot/subsystems/LED.java b/src/main/java/frc4388/robot/subsystems/led/LED.java similarity index 98% rename from src/main/java/frc4388/robot/subsystems/LED.java rename to src/main/java/frc4388/robot/subsystems/led/LED.java index 58a4591..07f2876 100644 --- a/src/main/java/frc4388/robot/subsystems/LED.java +++ b/src/main/java/frc4388/robot/subsystems/led/LED.java @@ -5,7 +5,7 @@ /* the project. */ /*----------------------------------------------------------------------------*/ -package frc4388.robot.subsystems; +package frc4388.robot.subsystems.led; import java.util.concurrent.TimeUnit; diff --git a/src/main/java/frc4388/robot/subsystems/shooter/Shooter.java b/src/main/java/frc4388/robot/subsystems/shooter/Shooter.java index 15f0edf..cb46a97 100644 --- a/src/main/java/frc4388/robot/subsystems/shooter/Shooter.java +++ b/src/main/java/frc4388/robot/subsystems/shooter/Shooter.java @@ -15,8 +15,8 @@ import edu.wpi.first.units.measure.AngularVelocity; import edu.wpi.first.units.measure.Current; import edu.wpi.first.wpilibj2.command.SubsystemBase; import frc4388.robot.constants.Constants; -import frc4388.robot.subsystems.LED; import frc4388.robot.subsystems.intake.Intake; +import frc4388.robot.subsystems.led.LED; import frc4388.robot.subsystems.swerve.SwerveDrive; import frc4388.utility.compute.FieldPositions; diff --git a/src/main/java/frc4388/robot/subsystems/swerve/SwerveDrive.java b/src/main/java/frc4388/robot/subsystems/swerve/SwerveDrive.java index 9cb77d1..6065683 100644 --- a/src/main/java/frc4388/robot/subsystems/swerve/SwerveDrive.java +++ b/src/main/java/frc4388/robot/subsystems/swerve/SwerveDrive.java @@ -377,6 +377,10 @@ public class SwerveDrive extends SubsystemBase implements Queryable { driveFieldAngle(leftStick, ang); } + public Pose2d getCurrentPose(){ + return state.currentPose; + } + public void driveRelativeLockedAngle(Translation2d leftStick, Rotation2d heading) { leftStick = leftStick.rotateBy(heading); @@ -515,6 +519,7 @@ public class SwerveDrive extends SubsystemBase implements Queryable { gear_index = i; } + public void setPercentOutput(double speed) { speedAdjust = SwerveDriveConstants.MAX_SPEED_MEETERS_PER_SEC * speed; gear_index = -1; diff --git a/src/main/java/frc4388/robot/subsystems/Lidar.java b/src/main/java/frc4388/robot/subsystems/vision/Lidar.java similarity index 98% rename from src/main/java/frc4388/robot/subsystems/Lidar.java rename to src/main/java/frc4388/robot/subsystems/vision/Lidar.java index fdbf2a6..d859102 100644 --- a/src/main/java/frc4388/robot/subsystems/Lidar.java +++ b/src/main/java/frc4388/robot/subsystems/vision/Lidar.java @@ -1,4 +1,4 @@ -package frc4388.robot.subsystems; +package frc4388.robot.subsystems.vision; import java.util.ArrayList; import java.util.LinkedList; @@ -20,8 +20,8 @@ import edu.wpi.first.math.geometry.Translation2d; import edu.wpi.first.math.util.Units; import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import edu.wpi.first.wpilibj2.command.SubsystemBase; -import frc4388.robot.subsystems.RPLidarA1.PolarPoint; -import frc4388.robot.subsystems.RPLidarA1.ScanListener; +import frc4388.robot.subsystems.vision.RPLidarA1.PolarPoint; +import frc4388.robot.subsystems.vision.RPLidarA1.ScanListener; import frc4388.utility.configurable.ConfigurableDouble; import frc4388.utility.status.FaultA1M8; diff --git a/src/main/java/frc4388/robot/subsystems/RPLidarA1.java b/src/main/java/frc4388/robot/subsystems/vision/RPLidarA1.java similarity index 99% rename from src/main/java/frc4388/robot/subsystems/RPLidarA1.java rename to src/main/java/frc4388/robot/subsystems/vision/RPLidarA1.java index fa5aa32..41ad20a 100644 --- a/src/main/java/frc4388/robot/subsystems/RPLidarA1.java +++ b/src/main/java/frc4388/robot/subsystems/vision/RPLidarA1.java @@ -1,4 +1,4 @@ -package frc4388.robot.subsystems; +package frc4388.robot.subsystems.vision; import com.fazecast.jSerialComm.SerialPort; import edu.wpi.first.wpilibj.DriverStation; diff --git a/src/main/java/frc4388/utility/status/FaultA1M8.java b/src/main/java/frc4388/utility/status/FaultA1M8.java index aedb866..e99eb3e 100644 --- a/src/main/java/frc4388/utility/status/FaultA1M8.java +++ b/src/main/java/frc4388/utility/status/FaultA1M8.java @@ -1,7 +1,7 @@ package frc4388.utility.status; -import frc4388.robot.subsystems.RPLidarA1; -import frc4388.robot.subsystems.RPLidarA1.ConnectionStatus; +import frc4388.robot.subsystems.vision.RPLidarA1; +import frc4388.robot.subsystems.vision.RPLidarA1.ConnectionStatus; import frc4388.utility.status.Status.ReportLevel; // Fault reporter for the RPLidar A1M8 Revolving lidar sensor