diff --git a/src/main/java/frc4388/robot/RobotContainer.java b/src/main/java/frc4388/robot/RobotContainer.java index 99f5cb1..23c1b0f 100644 --- a/src/main/java/frc4388/robot/RobotContainer.java +++ b/src/main/java/frc4388/robot/RobotContainer.java @@ -13,6 +13,8 @@ import edu.wpi.first.wpilibj2.command.RunCommand; import edu.wpi.first.wpilibj2.command.button.JoystickButton; import frc4388.robot.Constants.*; +import frc4388.robot.commands.Swerve.JoystickPlayback; +import frc4388.robot.commands.Swerve.JoystickRecorder; import frc4388.robot.subsystems.LED; import frc4388.utility.LEDPatterns; import frc4388.utility.controller.IHandController; @@ -57,6 +59,18 @@ public RobotContainer() { private void configureButtonBindings() { /* Driver Buttons */ // test command to spin the robot while pressing A on the driver controller + // new JoystickButton(getDeadbandedDriverController(), XboxController.RIGHT_BUMPER_BUTTON) + // .whileTrue(new JoystickRecorder(m_robotSwerveDrive, + // () -> getDeadbandedDriverController().getLeftX(), + // () -> getDeadbandedDriverController().getLeftY(), + // () -> getDeadbandedDriverController().getRightX(), + // () -> getDeadbandedDriverController().getRightY(), + // "Blue1Path.txt")) + // .onFalse(new InstantCommand()); + + // new JoystickButton(getDeadbandedDriverController(), XboxController.LEFT_BUMPER_BUTTON) + // .onTrue(new JoystickPlayback(m_robotSwerveDrive, "Blue1Path.txt")) + // .onFalse(new InstantCommand()); /* Operator Buttons */ // activates "Lit Mode" diff --git a/src/main/java/frc4388/robot/commands/Autos/PlaybackChooser.java b/src/main/java/frc4388/robot/commands/Autos/PlaybackChooser.java new file mode 100644 index 0000000..2438a38 --- /dev/null +++ b/src/main/java/frc4388/robot/commands/Autos/PlaybackChooser.java @@ -0,0 +1,91 @@ +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(); + + 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++); + + for (String auto : m_dir.list()) { + 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/Autos/Taxi.txt b/src/main/java/frc4388/robot/commands/Autos/Taxi.txt new file mode 100644 index 0000000..c99ee2c --- /dev/null +++ b/src/main/java/frc4388/robot/commands/Autos/Taxi.txt @@ -0,0 +1,225 @@ +0.0,0.0,0.0,0.0,0 +0.0,0.0,0.0,0.0,0 +0.0,0.0,0.0,0.0,12 +0.0,0.0,0.0,0.0,26 +0.0,0.0,0.0,0.0,37 +0.0,0.0,0.0,0.0,50 +0.0,0.0,0.0,0.0,62 +0.0,0.0,0.0,0.0,73 +0.0,0.0,0.0,0.0,88 +0.0,0.0,0.0,0.0,103 +0.0,0.0,0.0,0.0,116 +0.0,0.0,0.0,0.0,160 +0.0,0.0,0.0,0.0,173 +0.0,0.0,0.0,0.0,185 +0.0,0.0,0.0,0.0,200 +0.0,0.0,0.0,0.0,211 +0.0,0.0,0.0,0.0,223 +0.0,0.0,0.0,0.0,235 +0.0,0.0,0.0,0.0,247 +0.0,0.0,0.0,0.0,263 +0.0,0.0,0.0,0.0,283 +0.0,0.0,0.0,0.0,303 +0.0,-0.109375,0.0,0.0,323 +0.0,-0.1484375,0.0,0.0,343 +0.0,-0.2109375,0.0,0.0,363 +0.0,-0.3671875,0.0,0.0,398 +0.0,-0.4140625,0.0,0.0,411 +0.0,-0.4765625,0.0,0.0,425 +0.0,-0.5078125,0.0,0.0,443 +0.0,-0.5078125,0.0,0.0,463 +0.0,-0.53125,0.0,0.0,483 +0.0,-0.5546875,0.0,0.0,504 +0.0,-0.5625,0.0,0.0,523 +0.0,-0.5625,0.0,0.0,544 +0.0,-0.5703125,0.0,0.0,563 +0.0,-0.5859375,0.0,0.0,584 +0.0,-0.5859375,0.0,0.0,603 +0.0,-0.5859375,0.0,0.0,640 +0.0,-0.59375,0.0,0.0,657 +0.0,-0.6015625,0.0,0.0,672 +0.0,-0.6015625,0.0,0.0,685 +0.0,-0.6015625,0.0,0.0,703 +0.0,-0.6015625,0.0,0.0,723 +0.0,-0.6015625,0.0,0.0,743 +0.0,-0.6015625,0.0,0.0,763 +0.0,-0.6015625,0.0,0.0,783 +0.0,-0.6015625,0.0,0.0,803 +0.0,-0.6015625,0.0,0.0,823 +0.0,-0.6015625,0.0,0.0,844 +0.0,-0.6015625,0.0,0.0,878 +0.0,-0.6015625,0.0,0.0,893 +0.0,-0.6015625,0.0,0.0,907 +0.0,-0.6015625,0.0,0.0,924 +0.0,-0.609375,0.0,0.0,943 +0.0,-0.609375,0.0,0.0,963 +0.0,-0.609375,0.0,0.0,983 +0.0,-0.609375,0.0,0.0,1004 +0.0,-0.609375,0.0,0.0,1023 +0.0,-0.609375,0.0,0.0,1043 +0.0,-0.609375,0.0,0.0,1064 +0.0,-0.609375,0.0,0.0,1083 +0.0,-0.609375,0.0,0.0,1156 +0.0,-0.609375,0.0,0.0,1172 +0.0,-0.609375,0.0,0.0,1185 +0.0,-0.609375,0.0,0.0,1200 +0.0,-0.609375,0.0,0.0,1215 +0.0,-0.609375,0.0,0.0,1225 +0.0,-0.609375,0.0,0.0,1236 +0.0,-0.609375,0.0,0.0,1249 +0.0,-0.609375,0.0,0.0,1263 +0.0,-0.609375,0.0,0.0,1283 +0.0,-0.609375,0.0,0.0,1303 +0.0,-0.609375,0.0,0.0,1323 +0.0,-0.609375,0.0,0.0,1363 +0.0,-0.6015625,0.0,0.0,1376 +0.0,-0.6015625,0.0,0.0,1394 +0.0,-0.6015625,0.0,0.0,1405 +0.0,-0.6015625,0.0,0.0,1423 +0.0,-0.6015625,0.0,0.0,1443 +0.0,-0.6015625,0.0,0.0,1463 +0.0,-0.6015625,0.0,0.0,1483 +0.0,-0.6015625,0.0,0.0,1503 +0.0,-0.6015625,0.0,0.0,1523 +0.0,-0.6015625,0.0,0.0,1543 +0.0,-0.6015625,0.0,0.0,1563 +0.0,-0.6015625,0.0,0.0,1597 +0.0,-0.6015625,0.0,0.0,1608 +0.0,-0.6015625,0.0,0.0,1624 +0.0,-0.6015625,0.0,0.0,1643 +0.0,-0.6015625,0.0,0.0,1664 +0.0,-0.5859375,0.0,0.0,1683 +0.0,-0.5859375,0.0,0.0,1703 +0.0,-0.5625,0.0,0.0,1723 +0.0,-0.5625,0.0,0.0,1743 +0.0,-0.5625,0.0,0.0,1763 +0.0,-0.5625,0.0,0.0,1783 +0.0,-0.5625,0.0,0.0,1803 +0.0,-0.5625,0.0,0.0,1843 +0.0,-0.5625,0.0,0.0,1855 +0.0,-0.5625,0.0,0.0,1868 +0.0,-0.5625,0.0,0.0,1883 +0.0,-0.5625,0.0,0.0,1903 +0.0,-0.5625,0.0,0.0,1923 +0.0,-0.5625,0.0,0.0,1943 +0.0,-0.5625,0.0,0.0,1963 +0.0,-0.5625,0.0,0.0,1983 +0.0,-0.5625,0.0,0.0,2003 +0.0,-0.5625,0.0,0.0,2024 +0.0,-0.5625,0.0,0.0,2043 +0.0,-0.5625,0.0,0.0,2081 +0.0,-0.5625,0.0,0.0,2093 +0.0,-0.5625,0.0,0.0,2105 +0.0,-0.5625,0.0,0.0,2123 +0.0,-0.5625,0.0,0.0,2143 +0.0,-0.5625,0.0,0.0,2163 +0.0,-0.5625,0.0,0.0,2183 +0.0,-0.5625,0.0,0.0,2203 +0.0,-0.5625,0.0,0.0,2223 +0.0,-0.5625,0.0,0.0,2243 +0.0,-0.5625,0.0,0.0,2263 +0.0,-0.5625,0.0,0.0,2283 +0.0,-0.5625,0.0,0.0,2366 +0.0,-0.5625,0.0,0.0,2377 +0.0,-0.5625,0.0,0.0,2394 +0.0,-0.5703125,0.0,0.0,2405 +0.0,-0.5703125,0.0,0.0,2418 +0.0,-0.5703125,0.0,0.0,2431 +0.0,-0.5703125,0.0,0.0,2444 +0.0,-0.5703125,0.0,0.0,2458 +0.0,-0.5703125,0.0,0.0,2470 +0.0,-0.5703125,0.0,0.0,2485 +0.0,-0.5703125,0.0,0.0,2503 +0.0,-0.5703125,0.0,0.0,2523 +0.0,-0.5703125,0.0,0.0,2563 +0.0,-0.5703125,0.0,0.0,2577 +0.0,-0.5703125,0.0,0.0,2591 +0.0,-0.5703125,0.0,0.0,2608 +0.0,-0.5703125,0.0,0.0,2624 +0.0,-0.5703125,0.0,0.0,2643 +0.0,-0.5703125,0.0,0.0,2677 +0.0,-0.5703125,0.0,0.0,2698 +0.0,-0.5703125,0.0,0.0,2711 +0.0,-0.5703125,0.0,0.0,2725 +0.0,-0.5703125,0.0,0.0,2743 +0.0,-0.5703125,0.0,0.0,2764 +0.0,-0.5703125,0.0,0.0,2810 +0.0,-0.5703125,0.0,0.0,2820 +0.0,-0.5703125,0.0,0.0,2833 +0.0,-0.5703125,0.0,0.0,2845 +0.0,-0.5703125,0.0,0.0,2863 +0.0,-0.5703125,0.0,0.0,2883 +0.0,-0.5703125,0.0,0.0,2904 +0.0,-0.5703125,0.0,0.0,2924 +0.0,-0.5703125,0.0,0.0,2943 +0.0,-0.5703125,0.0,0.0,2963 +0.0,-0.5703125,0.0,0.0,2983 +0.0,-0.5703125,0.0,0.0,3003 +0.0,-0.5703125,0.0,0.0,3033 +0.0,-0.5703125,0.0,0.0,3050 +0.0,-0.5703125,0.0,0.0,3065 +0.0,-0.5703125,0.0,0.0,3083 +0.0,-0.5703125,0.0,0.0,3103 +0.0,-0.5703125,0.0,0.0,3123 +0.0,-0.5703125,0.0,0.0,3144 +0.0,-0.5703125,0.0,0.0,3164 +0.0,-0.5703125,0.0,0.0,3184 +0.0,-0.5703125,0.0,0.0,3203 +0.0,-0.5703125,0.0,0.0,3223 +0.0,-0.5703125,0.0,0.0,3243 +0.0,-0.5703125,0.0,0.0,3272 +0.0,-0.5703125,0.0,0.0,3289 +0.0,-0.5703125,0.0,0.0,3303 +0.0,-0.5703125,0.0,0.0,3323 +0.0,-0.5703125,0.0,0.0,3343 +0.0,-0.5703125,0.0,0.0,3363 +0.0,-0.5703125,0.0,0.0,3383 +0.0,-0.5703125,0.0,0.0,3403 +0.0,-0.5703125,0.0,0.0,3423 +0.0,-0.5703125,0.0,0.0,3443 +0.0,-0.5703125,0.0,0.0,3463 +0.0,-0.5703125,0.0,0.0,3483 +0.0,-0.5703125,0.0,0.0,3566 +0.0,-0.5703125,0.0,0.0,3578 +0.0,-0.5703125,0.0,0.0,3596 +0.0,-0.5703125,0.0,0.0,3610 +0.0,-0.5703125,0.0,0.0,3623 +0.0,-0.5703125,0.0,0.0,3640 +0.0,-0.5703125,0.0,0.0,3651 +0.0,-0.5703125,0.0,0.0,3663 +0.0,-0.5703125,0.0,0.0,3678 +0.0,-0.5703125,0.0,0.0,3691 +0.0,-0.5703125,0.0,0.0,3706 +0.0,-0.5703125,0.0,0.0,3723 +0.0,-0.5703125,0.0,0.0,3766 +0.0,-0.5703125,0.0,0.0,3778 +0.0,-0.5703125,0.0,0.0,3792 +0.0,-0.5703125,0.0,0.0,3807 +0.0,-0.5703125,0.0,0.0,3823 +0.0,-0.5703125,0.0,0.0,3843 +0.0,-0.53125,0.0,0.0,3863 +0.0,-0.53125,0.0,0.0,3884 +0.0,-0.421875,0.0,0.0,3904 +0.0,0.0,0.0,0.0,3924 +0.0,0.0,0.0,0.0,3944 +0.0,0.0,0.0,0.0,3963 +0.0,0.0,0.0,0.0,3999 +0.0,0.0,0.0,0.0,4010 +0.0,0.0,0.0,0.0,4025 +0.0,0.0,0.0,0.0,4043 +0.0,0.0,0.0,0.0,4063 +0.0,0.0,0.0,0.0,4083 +0.0,0.0,0.0,0.0,4103 +0.0,0.0,0.0,0.0,4123 +0.0,0.0,0.0,0.0,4143 +0.0,0.0,0.0,0.0,4163 +0.0,0.0,0.0,0.0,4183 +0.0,0.0,0.0,0.0,4203 +0.0,0.0,0.0,0.0,4236 +0.0,0.0,0.0,0.0,4247 +0.0,0.0,0.0,0.0,4264 +0.0,0.0,0.0,0.0,4284 +0.0,0.0,0.0,0.0,4304 +0.0,0.0,0.0,0.0,4324 +0.0,0.0,0.0,0.0,4343 +0.0,0.0,0.0,0.0,4363 \ No newline at end of file diff --git a/src/main/java/frc4388/robot/commands/PID.java b/src/main/java/frc4388/robot/commands/PID.java new file mode 100644 index 0000000..c592c73 --- /dev/null +++ b/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.CommandBase; +import frc4388.utility.Gains; + +public abstract class PID extends CommandBase { + 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(); + + /** figure it out bitch */ + 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; + } +} diff --git a/src/main/java/frc4388/robot/commands/Swerve/JoystickPlayback.java b/src/main/java/frc4388/robot/commands/Swerve/JoystickPlayback.java new file mode 100644 index 0000000..56e8d43 --- /dev/null +++ b/src/main/java/frc4388/robot/commands/Swerve/JoystickPlayback.java @@ -0,0 +1,141 @@ +// 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.CommandBase; +import frc4388.robot.subsystems.SwerveDrive; +import frc4388.utility.UtilityStructs.TimedOutput; + +public class JoystickPlayback extends CommandBase { + 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); + + 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; + } +} diff --git a/src/main/java/frc4388/robot/commands/Swerve/JoystickRecorder.java b/src/main/java/frc4388/robot/commands/Swerve/JoystickRecorder.java new file mode 100644 index 0000000..84608cc --- /dev/null +++ b/src/main/java/frc4388/robot/commands/Swerve/JoystickRecorder.java @@ -0,0 +1,97 @@ +// 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.CommandBase; +import frc4388.robot.subsystems.SwerveDrive; +import frc4388.utility.UtilityStructs.TimedOutput; + +public class JoystickRecorder extends CommandBase { + 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.driveWithInput(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; + } +} diff --git a/src/main/java/frc4388/robot/commands/Swerve/RotateToAngle.java b/src/main/java/frc4388/robot/commands/Swerve/RotateToAngle.java new file mode 100644 index 0000000..a2945c0 --- /dev/null +++ b/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.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); + } +}