Team4388/2026KPopRobotHunters
1
0
Fork 0
mirror of https://github.com/Team4388/2026KPopRobotHunters.git synced 2026-06-09 08:48:05 -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/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();
}
}