From 603be6538bfce3ccd231e969591db93e89a9b616 Mon Sep 17 00:00:00 2001 From: Abhi <90010729+Abhishrek05@users.noreply.github.com> Date: Tue, 14 Feb 2023 19:00:09 -0700 Subject: [PATCH] Updated Auto recording --- .../java/frc4388/robot/RobotContainer.java | 71 ++++--------------- .../robot/commands/JoystickPlayback.java | 24 +------ .../robot/commands/JoystickRecorder.java | 65 +++++++++-------- .../java/frc4388/utility/UtilityStructs.java | 12 ++++ 4 files changed, 61 insertions(+), 111 deletions(-) create mode 100644 src/main/java/frc4388/utility/UtilityStructs.java diff --git a/src/main/java/frc4388/robot/RobotContainer.java b/src/main/java/frc4388/robot/RobotContainer.java index aa94317..731275b 100644 --- a/src/main/java/frc4388/robot/RobotContainer.java +++ b/src/main/java/frc4388/robot/RobotContainer.java @@ -7,9 +7,6 @@ package frc4388.robot; -import javax.print.attribute.standard.OrientationRequested; - -import edu.wpi.first.wpilibj.Joystick; import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.InstantCommand; import edu.wpi.first.wpilibj2.command.RunCommand; @@ -20,7 +17,6 @@ import frc4388.robot.commands.JoystickPlayback; import frc4388.robot.commands.JoystickRecorder; import frc4388.robot.subsystems.SwerveDrive; import frc4388.utility.controller.DeadbandedXboxController; -import frc4388.utility.controller.IHandController; import frc4388.utility.controller.XboxController; /** @@ -35,14 +31,13 @@ public class RobotContainer { public final RobotMap m_robotMap = new RobotMap(); /* Subsystems */ - public final SwerveDrive m_robotSwerveDrive = new SwerveDrive(m_robotMap.leftFront, m_robotMap.rightFront, m_robotMap.leftBack, m_robotMap.rightBack, m_robotMap.gyro); - // private final LED m_robotLED = new LED(m_robotMap.LEDController); - + public final SwerveDrive m_robotSwerveDrive = new SwerveDrive(m_robotMap.leftFront, + m_robotMap.rightFront, + m_robotMap.leftBack, + m_robotMap.rightBack, + m_robotMap.gyro); /* Controllers */ - // private final XboxController m_driverXbox = new XboxController(OIConstants.XBOX_DRIVER_ID); - // private final XboxController m_operatorXbox = new XboxController(OIConstants.XBOX_OPERATOR_ID); - private final DeadbandedXboxController m_driverXbox = new DeadbandedXboxController(OIConstants.XBOX_DRIVER_ID); private final DeadbandedXboxController m_operatorXbox = new DeadbandedXboxController(OIConstants.XBOX_OPERATOR_ID); @@ -52,13 +47,13 @@ public class RobotContainer { public RobotContainer() { configureButtonBindings(); - /* Default Commands */ - - m_robotSwerveDrive.setDefaultCommand(new RunCommand(() -> - m_robotSwerveDrive.driveWithInput(getDeadbandedDriverController().getLeft(), getDeadbandedDriverController().getRight(), true) - , m_robotSwerveDrive).withName("SwerveDrive DefaultCommand")); - - // m_robotLED.setDefaultCommand(new RunCommand(() -> m_robotLED.updateLED(), m_robotLED)); + // * Default Commands + m_robotSwerveDrive.setDefaultCommand(new RunCommand(() -> { + m_robotSwerveDrive.driveWithInput(getDeadbandedDriverController().getLeft(), + getDeadbandedDriverController().getRight(), + true); + }, m_robotSwerveDrive) + .withName("SwerveDrive DefaultCommand")); } @@ -69,14 +64,12 @@ public class RobotContainer { * passing it to a {@link edu.wpi.first.wpilibj2.command.button.JoystickButton}. */ private void configureButtonBindings() { - /* Driver Buttons */ - + // * Driver Buttons new JoystickButton(getDeadbandedDriverController(), XboxController.A_BUTTON) .onTrue(new InstantCommand(() -> m_robotSwerveDrive.resetGyro(), m_robotSwerveDrive)); new JoystickButton(getDeadbandedDriverController(), XboxController.X_BUTTON) .onTrue(new InstantCommand(() -> m_robotSwerveDrive.resetOdometry(), m_robotSwerveDrive)); - // // .onFalse() new JoystickButton(getDeadbandedDriverController(), XboxController.Y_BUTTON) .onTrue(new AutoBalance(m_robotMap.gyro, m_robotSwerveDrive)); @@ -86,18 +79,13 @@ public class RobotContainer { () -> getDeadbandedDriverController().getLeftX(), () -> getDeadbandedDriverController().getLeftY(), () -> getDeadbandedDriverController().getRightX(), - () -> getDeadbandedDriverController().getRightY() - - )) + () -> getDeadbandedDriverController().getRightY())) .onFalse(new InstantCommand()); new JoystickButton(getDeadbandedDriverController(), XboxController.LEFT_BUMPER_BUTTON) .onTrue(new JoystickPlayback(m_robotSwerveDrive)); - // /* Operator Buttons */ - // // interrupt button - // new JoystickButton(getOperatorJoystick(), XboxController.X_BUTTON) - // .onTrue(new InstantCommand()); + // * Operator Buttons } /** @@ -106,17 +94,9 @@ public class RobotContainer { * @return the command to run in autonomous */ public Command getAutonomousCommand() { - return new InstantCommand(); } - /** - * Add your docs here. - */ - // public IHandController getDriverController() { - // return m_driverXbox; - // } - public DeadbandedXboxController getDeadbandedDriverController() { return this.m_driverXbox; } @@ -124,25 +104,4 @@ public class RobotContainer { public DeadbandedXboxController getDeadbandedOperatorController() { return this.m_operatorXbox; } - - /** - * Add your docs here. - */ - // public IHandController getOperatorController() { - // return m_operatorXbox; - // } - - /** - * Add your docs here. - */ - // public Joystick getOperatorJoystick() { - // return m_operatorXbox.getJoyStick(); - // } - - /** - * Add your docs here. - */ - // public Joystick getDriverJoystick() { - // return m_driverXbox.getJoyStick(); - // } } diff --git a/src/main/java/frc4388/robot/commands/JoystickPlayback.java b/src/main/java/frc4388/robot/commands/JoystickPlayback.java index 4240ecf..6961d09 100644 --- a/src/main/java/frc4388/robot/commands/JoystickPlayback.java +++ b/src/main/java/frc4388/robot/commands/JoystickPlayback.java @@ -6,34 +6,17 @@ package frc4388.robot.commands; import java.io.File; import java.io.FileNotFoundException; -import java.io.IOException; -import java.io.PrintWriter; -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; +import frc4388.utility.UtilityStructs.TimedOutput; public class JoystickPlayback extends CommandBase { - private static class TimedOutput { - public double leftX = 0.0; - public double leftY = 0.0; - public double rightX = 0.0; - public double rightY = 0.0; - - public long timedOffset = 0; - } - private final SwerveDrive swerve; private Scanner input; - private final ArrayList outputs; + private final ArrayList outputs = new ArrayList<>(); private int counter = 0; private long startTime = 0; private long playbackTime = 0; @@ -42,10 +25,7 @@ public class JoystickPlayback extends CommandBase { /** Creates a new JoystickPlayback. */ public JoystickPlayback(SwerveDrive swerve) { - // Use addRequirements() here to declare subsystem dependencies. this.swerve = swerve; - outputs = new ArrayList<>(); - addRequirements(this.swerve); } diff --git a/src/main/java/frc4388/robot/commands/JoystickRecorder.java b/src/main/java/frc4388/robot/commands/JoystickRecorder.java index c13ea6c..2b3fd24 100644 --- a/src/main/java/frc4388/robot/commands/JoystickRecorder.java +++ b/src/main/java/frc4388/robot/commands/JoystickRecorder.java @@ -13,31 +13,28 @@ import java.util.function.Supplier; import edu.wpi.first.math.geometry.Translation2d; import edu.wpi.first.wpilibj2.command.CommandBase; import frc4388.robot.subsystems.SwerveDrive; +import frc4388.utility.UtilityStructs.TimedOutput; public class JoystickRecorder extends CommandBase { - - SwerveDrive swerve; + public final SwerveDrive swerve; - Supplier leftXSupplier; - Supplier leftYSupplier; - Supplier rightXSupplier; - Supplier rightYSupplier; - - ArrayList outputs; - - private long startTime; + public final Supplier leftX; + public final Supplier leftY; + public final Supplier rightX; + public final Supplier rightY; + public final ArrayList outputs = new ArrayList<>(); + private long startTime = -1; /** Creates a new JoystickRecorder. */ - public JoystickRecorder(SwerveDrive swerve, Supplier leftXSupplier, Supplier leftYSupplier, Supplier rightXSupplier, Supplier rightYSupplier) { - // Use addRequirements() here to declare subsystem dependencies. + public JoystickRecorder(SwerveDrive swerve, Supplier leftX, Supplier leftY, + Supplier rightX, Supplier rightY) + { this.swerve = swerve; - this.leftXSupplier = leftXSupplier; - this.leftYSupplier = leftYSupplier; - this.rightXSupplier = rightXSupplier; - this.rightYSupplier = rightYSupplier; - - this.outputs = new ArrayList(); + this.leftX = leftX; + this.leftY = leftY; + this.rightX = rightX; + this.rightY = rightY; addRequirements(this.swerve); } @@ -47,21 +44,24 @@ public class JoystickRecorder extends CommandBase { public void initialize() { this.startTime = System.currentTimeMillis(); - // timedInput.put((long) 0, new double[] {0.0, 0.0, 0.0, 0.0}); - outputs.add(new Object[] {(double) 0.0, (double) 0.0, (double) 0.0, (double) 0.0, (long) 0}); - - System.out.println("STARTING RECORDING"); + outputs.add(new TimedOutput()); } // Called every time the scheduler runs while the command is scheduled. @Override public void execute() { - Object[] inputs = new Object[] {(double) leftXSupplier.get(), (double) leftYSupplier.get(), (double) rightXSupplier.get(), (double) rightYSupplier.get(), (long) (System.currentTimeMillis() - startTime)}; + 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.driveWithInput(new Translation2d((double) inputs[0], (double) inputs[1]), new Translation2d((double) inputs[2], (double) inputs[3]), true); - - System.out.println("RECORDING"); + swerve.driveWithInput(new Translation2d(inputs.leftX, inputs.leftY), + new Translation2d(inputs.rightX, inputs.rightY), + true); } // Called once the command ends or is interrupted. @@ -69,18 +69,17 @@ public class JoystickRecorder extends CommandBase { public void end(boolean interrupted) { File output = new File("/home/lvuser/JoystickInputs.txt"); - try(PrintWriter writer = new PrintWriter(output)) { - - for(Object[] input : outputs) { - writer.println(input[0] + "," + input[1] + "," + input[2] + "," + input[3] + "," + input[4]); + 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) { + } catch (IOException e) { e.printStackTrace(); } - - System.out.println("STOPPED RECORDING"); } // Returns true when the command should end. diff --git a/src/main/java/frc4388/utility/UtilityStructs.java b/src/main/java/frc4388/utility/UtilityStructs.java new file mode 100644 index 0000000..e8b10cc --- /dev/null +++ b/src/main/java/frc4388/utility/UtilityStructs.java @@ -0,0 +1,12 @@ +package frc4388.utility; + +public class UtilityStructs { + public static class TimedOutput { + public double leftX = 0.0; + public double leftY = 0.0; + public double rightX = 0.0; + public double rightY = 0.0; + + public long timedOffset = 0; + } +}