Team4388/2026KPopRobotHunters
1
0
Fork 0
mirror of https://github.com/Team4388/2026KPopRobotHunters.git synced 2026-06-09 00:38:03 -06:00
Code Issues Packages Projects Releases Wiki Activity

Work in progress

Browse Source
This commit is contained in:
mimigamin
2026-03-10 08:39:05 -06:00
parent 5b85bb4ace
commit 50b8312c48
17 changed files with 219 additions and 779 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/main/java/frc4388/robot/RobotContainer.java
+137 -126
View File
@@ -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<Subsystem> subsystems = new ArrayList<>();
private final StayInPosition m_stayInPosition = new StayInPosition(m_robotSwerveDrive);
// ! Teleop Commands
public void stop() {
new InstantCommand(()->{}, m_robotSwerveDrive).schedule();
m_robotSwerveDrive.stopModules();
Constants.AutoConstants.Y_OFFSET_TRIM.set(0);
}
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);
}
// ! /* Autos */
private SendableChooser<String> autoChooser;
private Command autoCommand;
// ! /* Autos */
private SendableChooser<String> autoChooser;
private Command autoCommand;
private Command IntakeExtended = new SequentialCommandGroup(
new InstantCommand(() -> m_robotIntake.setMode(IntakeMode.Extended), m_robotIntake)
);
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)
// 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))
// );
// // 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 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 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 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)
);
public RobotContainer() {
public RobotContainer() {
configureButtonBindings();
configureButtonBindings();
// Called on first robot enable
DeferredBlock.addBlock(() -> {
m_robotSwerveDrive.resetGyro();
}, false);
// Called on first robot enable
DeferredBlock.addBlock(() -> {
m_robotSwerveDrive.resetGyro();
}, false);
// Called on every robot enable
DeferredBlock.addBlock(() -> {
TimesNegativeOne.update();
FieldPositions.update();
// 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);
}
/**
* 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();
}, 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);
}
/**
* 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();
}));
// 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))
//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();
}));
src/main/java/frc4388/robot/commands/MoveForTimeCommand.java
-49
View File
@@ -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;
}
}
src/main/java/frc4388/robot/commands/MoveUntilSuply.java
-45
View File
@@ -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<Boolean> truth;
private final boolean robotRelative;
public MoveUntilSuply(
SwerveDrive swerveDrive,
Translation2d leftStick,
Translation2d rightStick,
Supplier<Boolean> 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();
}
}
src/main/java/frc4388/robot/commands/Swerve/RotateToAngle.java
-35
View File
@@ -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);
}
}
src/main/java/frc4388/robot/commands/Swerve/StayInPosition.java
+56
View File
@@ -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;
// }
}
src/main/java/frc4388/robot/commands/Swerve/neoJoystickPlayback.java
-197
View File
@@ -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<AutoRecordingFrame> frames = new ArrayList<>();
private final Supplier<String> 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 <b>Order-Specific</b> 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<String> 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 <b>Order-Specific</b> 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 <b>Order-Specific</b> Array of Virtual controllers, index 0 means driver, index 1 means operator, etc.
*/
public neoJoystickPlayback(SwerveDrive swerve, Supplier<String> 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 <b>Order-Specific</b> 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;
}
}
src/main/java/frc4388/robot/commands/Swerve/neoJoystickRecorder.java
-129
View File
@@ -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<String> filenameGetter;
private long startTime = -1;
private final ArrayList<AutoRecordingFrame> frames = new ArrayList<>();
/**
* Creates an new NEO Joystick Playback with specifyed pramiters.
* @param swerve m_robotSwerveDrive
* @param controllers an <b>Order-Specific</b> 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<String> 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 <b>Order-Specific</b> 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();
}
}
}
src/main/java/frc4388/robot/commands/WhileTrueCommand.java
-104
View File
@@ -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.
*
* <p>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.
*
* <p>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);
}
}
src/main/java/frc4388/robot/commands/alignment/AutoAlign.java
+1 -1
View File
@@ -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;
src/main/java/frc4388/robot/commands/alignment/RotTo45.java
-37
View File
@@ -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;
}
}
src/main/java/frc4388/robot/commands/wait/waitSupplier.java
-36
View File
@@ -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<Boolean> truth;
public waitSupplier(Supplier<Boolean> 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();
}
}
src/main/java/frc4388/robot/subsystems/LED.java → src/main/java/frc4388/robot/subsystems/led/LED.java
+1 -1
View File
@@ -5,7 +5,7 @@
/* the project. */
/*----------------------------------------------------------------------------*/
package frc4388.robot.subsystems;
package frc4388.robot.subsystems.led;
import java.util.concurrent.TimeUnit;
src/main/java/frc4388/robot/subsystems/shooter/Shooter.java
+1 -1
View File
@@ -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;
src/main/java/frc4388/robot/subsystems/swerve/SwerveDrive.java
+5
View File
@@ -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;
src/main/java/frc4388/robot/subsystems/Lidar.java → src/main/java/frc4388/robot/subsystems/vision/Lidar.java
+3 -3
View File
@@ -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;
src/main/java/frc4388/robot/subsystems/RPLidarA1.java → src/main/java/frc4388/robot/subsystems/vision/RPLidarA1.java
+1 -1
View File
@@ -1,4 +1,4 @@
package frc4388.robot.subsystems;
package frc4388.robot.subsystems.vision;
import com.fazecast.jSerialComm.SerialPort;
import edu.wpi.first.wpilibj.DriverStation;
src/main/java/frc4388/utility/status/FaultA1M8.java
+2 -2
View File
@@ -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