Initial commit

src/main/java/frc4388/robot/commands/Autos/neo AutoRecoding format.txt

@@ -0,0 +1,20 @@
+AUTO file format
+
+HEADER static size 0x5
+0x00 BYTE   NUM AXES: defualts to 6
+0x01 BYTE   NUM POV: defualts to 1
+0x02 BYTE   NUM CONTROLLERS: defualts to 2
+0x03 SHORT  FRAMES: any value greator or equal than one.
+
+FRAME PER CONTROLLER: defualt size 0x34
+0x00 DOUBLE AXES[NUM AXES]
+0x30 SHORT  BUTTONS
+0x32 SHORT  POVs[NUM POV]
+
+FRAME: size varrys
+FRAME PER CONTROLLER[NUM CONTROLLERS]
+INT         UNIXTIMESTAMP
+
+FILE:
+HEADER
+FRAME[FRAMES]

src/main/java/frc4388/robot/commands/Autos/neoPlaybackChooser.java

@@ -0,0 +1,108 @@
+package frc4388.robot.commands.autos;
+// package frc4388.robot.commands.Autos;
+
+// import java.io.File;
+// import java.util.ArrayList;
+// import java.util.HashMap;
+
+// import edu.wpi.first.wpilibj.shuffleboard.BuiltInWidgets;
+// import edu.wpi.first.wpilibj.shuffleboard.ComplexWidget;
+// import edu.wpi.first.wpilibj.shuffleboard.Shuffleboard;
+// import edu.wpi.first.wpilibj.smartdashboard.SendableChooser;
+// import edu.wpi.first.wpilibj2.command.Command;
+// import edu.wpi.first.wpilibj2.command.InstantCommand;
+// import frc4388.robot.commands.Swerve.JoystickPlayback;
+// import frc4388.robot.commands.Swerve.neoJoystickPlayback;
+// import frc4388.robot.subsystems.SwerveDrive;
+// import frc4388.utility.controller.VirtualController;
+
+// public class neoPlaybackChooser {
+//     private final SendableChooser<String> m_teamChosser = new SendableChooser<String>();    
+//     private final SendableChooser<String> m_possitionChosser = new SendableChooser<String>();
+//     private final SendableChooser<String> m_autoNameChosser = new SendableChooser<String>();
+
+//     private final SwerveDrive m_swerve;
+//     private final VirtualController[] m_controllers;
+//     // private final ArrayList<SendableChooser<Command>> m_choosers    = new ArrayList<>();
+//     // private       SendableChooser<Command>            m_playback    = null;
+//     private final ArrayList<ComplexWidget>            m_widgets     = new ArrayList<>();
+//     // private final HashMap<String, Command>            m_commandPool = new HashMap<>();
+    
+//     // private final File        m_dir    = new File("/home/lvuser/autos/");
+//     // private       int         m_cmdNum = 0;
+
+//     // commands
+//     private Command m_noAuto = new InstantCommand();
+    
+//     public neoPlaybackChooser(SwerveDrive swerve, VirtualController[] controllers) {
+//         m_teamChosser.addOption("Red", "red");
+//         m_teamChosser.setDefaultOption("Blue", "blue");
+//         m_teamChosser.addOption("Nuetral", "nuetral");
+//         m_possitionChosser.addOption("AMP", "amp");
+//         m_possitionChosser.setDefaultOption("Center", "center");
+//         m_possitionChosser.addOption("Source", "source");
+//         m_swerve = swerve;
+//         m_controllers = controllers;
+//     }
+
+//     public neoPlaybackChooser addOption(String name, String option) {
+//         m_autoNameChosser.addOption(name, option);
+//         return this;
+//     }
+
+//     // public PlaybackChooser buildDisplay() {
+//     //     for (int i = 0; i < 10; i++) {
+//     //         appendCommand();
+//     //     }
+//     //     m_playback = m_choosers.get(0);
+//     //     nextChooser();
+
+//     //     // ! This does not work, why?
+//     //     Shuffleboard.getTab("Auto Chooser")
+//     //         .add("Add Sequence", new InstantCommand(() -> nextChooser()))
+//     //         .withPosition(4, 0);
+//     //     return this;
+//     // }
+
+//     // This will be bound to a button for the time being
+//     public void render() {
+//         // var chooser = new SendableChooser<Command>();
+//         // // chooser.setDefaultOption("No Auto", m_noAuto);
+
+//         // m_choosers.add(chooser);
+//         ComplexWidget widget = Shuffleboard.getTab("Neo Auto Chooser")
+//             .add("Command: " + m_choosers.size(), chooser)
+//             .withSize(4, 1)
+//             .withPosition(0, m_choosers.size() - 1)
+//             .withWidget(BuiltInWidgets.kSplitButtonChooser)
+//             .withWidget(BuiltInWidgets.kComboBoxChooser);
+
+
+//         m_widgets.add(widget);
+//     }
+
+//     // public void nextChooser() {
+//     //     SendableChooser<Command> chooser = m_choosers.get(m_cmdNum++);
+
+//     //     String[] dirs = m_dir.list();
+
+//     //     if(dirs != null){ // Fix funny error
+//     //         for (String auto : dirs) {
+//     //             chooser.addOption(auto, new JoystickPlayback(m_swerve, auto));
+//     //         }
+//     //     }
+
+        
+//     //     for (var cmd_name : m_commandPool.keySet()) {
+//     //         chooser.addOption(cmd_name, m_commandPool.get(cmd_name));
+//     //     }
+//     // }
+    
+//     public String autoName() {
+//         return m_teamChosser.getSelected() + "_" + m_possitionChosser.getSelected() + "_" + m_autoNameChosser.getSelected() + ".auto";
+//     }
+
+//     public Command getCommand() {
+//         return new neoJoystickPlayback(m_swerve, autoName(), m_controllers, true, true);
+//     }
+// }

src/main/java/frc4388/robot/commands/MoveForTimeCommand.java

@@ -0,0 +1,49 @@
+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

@@ -0,0 +1,45 @@
+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/PID.java

@@ -0,0 +1,60 @@
+// 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;
+
+import edu.wpi.first.wpilibj2.command.Command;
+import frc4388.utility.structs.Gains;
+
+public abstract class PID extends Command {
+	protected Gains  gains;
+	private   double output    = 0;
+	private   double tolerance = 0;
+
+	/** Creates a new PelvicInflammatoryDisease. */
+	public PID(double kp, double ki, double kd, double kf, double tolerance) {
+		gains          = new Gains(kp, ki, kd, kf, 0);
+		this.tolerance = tolerance;
+	}
+
+	public PID(Gains gains, double tolerance) {
+		this.gains     = gains;
+		this.tolerance = tolerance;
+	}
+
+	/** produces the error from the setpoint */
+	public abstract double getError();
+	
+	/** todo: javadoc */
+	public abstract void runWithOutput(double output);
+
+	// Called when the command is initially scheduled.
+	@Override 
+	public final void initialize() {
+		output = 0;
+	}
+
+	private double prevError, cumError = 0;
+	
+	// Called every time the scheduler runs while the command is scheduled.
+	@Override 
+	public final void execute() {
+		double error = getError();
+		cumError += error * .02; // 20 ms
+		double delta = error - prevError;
+
+		output = error * gains.kP;
+		output += cumError * gains.kI;
+		output += delta * gains.kD;
+		output += gains.kF;
+
+		runWithOutput(output);
+	}
+
+	// Returns true when the command should end.
+	@Override
+	public final boolean isFinished() {
+		return Math.abs(getError()) < tolerance;
+	}
+}

src/main/java/frc4388/robot/commands/Swerve/RotateToAngle.java

@@ -0,0 +1,35 @@
+// 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/neoJoystickPlayback.java

@@ -0,0 +1,197 @@
+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

@@ -0,0 +1,129 @@
+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

@@ -0,0 +1,104 @@
+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/DriveToReef.java

@@ -0,0 +1,188 @@
+package frc4388.robot.commands.alignment;
+
+import edu.wpi.first.math.geometry.Pose2d;
+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.Command;
+import frc4388.robot.constants.Constants.AutoConstants;
+import frc4388.robot.subsystems.swerve.SwerveDrive;
+import frc4388.robot.subsystems.vision.Vision;
+import frc4388.utility.compute.ReefPositionHelper;
+import frc4388.utility.compute.TimesNegativeOne;
+import frc4388.utility.compute.ReefPositionHelper.Side;
+import frc4388.utility.structs.Gains;
+
+public class DriveToReef extends Command {
+    
+
+    // private Translation2d translation2d= new Translation2d(14.579471999999997,0.24587199999999998);
+    // private Translation2d translation2d= new Translation2d(16.579342-0.15,5.547867999999999);
+
+    private PID xPID = new PID(AutoConstants.XY_GAINS, 0);
+    private PID yPID = new PID(AutoConstants.XY_GAINS, 0);
+    // private PID rotPID = new PID(AutoConstants.ROT_GAINS, 0);
+    private Pose2d targetpos;
+
+    SwerveDrive swerveDrive;
+    Vision vision;
+    double distance;
+    Side side;
+    boolean waitVelocityZero;
+
+    /**
+     * Command to drive robot to position.
+     * @param SwerveDrive m_robotSwerveDrive
+     */
+
+    public DriveToReef(SwerveDrive swerveDrive, Vision vision, double distance, Side side, boolean waitVelocityZero) {
+        this.swerveDrive = swerveDrive;
+        this.vision = vision;
+        this.distance = distance;
+        this.side = side;
+        this.waitVelocityZero = waitVelocityZero && false;
+        addRequirements(swerveDrive);
+    }
+
+
+    public static double tagRelativeXError = -1;
+    private static void setTagRelativeXError(double val){
+        tagRelativeXError = val;
+    }
+
+    @Override
+    public void initialize() {
+        xPID.initialize();
+        yPID.initialize();
+        this.targetpos = ReefPositionHelper.getNearestPosition(
+            this.vision.getPose2d(), 
+            side, 
+            Units.inchesToMeters(AutoConstants.X_OFFSET_TRIM.get()), 
+            distance + Units.inchesToMeters(AutoConstants.Y_OFFSET_TRIM.get())
+        );
+    }
+    
+    double xerr;
+    double yerr;
+    double roterr;
+
+    double xoutput;
+    double youtput;
+    double rotoutput;
+
+    @Override
+    public void execute() {
+        xerr = TimesNegativeOne.invert(targetpos.getX() - vision.getPose2d().getX(), TimesNegativeOne.XAxis);
+        yerr = TimesNegativeOne.invert(targetpos.getY() - vision.getPose2d().getY(), !TimesNegativeOne.YAxis);
+        // xerr = targetpos.getX() - vision.getPose2d().getX();
+        // yerr = targetpos.getX() - vision.getPose2d().getY();
+
+        // roterr = TimesNegativeOne.invert(targetpos.getRotation().getDegrees() - vision.getPose2d().getRotation().getDegrees(), TimesNegativeOne.isRed);
+
+        roterr = ((targetpos.getRotation().getDegrees() - vision.getPose2d().getRotation().getDegrees()));
+
+        if(roterr > 180){
+            roterr -= 360;
+        }else if(roterr < -180){
+            roterr += 360;
+        }
+
+        // SmartDashboard.putNumber("Rotational PID target", targetpos.getRotation().getDegrees());
+        // SmartDashboard.putNumber("Rotational PID position", vision.getPose2d().getRotation().getDegrees());
+        // SmartDashboard.putNumber("Rotational PID error", roterr);
+
+        SmartDashboard.putNumber("PID X Error", xerr);
+        SmartDashboard.putNumber("PID Y Error", yerr);
+        SmartDashboard.putNumber("PID Rot Error", roterr);
+
+        xoutput = xPID.update(xerr);
+        youtput = yPID.update(yerr);
+        // rotoutput = rotPID.update(roterr);
+
+        xoutput *= Math.abs(xerr) < AutoConstants.XY_TOLERANCE ? 0 : 1;
+        youtput *= Math.abs(yerr) < AutoConstants.XY_TOLERANCE ? 0 : 1;
+        // rotoutput *= Math.abs(roterr) < AutoConstants.ROT_TOLERANCE ? 0 : 1;
+        
+
+
+        Translation2d leftStick = new Translation2d(
+            Math.max(Math.min(-youtput, 1), -1),
+            Math.max(Math.min(-xoutput, 1), -1)
+            // 0,0
+        );
+
+        // Translation2d rightStick = new Translation2d(
+        //     Math.max(Math.min(rotoutput, 1), -1), 
+        //    0
+        // );
+
+        setTagRelativeXError(
+            new Translation2d(xerr, yerr).getAngle()
+            .rotateBy(targetpos.getRotation())
+            .getCos());
+
+        swerveDrive.driveRelativeAngle(leftStick, targetpos.getRotation());
+        // swerveDrive.driveWithInputOrientation(leftStick, rightStick, true);
+    }
+
+    @Override
+    public final boolean isFinished() {
+        boolean finished = (
+            (Math.abs(xerr) < AutoConstants.XY_TOLERANCE || Math.abs(xoutput) <= AutoConstants.MIN_XY_PID_OUTPUT) && 
+            (Math.abs(yerr) < AutoConstants.XY_TOLERANCE || Math.abs(youtput) <= AutoConstants.MIN_XY_PID_OUTPUT) && 
+            (Math.abs(roterr) < AutoConstants.ROT_TOLERANCE) &&
+            (!waitVelocityZero || swerveDrive.lastOdomSpeed < AutoConstants.VELOCITY_THRESHHOLD)
+        );
+        // System.out.println(finished);
+
+        if(finished)
+            swerveDrive.softStop();
+
+        return finished;
+                // this statement is a boolean in and of itself
+    }
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+    private class PID {
+        protected Gains  gains;
+        private   double output    = 0;
+
+
+        /** Creates a new PelvicInflammatoryDisease. */
+        public PID(Gains gains, double tolerance) {
+            this.gains     = gains;
+        }
+
+        // Called when the command is initially scheduled.
+        public final void initialize() {
+            output = 0;
+        }
+
+        private double prevError, cumError = 0;
+        
+        // Called every time the scheduler runs while the command is scheduled.
+        public double update(double error) {
+            cumError += error * .02; // 20 ms
+            double delta = error - prevError;
+
+            output = error * gains.kP;
+            output += cumError * gains.kI;
+            output += delta * gains.kD;
+            output += gains.kF;
+
+            return output;
+        }
+    }
+}

src/main/java/frc4388/robot/commands/alignment/DriveUntilLiDAR.java

@@ -0,0 +1,48 @@
+package frc4388.robot.commands.alignment;
+
+import edu.wpi.first.math.geometry.Translation2d;
+import edu.wpi.first.wpilibj2.command.Command;
+import frc4388.robot.subsystems.lidar.LiDAR;
+import frc4388.robot.subsystems.swerve.SwerveDrive;
+
+// Command to repeat a joystick movement for a specific time.
+public class DriveUntilLiDAR extends Command {
+    private final SwerveDrive swerveDrive;
+    private final Translation2d leftStick;
+    private final Translation2d rightStick;
+    private final LiDAR m_lidar;
+    private final double mindistance;
+
+    public DriveUntilLiDAR(
+        SwerveDrive swerveDrive, 
+        Translation2d leftStick, 
+        Translation2d rightStick, 
+        LiDAR lidar,
+        double mindistance) {
+            addRequirements(swerveDrive);
+
+        this.swerveDrive = swerveDrive;
+        this.leftStick = leftStick;
+        this.rightStick = rightStick;
+        this.m_lidar = lidar;
+        this.mindistance = mindistance;
+    }
+
+    @Override
+    public void initialize() {
+    }
+
+    @Override
+    public void execute() {
+        swerveDrive.driveFine(leftStick, rightStick, 0.3);
+    }
+
+    @Override
+    public boolean isFinished() {
+        if (Math.abs(m_lidar.getDistance()) < mindistance) {
+            swerveDrive.softStop();
+            return true;
+        }
+        return false;
+    }
+}

src/main/java/frc4388/robot/commands/alignment/LidarAlign.java

@@ -0,0 +1,107 @@
+// 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.alignment;
+
+import edu.wpi.first.math.geometry.Rotation2d;
+import edu.wpi.first.math.geometry.Translation2d;
+import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
+import edu.wpi.first.wpilibj2.command.Command;
+import frc4388.robot.subsystems.lidar.LiDAR;
+import frc4388.robot.subsystems.swerve.SwerveDrive;
+
+/* 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 LidarAlign extends Command {
+  private SwerveDrive swerveDrive;
+  private LiDAR lidar;  
+
+  private int currentFinderTick;
+  // private int tickFoundPipe;
+  private boolean foundReef;
+  private boolean headedRight;
+  private double speed;
+  private int bounces;
+  private double additionalDistance = 0;
+  // private final boolean constructedHeadedRight;
+
+  /** Creates a new LidarAlign. */
+  public LidarAlign(SwerveDrive swerveDrive, LiDAR lidar) {//, boolean headedRight) {
+    // Use addRequirements() here to declare subsystem dependencies.
+
+    this.swerveDrive = swerveDrive;
+    this.lidar = lidar;
+
+    addRequirements(swerveDrive, lidar);
+  }
+
+  // Called when the command is initially scheduled.
+  @Override
+  public void initialize() {
+    this.currentFinderTick = 0;
+    this.speed = 0.4; // TODO: find good speed for this
+    this.foundReef = false;
+    this.headedRight = (DriveToReef.tagRelativeXError < 0);
+    this.additionalDistance = 0;
+    this.bounces = 0;
+  }
+
+
+  // Called every time the scheduler runs while the command is scheduled.
+  @Override
+  public void execute() {
+    if (lidar.withinDistance()) {
+      swerveDrive.softStop(); 
+      foundReef = true;
+      return;
+    }
+
+    if (currentFinderTick > (15 + additionalDistance)) { //arbutrary threshhold for now.
+      headedRight = !headedRight;
+      currentFinderTick *= -1;
+      bounces++;
+      additionalDistance += 5;
+      if (bounces == 5) return;
+    }
+    double currentHeading = (swerveDrive.getGyroAngle() * 180) / Math.PI;
+    double relAngle = (Math.round(currentHeading / 60.d) * 60); // Relative driving to the side of the reef
+    SmartDashboard.putNumber("Rel Angle", relAngle);
+    SmartDashboard.putNumber("heading", currentHeading);
+    if (!headedRight) {
+      swerveDrive.driveRelativeLockedAngle(new Translation2d(0, -speed), Rotation2d.fromDegrees(relAngle));
+    } else {
+      swerveDrive.driveRelativeLockedAngle(new Translation2d(0, speed), Rotation2d.fromDegrees(relAngle));
+    }
+
+    currentFinderTick++;
+  }
+
+  // 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() {
+    if (lidar.getDistance() < 4) {
+      swerveDrive.stopModules();
+      return true;
+    } else if (foundReef && lidar.withinDistance()) { // spot on
+      swerveDrive.stopModules();
+      return true;
+    } else if (foundReef && !lidar.withinDistance()) { // over shot
+      speed = speed / 2;
+      headedRight = !headedRight;
+      currentFinderTick = 0;
+      foundReef = false;
+      return false;
+    } else if (bounces >= 3) {
+      swerveDrive.stopModules();
+      return true;
+    } else {
+      return false;
+    }
+  }
+}

src/main/java/frc4388/robot/commands/wait/waitElevatorRefrence.java

@@ -0,0 +1,36 @@
+// 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 edu.wpi.first.wpilibj2.command.Command;
+import frc4388.robot.subsystems.elevator.Elevator;
+
+/* 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 waitElevatorRefrence extends Command {
+  /** Creates a new waitElevatorRefrence. */
+  private Elevator elevator;
+  public waitElevatorRefrence(Elevator elevator) {
+    this.elevator = elevator;
+    // Use addRequirements() here to declare subsystem dependencies.
+  }
+
+  // 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 elevator.elevatorAtReference();
+  }
+}

src/main/java/frc4388/robot/commands/wait/waitEndefectorRefrence.java

@@ -0,0 +1,36 @@
+// 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 edu.wpi.first.wpilibj2.command.Command;
+import frc4388.robot.subsystems.elevator.Elevator;
+
+/* 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 waitEndefectorRefrence extends Command {
+  /** Creates a new waitElevatorRefrence. */
+  private Elevator elevator;
+  public waitEndefectorRefrence(Elevator elevator) {
+    this.elevator = elevator;
+    // Use addRequirements() here to declare subsystem dependencies.
+  }
+
+  // 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 elevator.endeffectorAtReference();
+  }
+}

src/main/java/frc4388/robot/commands/wait/waitFeedCoral.java

@@ -0,0 +1,36 @@
+// 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 edu.wpi.first.wpilibj2.command.Command;
+import frc4388.robot.subsystems.elevator.Elevator;
+
+/* 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 waitFeedCoral extends Command {
+  /** Creates a new waitElevatorRefrence. */
+  private Elevator elevator;
+  public waitFeedCoral(Elevator elevator) {
+    this.elevator = elevator;
+    // Use addRequirements() here to declare subsystem dependencies.
+  }
+
+  // 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 elevator.hasCoral();
+  }
+}

src/main/java/frc4388/robot/commands/wait/waitSupplier.java

@@ -0,0 +1,36 @@
+// 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();
+  }
+}