diff --git a/src/main/java/frc4388/robot/commands/Autos/PlaybackChooser.java b/src/main/java/frc4388/robot/commands/Autos/PlaybackChooser.java deleted file mode 100644 index f3d636d..0000000 --- a/src/main/java/frc4388/robot/commands/Autos/PlaybackChooser.java +++ /dev/null @@ -1,98 +0,0 @@ -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.subsystems.SwerveDrive; - -public class PlaybackChooser { - private final ArrayList> m_choosers = new ArrayList<>(); - private SendableChooser m_playback = null; - private final ArrayList m_widgets = new ArrayList<>(); - private final HashMap m_commandPool = new HashMap<>(); - - private final File m_dir = new File("/home/lvuser/autos/"); - private int m_cmdNum = 0; - private final SwerveDrive m_swerve; - - // commands - private Command m_noAuto = new InstantCommand(); - - public PlaybackChooser(SwerveDrive swerve, Object... pool) { - m_swerve = swerve; - } - - public PlaybackChooser addOption(String name, Command option) { - m_commandPool.put(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 appendCommand() { - var chooser = new SendableChooser(); - chooser.setDefaultOption("No Auto", m_noAuto); - - m_choosers.add(chooser); - ComplexWidget widget = Shuffleboard.getTab("Auto Chooser") - .add("Command: " + m_choosers.size(), chooser) - .withSize(4, 1) - .withPosition(0, m_choosers.size() - 1) - .withWidget(BuiltInWidgets.kSplitButtonChooser); - - m_widgets.add(widget); - } - - public void nextChooser() { - SendableChooser 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 Command getCommand() { - Command command = m_playback.getSelected(); - command = command == null ? m_noAuto : command.asProxy(); - - Command[] commands = new Command[m_cmdNum - 1]; - for (int i = 0; i < m_cmdNum - 1; i++) { - Command command2 = m_choosers.get(i + 1).getSelected(); - command2 = command2 == null ? m_noAuto : command2.asProxy(); - - commands[i] = command2.asProxy(); - } - - return command.andThen(commands); - } -} diff --git a/src/main/java/frc4388/robot/commands/Swerve/JoystickPlayback.java b/src/main/java/frc4388/robot/commands/Swerve/JoystickPlayback.java deleted file mode 100644 index ae054ed..0000000 --- a/src/main/java/frc4388/robot/commands/Swerve/JoystickPlayback.java +++ /dev/null @@ -1,145 +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 java.io.File; -import java.io.FileNotFoundException; -import java.util.ArrayList; -import java.util.Scanner; -import edu.wpi.first.math.geometry.Translation2d; -import edu.wpi.first.wpilibj2.command.Command; -import frc4388.robot.subsystems.SwerveDrive; -import frc4388.utility.UtilityStructs.TimedOutput; - -public class JoystickPlayback extends Command { - private final SwerveDrive swerve; - private String filename; - private int mult = 1; - private Scanner input; - private final ArrayList outputs = new ArrayList<>(); - private int counter = 0; - private long startTime = 0; - private long playbackTime = 0; - private int lastIndex; - private boolean m_finished = false; // ! find a better way - - /** Creates a new JoystickPlayback. */ - public JoystickPlayback(SwerveDrive swerve, String filename, int mult) { - this.swerve = swerve; - this.filename = filename; - this.mult = mult; - - addRequirements(this.swerve); - } - - /** Creates a new JoystickPlayback. */ - public JoystickPlayback(SwerveDrive swerve, String filename) { - this(swerve, filename, 1); - } - - // Called when the command is initially scheduled. - @Override - public void initialize() { - outputs.clear(); - m_finished = false; - - startTime = System.currentTimeMillis(); - playbackTime = 0; - lastIndex = 0; - try { - input = new Scanner(new File("/home/lvuser/autos/" + filename)); - - String line = ""; - while (input.hasNextLine()) { - line = input.nextLine(); - - if (line.isEmpty() || line.isBlank() || line.equals("\n")) { - continue; - } - - String[] values = line.split(","); - System.out.println("values: " + values[0] + " " + values[1] + " " + values[2] + " " + values[3]); - - var out = new TimedOutput(); - out.leftX = Double.parseDouble(values[0]) * mult; - out.leftY = Double.parseDouble(values[1]); - out.rightX = Double.parseDouble(values[2]); - out.rightY = Double.parseDouble(values[3]); - - out.timedOffset = Long.parseLong(values[4]); - - outputs.add(out); - } - - input.close(); - } catch (FileNotFoundException e) { - e.printStackTrace(); - } - } - - // Called every time the scheduler runs while the command is scheduled. - @Override - public void execute() { - if (counter == 0) { - startTime = System.currentTimeMillis(); - playbackTime = 0; - } else { - playbackTime = System.currentTimeMillis() - startTime; - } - - // skip to reasonable time frame - // too tired to write comment: ask daniel thomas; it goes to the thing until it's bigger than the other thing - { - int i = lastIndex == 0 ? 1 : lastIndex; - while (i < outputs.size() && outputs.get(i).timedOffset < playbackTime) { - i++; - } - - if (i >= outputs.size()) { - m_finished = true; // ! kind of a hack - return; - } - lastIndex = i; - } - - TimedOutput lastOut = outputs.get(lastIndex - 1); - TimedOutput out = outputs.get(lastIndex); - - double deltaTime = out.timedOffset - lastOut.timedOffset; - double playbackDelta = playbackTime - lastOut.timedOffset; - - double lerpLX = lastOut.leftX + (out.leftX - lastOut.leftX) * (playbackDelta / deltaTime); - double lerpLY = lastOut.leftY + (out.leftY - lastOut.leftY) * (playbackDelta / deltaTime); - double lerpRX = lastOut.rightX + (out.rightX - lastOut.rightX) * (playbackDelta / deltaTime); - double lerpRY = lastOut.rightY + (out.rightY - lastOut.rightY) * (playbackDelta / deltaTime); - - // this.swerve.driveWithInput(new Translation2d(out.leftX, out.leftY), - // new Translation2d(out.rightX, out.rightY), - // true); - - // this.swerve.driveWithInput( new Translation2d(lerpLX, lerpLY), - // new Translation2d(lerpRX, lerpRY), - // true); - - this.swerve.playbackDriveWithInput( new Translation2d(lerpLX, lerpLY), - new Translation2d(lerpRX, lerpRY), - true); - - counter++; - } - - // Called once the command ends or is interrupted. - @Override - public void end(boolean interrupted) { - input.close(); - swerve.stopModules(); - } - - // Returns true when the command should end. - @Override - public boolean isFinished() { - return m_finished; - } -} \ No newline at end of file diff --git a/src/main/java/frc4388/robot/commands/Swerve/JoystickRecorder.java b/src/main/java/frc4388/robot/commands/Swerve/JoystickRecorder.java deleted file mode 100644 index 0224fcf..0000000 --- a/src/main/java/frc4388/robot/commands/Swerve/JoystickRecorder.java +++ /dev/null @@ -1,97 +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 java.io.File; -import java.io.IOException; -import java.io.PrintWriter; -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.SwerveDrive; -import frc4388.utility.UtilityStructs.TimedOutput; - -public class JoystickRecorder extends Command { - public final SwerveDrive swerve; - - public final Supplier leftX; - public final Supplier leftY; - public final Supplier rightX; - public final Supplier rightY; - private String filename; - public final ArrayList outputs = new ArrayList<>(); - private long startTime = -1; - - - /** Creates a new JoystickRecorder. */ - public JoystickRecorder(SwerveDrive swerve, Supplier leftX, Supplier leftY, - Supplier rightX, Supplier rightY, - String filename) - { - this.swerve = swerve; - this.leftX = leftX; - this.leftY = leftY; - this.rightX = rightX; - this.rightY = rightY; - this.filename = filename; - - addRequirements(this.swerve); - } - - // Called when the command is initially scheduled. - @Override - public void initialize() { - outputs.clear(); - - this.startTime = System.currentTimeMillis(); - - outputs.add(new TimedOutput()); - } - - // Called every time the scheduler runs while the command is scheduled. - @Override - public void execute() { - var inputs = new TimedOutput(); - inputs.leftX = leftX.get(); - inputs.leftY = leftY.get(); - inputs.rightX = rightX.get(); - inputs.rightY = rightY.get(); - inputs.timedOffset = System.currentTimeMillis() - startTime; - - outputs.add(inputs); - - swerve.playbackDriveWithInput(new Translation2d(inputs.leftX, inputs.leftY), - new Translation2d(inputs.rightX, inputs.rightY), - true); - - //System.out.println("RECORDING"); - } - - // Called once the command ends or is interrupted. - @Override - public void end(boolean interrupted) { - File output = new File("/home/lvuser/autos/" + filename); - - try (PrintWriter writer = new PrintWriter(output)) { - for (var input : outputs) { - writer.println( input.leftX + "," + input.leftY + "," + - input.rightX + "," + input.rightY + "," + - input.timedOffset); - } - - writer.close(); - } catch (IOException e) { - e.printStackTrace(); - } - } - - // Returns true when the command should end. - @Override - public boolean isFinished() { - return false; - } -} \ No newline at end of file