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/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();
}
}
}