From 2316b1d4268eaa1ff61b6da9ebbc88978c3ba6d2 Mon Sep 17 00:00:00 2001 From: 66945 <54561572+66945@users.noreply.github.com> Date: Tue, 22 Mar 2022 14:32:56 -0600 Subject: [PATCH 1/3] Robot Rock auto thignbleljkefoijwejfoiwjfojfowjfkjkjvkjjfkwje --- .../robot/commands/PlaybackDriveInput.java | 156 ++++++++++++++++++ .../robot/commands/RecordDriveInput.java | 106 ++++++++++++ 2 files changed, 262 insertions(+) create mode 100644 src/main/java/frc4388/robot/commands/PlaybackDriveInput.java create mode 100644 src/main/java/frc4388/robot/commands/RecordDriveInput.java diff --git a/src/main/java/frc4388/robot/commands/PlaybackDriveInput.java b/src/main/java/frc4388/robot/commands/PlaybackDriveInput.java new file mode 100644 index 0000000..9827145 --- /dev/null +++ b/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 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 Number linearInterpolate(final E[] table, final Number lookupValue, final Function lookupGetter, final Function targetGetter) { + final Map.Entry 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 Optional> lookup(final E[] table, final Number value, final Function valueGetter, final boolean exactMatch) { + final Optional> 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; + } +} diff --git a/src/main/java/frc4388/robot/commands/RecordDriveInput.java b/src/main/java/frc4388/robot/commands/RecordDriveInput.java new file mode 100644 index 0000000..b5437e3 --- /dev/null +++ b/src/main/java/frc4388/robot/commands/RecordDriveInput.java @@ -0,0 +1,106 @@ +// 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 timedInput; + + private SwerveDrive swerve; + + private Supplier leftAxisXSupplier; + private Supplier leftAxisYSupplier; + private Supplier rightAxisXSupplier; + + private String autoName; + + private long startTime; + private long count; + + private int nthRecord; + + /** Creates a new RecordDriveInput. */ + public RecordDriveInput( + SwerveDrive swerve, + Supplier leftAxisXSupplier, + Supplier leftAxisYSupplier, + Supplier 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 leftAxisXSupplier, + Supplier leftAxisYSupplier, + Supplier 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.close(); + } catch(Exception e) { + e.printStackTrace(); + } + } + + // Returns true when the command should end. + @Override + public boolean isFinished() { + return false; + } +} \ No newline at end of file From 9e95856cbc723bb40112a8e32016e42bacb820d2 Mon Sep 17 00:00:00 2001 From: 66945 <54561572+66945@users.noreply.github.com> Date: Tue, 22 Mar 2022 16:44:01 -0600 Subject: [PATCH 2/3] stop at zero --- src/main/java/frc4388/robot/commands/RecordDriveInput.java | 1 + 1 file changed, 1 insertion(+) diff --git a/src/main/java/frc4388/robot/commands/RecordDriveInput.java b/src/main/java/frc4388/robot/commands/RecordDriveInput.java index b5437e3..19e27f3 100644 --- a/src/main/java/frc4388/robot/commands/RecordDriveInput.java +++ b/src/main/java/frc4388/robot/commands/RecordDriveInput.java @@ -92,6 +92,7 @@ public class RecordDriveInput extends CommandBase { 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(); From 81ef2a858ae64086b7dfda0edd3b2eff1884f88c Mon Sep 17 00:00:00 2001 From: aarav18 Date: Tue, 22 Mar 2022 16:56:40 -0600 Subject: [PATCH 3/3] tested RunCommandForTime + DWIForTime in robotsim, both work --- simgui.json | 1 + .../java/frc4388/robot/RobotContainer.java | 32 ++++++++++--------- .../DriveCommands/DriveWithInputForTime.java | 6 +++- .../robot/commands/RunCommandForTime.java | 3 ++ .../frc4388/robot/subsystems/BoomBoom.java | 12 +++---- 5 files changed, 32 insertions(+), 22 deletions(-) diff --git a/simgui.json b/simgui.json index f3617f0..2a797b5 100644 --- a/simgui.json +++ b/simgui.json @@ -22,6 +22,7 @@ "types": { "/FMSInfo": "FMSInfo", "/LiveWindow/BoomBoom": "Subsystem", + "/LiveWindow/Claws": "Subsystem", "/LiveWindow/Climber": "Subsystem", "/LiveWindow/Extender": "Subsystem", "/LiveWindow/Hood": "Subsystem", diff --git a/src/main/java/frc4388/robot/RobotContainer.java b/src/main/java/frc4388/robot/RobotContainer.java index 9bd41dc..ee57648 100644 --- a/src/main/java/frc4388/robot/RobotContainer.java +++ b/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; @@ -226,7 +227,7 @@ public class RobotContainer { }, m_robotClimber)); m_robotBoomBoom.setDefaultCommand( - new RunCommand(() -> m_robotBoomBoom.runDrumShooter(0.45)) + new RunCommand(() -> m_robotBoomBoom.runDrumShooter(0.45), m_robotBoomBoom) ); // autoInit(); @@ -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), diff --git a/src/main/java/frc4388/robot/commands/DriveCommands/DriveWithInputForTime.java b/src/main/java/frc4388/robot/commands/DriveCommands/DriveWithInputForTime.java index 6f436b9..a2ce001 100644 --- a/src/main/java/frc4388/robot/commands/DriveCommands/DriveWithInputForTime.java +++ b/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); } } diff --git a/src/main/java/frc4388/robot/commands/RunCommandForTime.java b/src/main/java/frc4388/robot/commands/RunCommandForTime.java index 5e35682..b66947d 100644 --- a/src/main/java/frc4388/robot/commands/RunCommandForTime.java +++ b/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)); diff --git a/src/main/java/frc4388/robot/subsystems/BoomBoom.java b/src/main/java/frc4388/robot/subsystems/BoomBoom.java index 8e8c334..82eb661 100644 --- a/src/main/java/frc4388/robot/subsystems/BoomBoom.java +++ b/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()); }