Merge branch 'testRoboReveal' of https://github.com/Team4388/2022NoWayHome into testRoboReveal

src/main/java/frc4388/robot/RobotContainer.java

@@ -72,6 +72,7 @@ import frc4388.robot.Constants.OIConstants;
 import frc4388.robot.Constants.ShooterConstants;
 import frc4388.robot.Constants.StorageConstants;
 import frc4388.robot.Constants.SwerveDriveConstants;
+import frc4388.robot.commands.RunCommandForTime;
 import frc4388.robot.commands.ButtonBoxCommands.RunMiddleSwitch;
 import frc4388.robot.commands.ClimberCommands.RunClaw;
 import frc4388.robot.commands.ClimberCommands.RunClimberPath;
@@ -431,13 +432,6 @@ public class RobotContainer {
     //   }).withName("No Autonomous Path");
     // }
 
-    PIDController xController = SwerveDriveConstants.X_CONTROLLER;
-    PIDController yController = SwerveDriveConstants.Y_CONTROLLER;
-    ProfiledPIDController thetaController = SwerveDriveConstants.THETA_CONTROLLER;
-    // thetaController.enableContinuousInput(-Math.PI, Math.PI);
-
-    loadPath("Move Forward");
-
     // ! this will run each of the specified PathPlanner paths in sequence.
     // * return new SequentialCommandGroup(buildAuto(5.0, 5.0, "Path1", "Path2", "Path3"));
 
@@ -453,14 +447,22 @@ public class RobotContainer {
     //                                                       "Diamond"));
 
     // * assume turret is already pointed towards target.
-    return new SequentialCommandGroup( new InstantCommand(() -> m_robotSwerveDrive.resetGyro(), m_robotSwerveDrive),
-                                       new DriveWithInputForTime(m_robotSwerveDrive, new double[] {0.5, 0.5, 0.0, 0.0}, 1.0),
-                                       new ParallelRaceGroup(
-                                         new TrackTarget(m_robotTurret, m_robotBoomBoom, m_robotHood, m_robotVisionOdometry, true),
-                                         new RunCommand(() -> m_robotStorage.runStorage(StorageConstants.STORAGE_SPEED), m_robotStorage)
-                                       ));
+    // return new SequentialCommandGroup( new InstantCommand(() -> m_robotSwerveDrive.resetGyro(), m_robotSwerveDrive),
+    //                                    new DriveWithInputForTime(m_robotSwerveDrive, new double[] {0.5, 0.5, 0.0, 0.0}, 1.0),
+    //                                    new ParallelRaceGroup(
+    //                                      new TrackTarget(m_robotTurret, m_robotBoomBoom, m_robotHood, m_robotVisionOdometry, true),
+    //                                      new RunCommand(() -> m_robotStorage.runStorage(StorageConstants.STORAGE_SPEED), m_robotStorage)
+    //                                    ));
 
-    // TODO: we should test TrackTarget timing with my RunCommandForTime thing at some point, same with DriveWithInput timing
+    // return new RunCommandForTime(new RunCommand(() -> m_robotSwerveDrive.driveWithInput(0, 0, 0, true), m_robotSwerveDrive), 1.0, true);
+
+    return new SequentialCommandGroup( new InstantCommand(() -> m_robotSwerveDrive.resetGyro(), m_robotSwerveDrive),
+                                       new DriveWithInputForTime(m_robotSwerveDrive, new double[] {0.0, 0.3, 0.0, 0.0}, 1.0));//,
+                                      //  new TrackTarget(m_robotTurret, m_robotBoomBoom, m_robotHood, m_robotVisionOdometry, true),
+                                      //  new ParallelCommandGroup(
+                                      //    new TrackTarget(m_robotTurret, m_robotBoomBoom, m_robotHood, m_robotVisionOdometry, true),
+                                      //    new RunCommandForTime(new RunCommand(() -> m_robotStorage.runStorage(StorageConstants.STORAGE_SPEED), m_robotStorage), 1.0)
+                                       //));
 
     // * aim with RotateUntilTarget
     // return new SequentialCommandGroup( new InstantCommand(() -> m_robotSwerveDrive.resetGyro(), m_robotSwerveDrive),

src/main/java/frc4388/robot/commands/DriveCommands/DriveWithInputForTime.java

@@ -43,6 +43,7 @@ public class DriveWithInputForTime extends CommandBase {
   // Called every time the scheduler runs while the command is scheduled.
   @Override
   public void execute() {
+    System.out.println("RUNNING");
     elapsed = System.currentTimeMillis() - start;
     this.swerve.driveWithInput(inputs[0], inputs[1], inputs[2], inputs[3], true);
   }
@@ -54,6 +55,9 @@ public class DriveWithInputForTime extends CommandBase {
   // Returns true when the command should end.
   @Override
   public boolean isFinished() {
-    return (elapsed >= duration);
+    System.out.println("Duration: " + duration);
+    System.out.println("Elapsed: " + elapsed);
+
+    return ((double) elapsed >= duration);
   }
 }

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

@@ -0,0 +1,156 @@
+// 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 java.io.File;
+import java.io.IOException;
+import java.util.Comparator;
+import java.util.Map;
+import java.util.Optional;
+import java.util.function.Function;
+import java.util.logging.Level;
+import java.util.logging.Logger;
+import java.util.regex.Pattern;
+import java.util.stream.IntStream;
+
+import edu.wpi.first.wpilibj.Filesystem;
+import edu.wpi.first.wpilibj2.command.CommandBase;
+import frc4388.robot.subsystems.SwerveDrive;
+import frc4388.utility.CSV;
+
+public class PlaybackDriveInput extends CommandBase {
+  private static final Logger LOGGER = Logger.getLogger(PlaybackDriveInput.class.getSimpleName());
+
+  public static class DriveInputEntry {
+    public double millis, leftAxisX, leftAxisY, rightAxisX;
+  }
+
+  private DriveInputEntry[] driveInputEntries;
+
+  private SwerveDrive swerve;
+  private long startTime;
+
+  /** Creates a new PlaybackDriveInput. */
+  public PlaybackDriveInput(SwerveDrive swerve, String autoName) {
+    this.swerve = swerve;
+
+    try {
+      // This is a helper class that allows us to read a CSV file into a Java array.
+      CSV<DriveInputEntry> csv = new CSV<>(DriveInputEntry::new) {
+        // This is a regular expression that removes all parentheses from the header of the CSV file.
+        private final Pattern parentheses = Pattern.compile("\\([^\\)]*+\\)");
+
+        /**
+         * Removes the parentheses from the CSV header
+         * 
+         * @param header The header to be sanitized.
+         * @return The headerSanitizer method is overriding the headerSanitizer method in the parent class.
+         *         The parentheses.matcher(header) is matching the header with the parentheses regular
+         *         expression. The replaceAll method is replacing all of the parentheses with an empty
+         *         string. The super.headerSanitizer(parentheses.matcher(header).replaceAll("")) is calling
+         *         the parent sanitizer.
+         */
+        @Override
+        protected String headerSanitizer(final String header) {
+          return super.headerSanitizer(parentheses.matcher(header).replaceAll(""));
+        }
+      };
+      // This is reading the CSV file into a Java array.
+      driveInputEntries = csv.read(new File(Filesystem.getDeployDirectory(), autoName + ".csv").toPath());
+      // This is a running a helper method that is logging the contents of the table to the console on a new thread.
+      // ! new Thread(() -> LOGGER.fine(() -> CSV.ReflectionTable.create(driveInputEntries, RobotBase.isSimulation()))).start();
+    } catch (final IOException exception) {
+      DriveInputEntry dummyEntry = new DriveInputEntry();
+      dummyEntry.millis = 0.0;
+      dummyEntry.leftAxisX = 0.0;
+      dummyEntry.leftAxisY = 0.0;
+      dummyEntry.rightAxisX = 0.0;
+      driveInputEntries = new DriveInputEntry[] { dummyEntry };
+      LOGGER.log(Level.SEVERE, "Exception while reading shooter CSV table.", exception);
+    }
+  }
+
+  // Called when the command is initially scheduled.
+  @Override
+  public void initialize() {
+    startTime = System.currentTimeMillis();
+  }
+
+  // Called every time the scheduler runs while the command is scheduled.
+  @Override
+  public void execute() {
+    long currentTime = System.currentTimeMillis() - startTime;
+
+    double leftAxisX = linearInterpolate(driveInputEntries, currentTime, e -> e.millis, e -> e.leftAxisX).doubleValue();
+    double leftAxisY = linearInterpolate(driveInputEntries, currentTime, e -> e.millis, e -> e.leftAxisY).doubleValue();
+    double rightAxisX = linearInterpolate(driveInputEntries, currentTime, e -> e.millis, e -> e.rightAxisX).doubleValue();
+
+    swerve.driveWithInput(leftAxisX, leftAxisY, rightAxisX, true);
+  }
+
+  /**
+   * Using the given lookup value (x) and lookup getter function, locates the nearest entries in the
+   * given table to be used as the lower (x0) and upper (x1) bounds for interpolation. Returns the
+   * interpolation (y) between the two values (y0 and y1) accquired by applying the target getter
+   * function to the lower and upper bounds entries.
+   * 
+   * @param table An array of entries to search through.
+   * @param lookupValue The value to lookup in the table.
+   * @param lookupGetter A function that takes an entry from the table and returns .
+   * @param targetGetter A function that takes an E and returns a Number.
+   * @return The interpolated value.
+   */
+  private static <E> Number linearInterpolate(final E[] table, final Number lookupValue, final Function<E, Number> lookupGetter, final Function<E, Number> targetGetter) {
+    final Map.Entry<Integer, E> closestEntry = lookup(table, lookupValue.doubleValue(), lookupGetter, false).orElse(Map.entry(table.length - 1, table[table.length - 1]));
+    final E closestRecord = closestEntry.getValue();
+    final int closestRecordIndex = closestEntry.getKey();
+    final E neighborRecord = table[lookupValue.doubleValue() <= lookupGetter.apply(closestRecord).doubleValue() ? Math.max(closestRecordIndex == 0 ? 1 : 0, closestRecordIndex - 1) : Math.min(closestRecordIndex + 1, table.length - (closestRecordIndex == table.length - 1 ? 2 : 1))];
+    return lerp2(lookupValue, lookupGetter.apply(closestRecord), targetGetter.apply(closestRecord), lookupGetter.apply(neighborRecord), targetGetter.apply(neighborRecord));
+  }
+
+  /**
+   * If the value is in the table, return the entry. Otherwise, return the entry with the closest
+   * value
+   * 
+   * @param table The array of values to search.
+   * @param value The value to search for.
+   * @param valueGetter A function that takes an element of the table and returns a Number to compare
+   *        with the given value.
+   * @param exactMatch If true, the lookup will only return a match if the value is exactly equal to
+   *        the value of the entry. If false, the lookup will return a match with a value closest to
+   *        the given value.
+   * @return The entry with the closest value to the given value.
+   */
+  private static <E> Optional<Map.Entry<Integer, E>> lookup(final E[] table, final Number value, final Function<E, Number> valueGetter, final boolean exactMatch) {
+    final Optional<Map.Entry<Integer, E>> match = IntStream.range(0, table.length).mapToObj(i -> Map.entry(i, table[i])).min(Comparator.comparingDouble(e -> Math.abs(valueGetter.apply(e.getValue()).doubleValue() - value.doubleValue())));
+    return !exactMatch || match.map(e -> valueGetter.apply(e.getValue()).equals(value)).orElse(false) ? match : Optional.empty();
+  }
+
+  /**
+   * Given a value x, and two values x0 and x1, and two values y0 and y1, return a value y that is a
+   * linear interpolation of the two values y0 and y1
+   * 
+   * @param x The value to interpolate.
+   * @param x0 the x coordinate of the lower bound to interpolate to
+   * @param y0 The value at x0.
+   * @param x1 the x-coordinate of the upper bound to interpolate to
+   * @param y1 The value at x1.
+   * @return The interpolation between y0 and y1 at x.
+   */
+  private static Number lerp2(final Number x, final Number x0, final Number y0, final Number x1, final Number y1) {
+    final Number f = (x.doubleValue() - x0.doubleValue()) / (x1.doubleValue() - x0.doubleValue());
+    return (1.0 - f.doubleValue()) * y0.doubleValue() + f.doubleValue() * y1.doubleValue();
+  }
+
+  // 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 false;
+  }
+}

src/main/java/frc4388/robot/commands/RecordDriveInput.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;
+
+import java.io.File;
+import java.io.FileOutputStream;
+import java.io.PrintWriter;
+import java.util.HashMap;
+import java.util.function.Supplier;
+
+import edu.wpi.first.wpilibj2.command.CommandBase;
+import frc4388.robot.subsystems.SwerveDrive;
+
+public class RecordDriveInput extends CommandBase {
+  private HashMap<Long, double[]> timedInput;
+
+  private SwerveDrive swerve;
+
+  private Supplier<Double> leftAxisXSupplier;
+  private Supplier<Double> leftAxisYSupplier;
+  private Supplier<Double> rightAxisXSupplier;
+
+  private String autoName;
+
+  private long startTime;
+  private long count;
+
+  private int nthRecord;
+
+  /** Creates a new RecordDriveInput. */
+  public RecordDriveInput(
+        SwerveDrive swerve,
+        Supplier<Double> leftAxisXSupplier,
+        Supplier<Double> leftAxisYSupplier,
+        Supplier<Double> rightAxisXSupplier,
+        String autoName,
+        int nthRecord)
+  {
+    this.swerve = swerve;
+    this.leftAxisXSupplier = leftAxisXSupplier;
+    this.leftAxisYSupplier = leftAxisYSupplier;
+    this.rightAxisXSupplier = rightAxisXSupplier;
+    this.autoName = autoName;
+    this.nthRecord = nthRecord;
+
+    timedInput = new HashMap<>();
+
+    addRequirements(this.swerve);
+  }
+
+  public RecordDriveInput(
+        SwerveDrive swerve,
+        Supplier<Double> leftAxisXSupplier,
+        Supplier<Double> leftAxisYSupplier,
+        Supplier<Double> rightAxisXSupplier,
+        String autoName)
+  {
+    this(swerve, leftAxisXSupplier, leftAxisYSupplier, rightAxisXSupplier, autoName, 1);
+  }
+
+  // Called when the command is initially scheduled.
+  @Override
+  public void initialize() {
+    startTime = System.currentTimeMillis();
+    count = 0;
+
+    timedInput.put((long) 0, new double[] {0, 0, 0});
+  }
+
+  // Called every time the scheduler runs while the command is scheduled.
+  @Override
+  public void execute() {
+    long timeFromStart = System.currentTimeMillis() - startTime;
+    double[] input = {leftAxisXSupplier.get(), leftAxisYSupplier.get(), rightAxisXSupplier.get()};
+
+    if(count % (long) nthRecord == 0)
+      timedInput.put(timeFromStart, input);
+
+    swerve.driveWithInput(input[0], input[1], input[2], true);
+    count++;
+  }
+
+  // Called once the command ends or is interrupted.
+  @Override
+  public void end(boolean interrupted) {
+    File csvOutput = new File(autoName + ".csv");
+    try(PrintWriter writer = new PrintWriter(csvOutput)) {
+      writer.println("millis,leftx,lefty,rightx");
+
+      for(long millis : timedInput.keySet())
+        writer.println(millis + "," + timedInput.get(millis)[0] + "," + timedInput.get(millis)[1] + "," + timedInput.get(millis)[2]);
+
+      writer.println((millis + 1) + ",0,0,0");
+      writer.close();
+    } catch(Exception e) {
+      e.printStackTrace();
+    }
+  }
+
+  // Returns true when the command should end.
+  @Override
+  public boolean isFinished() {
+    return false;
+  }
+}

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

@@ -56,6 +56,7 @@ public class RunCommandForTime extends CommandBase {
   // Called every time the scheduler runs while the command is scheduled.
   @Override
   public void execute() {
+    System.out.println("RUNNING");
     this.elapsed = System.currentTimeMillis() - this.start;
     this.command.execute();
   }
@@ -70,6 +71,8 @@ public class RunCommandForTime extends CommandBase {
   @Override
   public boolean isFinished() {
     if (this.override) {
+      System.out.println("Duration: " + duration);
+      System.out.println("Elapsed: " + elapsed);
       return (this.elapsed >= this.duration);
     } else {
       return (this.command.isFinished() && (this.elapsed >= this.duration));

src/main/java/frc4388/robot/subsystems/BoomBoom.java

@@ -174,9 +174,9 @@ public class BoomBoom extends SubsystemBase {
   public void periodic() {
     // This method will be called once per scheduler run
     // speed2 = SmartDashboard.getNumber("Shooter Offset", 0.0);
-    SmartDashboard.putNumber("Shooter Current", getCurrent());
-    SmartDashboard.putNumber("Shooter Voltage",  m_shooterFalconLeft.getMotorOutputVoltage());
-    SmartDashboard.putNumber("Shooter Actual Velocity", m_shooterFalconLeft.getSelectedSensorVelocity());
+    // SmartDashboard.putNumber("Shooter Current", getCurrent());
+    // SmartDashboard.putNumber("Shooter Voltage",  m_shooterFalconLeft.getMotorOutputVoltage());
+    // SmartDashboard.putNumber("Shooter Actual Velocity", m_shooterFalconLeft.getSelectedSensorVelocity());
   }
 
   public void passRequiredSubsystem(Hood subsystem0, Turret subsystem1) {
@@ -191,9 +191,9 @@ public class BoomBoom extends SubsystemBase {
   public void runDrumShooter(double speed) {
     // m_shooterFalconLeft.set(TalonFXControlMode.PercentOutput, speed + speed2);
     m_shooterFalconLeft.set(speed);
-    SmartDashboard.putNumber("BoomBoom percent speed", speed + speed2);
-    SmartDashboard.putNumber("BoomBoom current stator", m_shooterFalconLeft.getStatorCurrent());
-    SmartDashboard.putNumber("BoomBoom current supply", m_shooterFalconLeft.getSupplyCurrent());
+    // SmartDashboard.putNumber("BoomBoom percent speed", speed + speed2);
+    // SmartDashboard.putNumber("BoomBoom current stator", m_shooterFalconLeft.getStatorCurrent());
+    // SmartDashboard.putNumber("BoomBoom current supply", m_shooterFalconLeft.getSupplyCurrent());
 
   }