Team4388/2022NoWayHome
1
0
Fork 0
mirror of https://github.com/Team4388/2022NoWayHome.git synced 2026-06-09 08:48:07 -06:00
Code Issues Packages Projects Releases Wiki Activity

Add documentation and comments

Browse Source 
Fix invalid recording file names on Windows
Avoid null pointers with a dummy entry on shooter table load failures
This commit is contained in:
nathanrsxtn
2022-03-03 23:56:08 -07:00
parent d9b6eb4593
commit d6599dca70
4 changed files with 107 additions and 16 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/main/java/frc4388/robot/RobotContainer.java
+17 -5
View File
@@ -101,9 +101,10 @@ public class RobotContainer {
private final NetworkTableInstance networkTableInstance = NetworkTableInstance.getDefault();
private final NetworkTable recordingNetworkTable = networkTableInstance.getTable("Recording");
private static final DateTimeFormatter RECORDING_FILE_NAME_FORMATTER = DateTimeFormatter.ofPattern("'Recording' yyyy-MM-dd HH:mm:ss.SSS'.path'");
private static final DateTimeFormatter RECORDING_FILE_NAME_FORMATTER = DateTimeFormatter.ofPattern("'Recording' yyyy-MM-dd HH-mm-ss.SSS'.path'");
private static final Clock SYSTEM_CLOCK = Clock.system(ZoneId.systemDefault());
private static final Path PATHPLANNER_DIRECTORY = Filesystem.getDeployDirectory().toPath().resolve("pathplanner");
// Function that removes the ".path" from the end of a string.
private static final Function<CharSequence, String> PATH_EXTENSION_REMOVER = ((Function<CharSequence, Matcher>) Pattern.compile(".path")::matcher).andThen(m -> m.replaceFirst(""));
/**
@@ -224,15 +225,17 @@ public class RobotContainer {
return m_operatorXbox;
}
/**
* Creates a WatchKey for the path planner directory and registers it with the WatchService.
* Then creates a NotifierCommand that will update the auto chooser with the latest path files.
* Finally, adds the existing path files to the auto chooser
*/
private void autoInit() {
try {
WatchKey watchKey = PATHPLANNER_DIRECTORY.register(FileSystems.getDefault().newWatchService(), StandardWatchEventKinds.ENTRY_CREATE, StandardWatchEventKinds.ENTRY_MODIFY, StandardWatchEventKinds.ENTRY_DELETE);
// TODO: Store this and other commands as fields so they can be rescheduled.
new NotifierCommand(() -> updateAutoChooser(watchKey), 0.5) {
@Override
public boolean runsWhenDisabled() {
return true;
}
@Override public boolean runsWhenDisabled() { return true; }
}.withName("Path Watcher").schedule();
} catch (IOException exception) {
LOGGER.log(Level.SEVERE, "Exception with path file watcher.", exception);
@@ -243,6 +246,9 @@ public class RobotContainer {
SmartDashboard.putData("Auto Chooser", autoChooser);
}
/**
* Creates a button on the SmartDashboard that will record the path of the robot.
*/
public void recordInit() {
SmartDashboard.putData("Recording",
new RunCommand(this::recordPeriodic) {
@@ -258,6 +264,12 @@ public class RobotContainer {
}.withName("Record Path (Cancel to Save)"));
}
/**
* Called when a file is created, modified, or deleted.
* Adds newly created .path files to the SendableChooser.
* Reloads the path if the currently selected file is modified.
* @param watchKey The WatchKey that is being observed.
*/
private void updateAutoChooser(WatchKey watchKey) {
List<WatchEvent<?>> watchEvents = watchKey.pollEvents();
if (!watchEvents.isEmpty()) {
src/main/java/frc4388/robot/subsystems/BoomBoom.java
+66 -8
View File
@@ -10,6 +10,7 @@ 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;
@@ -40,8 +41,7 @@ public class BoomBoom extends SubsystemBase {
public Hood m_hoodSubsystem;
public Turret m_turretSubsystem;
// SimpleMotorFeedforward feedforward = new SimpleMotorFeedforward(69, 42, 0); //get real values
// later
// SimpleMotorFeedforward feedforward = new SimpleMotorFeedforward(69, 42, 0); //get real values later
public static class ShooterTableEntry {
public Double distance, hoodExt, drumVelocity;
@@ -58,31 +58,66 @@ public class BoomBoom extends SubsystemBase {
m_shooterFalconRight = shooterFalconRight;
try {
// This is a helper class that allows us to read a CSV file into a Java array.
CSV<ShooterTableEntry> csv = new CSV<>(ShooterTableEntry::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.
m_shooterTable = csv.read(new File(Filesystem.getDeployDirectory(), "ShooterData.csv").toPath());
new Thread(() -> LOGGER.fine(CSV.ReflectionTable.create(m_shooterTable, RobotBase.isSimulation()))).start();
} catch (final IOException e) {
e.printStackTrace();
// throw new RuntimeException(e);
// 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(m_shooterTable, RobotBase.isSimulation()))).start();
} catch (final IOException exception) {
ShooterTableEntry dummyEntry = new ShooterTableEntry();
dummyEntry.distance = 0.0;
dummyEntry.hoodExt = 0.0;
dummyEntry.drumVelocity = 0.0;
m_shooterTable = new ShooterTableEntry[] { dummyEntry };
LOGGER.log(Level.SEVERE, "Exception while reading shooter CSV table.", exception);
}
}
public Double getVelocity(final Double distance) {
// This is a function that takes a value (distance) and returns a value (drumVelocity) that is a
// linear interpolation of the two values (drumVelocity) at the two closest points in the table
// (m_shooterTable) to the given value (distance).
return linearInterpolate(m_shooterTable, distance, e -> e.distance, e -> e.drumVelocity).doubleValue();
}
public Double getHood(final Double distance) {
// This is a function that takes a value (distance) and returns a value (hoodExt) that is a linear
// interpolation of the two values (hoodExt) at the two closest points in the table (m_shooterTable)
// to the given value (distance).
return linearInterpolate(m_shooterTable, distance, e -> e.distance, e -> e.hoodExt).doubleValue();
}
/**
* 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();
@@ -91,11 +126,35 @@ public class BoomBoom extends SubsystemBase {
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();
@@ -104,7 +163,6 @@ public class BoomBoom extends SubsystemBase {
@Override
public void periodic() {
// This method will be called once per scheduler run
}
public void passRequiredSubsystem(Hood subsystem0, Turret subsystem1) {