diff --git a/src/main/java/frc4388/robot/commands/JoystickPlayback.java b/src/main/java/frc4388/robot/commands/JoystickPlayback.java index 7b3fedc..a15eb24 100644 --- a/src/main/java/frc4388/robot/commands/JoystickPlayback.java +++ b/src/main/java/frc4388/robot/commands/JoystickPlayback.java @@ -9,69 +9,36 @@ import java.io.FileNotFoundException; import java.io.IOException; import java.nio.file.Files; import java.nio.file.Path; -import java.sql.Time; -import java.util.ArrayList; import java.util.Scanner; import java.util.function.Supplier; -import edu.wpi.first.math.Pair; import edu.wpi.first.math.geometry.Translation2d; import edu.wpi.first.wpilibj2.command.CommandBase; import frc4388.robot.subsystems.SwerveDrive; import frc4388.utility.controller.DeadbandedXboxController; public class JoystickPlayback extends CommandBase { - private static class TimedOutput { - public double leftX = 0d; - public double leftY = 0d; - public double rightX = 0d; - public double rightY = 0d; - public long timed_offset = 0l; - } - - private final SwerveDrive m_swerve; - private Scanner m_input; - private final ArrayList m_outputs; - private long m_playback_time; - private int m_last_index; - private boolean m_finished = false; // ! find a better way + SwerveDrive swerve; + Scanner input; /** Creates a new JoystickPlayback. */ public JoystickPlayback(SwerveDrive swerve) { // Use addRequirements() here to declare subsystem dependencies. - this.m_swerve = swerve; - m_outputs = new ArrayList<>(); + this.swerve = swerve; - try { - m_input = new Scanner(new File("/home/lvuser/JoystickInputs.txt")); - - String line = ""; - while (m_input.hasNextLine()) { - line = m_input.nextLine(); - - String[] values = line.split(","); - - var out = new TimedOutput(); - out.leftX = Double.parseDouble(values[0]); - out.leftY = Double.parseDouble(values[1]); - out.rightX = Double.parseDouble(values[2]); - out.rightY = Double.parseDouble(values[3]); - - out.timed_offset = Long.MAX_VALUE; - - m_outputs.add(out); - } - } catch (FileNotFoundException e) { - e.printStackTrace(); - } - - addRequirements(this.m_swerve); + addRequirements(this.swerve); } // Called when the command is initially scheduled. @Override public void initialize() { + try { + input = new Scanner(new File("/home/lvuser/JoystickInputs.txt")); + } catch (FileNotFoundException e) { + e.printStackTrace(); + } + System.out.println("STARTING PLAYBACK"); System.out.println("STARTING PLAYBACK"); System.out.println("STARTING PLAYBACK"); @@ -81,39 +48,33 @@ public class JoystickPlayback extends CommandBase { // Called every time the scheduler runs while the command is scheduled. @Override public void execute() { - // 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 = m_last_index + 1; - while (i < m_outputs.size() && m_outputs.get(i).timed_offset < m_playback_time) { - i++; - } - if (i >= m_outputs.size()) { - m_finished = true; // ! kind of a hack - return; - } - m_last_index = i; + String line = ""; + if (input.hasNextLine()) { + line = input.nextLine(); } - TimedOutput out = m_outputs.get(m_last_index); + String[] values = line.split(","); + + double leftX = Double.parseDouble(values[0]); + double leftY = Double.parseDouble(values[1]); + double rightX = Double.parseDouble(values[2]); + double rightY = Double.parseDouble(values[3]); + + this.swerve.driveWithInput(new Translation2d(leftX, leftY), new Translation2d(-rightX, rightY), true); - this.m_swerve.driveWithInput(new Translation2d(out.leftX, out.leftY), - new Translation2d(-out.rightX, out.rightY), - true); System.out.println("PLAYING"); } // Called once the command ends or is interrupted. @Override public void end(boolean interrupted) { - m_input.close(); - m_swerve.stopModules(); + input.close(); } // Returns true when the command should end. @Override public boolean isFinished() { - return m_finished; + return false; } } diff --git a/src/main/java/frc4388/robot/subsystems/SwerveDrive.java b/src/main/java/frc4388/robot/subsystems/SwerveDrive.java index 9d73542..17bdd5b 100644 --- a/src/main/java/frc4388/robot/subsystems/SwerveDrive.java +++ b/src/main/java/frc4388/robot/subsystems/SwerveDrive.java @@ -156,12 +156,6 @@ public class SwerveDrive extends SubsystemBase { ); } - public void stopModules() { - for (SwerveModule module : this.modules) { - module.stop(); - } - } - /** * Resets the odometry of the SwerveDrive to 0. * *NOTE: If you reset your gyro, this method MUST be called with the new gyro angle and wheel encoder positions.