diff --git a/src/main/deploy/Robot Data - Angles.csv b/src/main/deploy/Robot Data - Angles.csv deleted file mode 100644 index 5f3965e..0000000 --- a/src/main/deploy/Robot Data - Angles.csv +++ /dev/null @@ -1,3 +0,0 @@ -Angle (deg),Displacement (deg) -0,0 -0,0 \ No newline at end of file diff --git a/src/main/deploy/Robot Data - Distances.csv b/src/main/deploy/Robot Data - Distances.csv index 7f63722..8bfa2d5 100644 --- a/src/main/deploy/Robot Data - Distances.csv +++ b/src/main/deploy/Robot Data - Distances.csv @@ -1,16 +1,16 @@ -Distance (in),Hood Ext. (u),Drum Velocity (u/ds),Center Displacment (deg) -0,16,12000,1, -65.9,16,12000,1, -103,19,12000,1, -126.6,20.28,12000,1.5, -156.6,28,12000,1.5, -174,28,12000,1.5, -178,28,12000,1.3 -180,28.5,12000,1.3, -185.85,28.5,12000,1.3, -187,28.5,12000,1.3 -200,28.4,12000,1.3 -231,28.4,12000,1.8, -245,28.8,12000,1.8, -262,28.8,12000,1.8, -999,28.8,12000,1.8, \ No newline at end of file +Distance (in) ,Hood Ext. (u) ,Drum Velocity (u/ds) ,Center Displacement (deg) +0 ,16 ,12000 ,1 +65.9 ,16 ,12000 ,1 +103 ,19 ,12000 ,1 +126.6 ,20.28 ,12000 ,1.5 +156.6 ,28 ,12000 ,1.5 +174 ,28 ,12000 ,1.5 +178 ,28 ,12000 ,1.3 +180 ,28.5 ,12000 ,1.3 +185.85 ,28.5 ,12000 ,1.3 +187 ,28.5 ,12000 ,1.3 +200 ,28.4 ,12000 ,1.3 +231 ,28.4 ,12000 ,1.8 +245 ,28.8 ,12000 ,1.8 +262 ,28.8 ,12000 ,1.8 +999 ,28.8 ,12000 ,1.8 \ No newline at end of file diff --git a/src/main/java/frc4388/robot/commands/shooter/TrackTarget.java b/src/main/java/frc4388/robot/commands/shooter/TrackTarget.java index b682cb1..047b060 100644 --- a/src/main/java/frc4388/robot/commands/shooter/TrackTarget.java +++ b/src/main/java/frc4388/robot/commands/shooter/TrackTarget.java @@ -76,7 +76,7 @@ public class TrackTarget extends CommandBase { if (target == 1.0) { // If target in view // Aiming Left/Right - xAngle = xAngle + m_shooter.m_shooterTable.getCenterDisplacement(realDistance); + xAngle = xAngle + m_shooter.getCenterDisplacement(realDistance); turnAmount = ((xAngle / VisionConstants.FOV) * VisionConstants.TURN_P_VALUE); if (Math.abs(xAngle) < VisionConstants.X_ANGLE_ERROR) { turnAmount = 0; @@ -92,7 +92,7 @@ public class TrackTarget extends CommandBase { //m_shooterAim.runshooterRotatePID(targetAngle); SmartDashboard.putNumber("Distance to Target", realDistance); - SmartDashboard.putNumber("Center Displacement", m_shooter.m_shooterTable.getCenterDisplacement(realDistance)); + SmartDashboard.putNumber("Center Displacement", m_shooter.getCenterDisplacement(realDistance)); //START Equation Code double yVel = Math.sqrt(2 * VisionConstants.GRAV * VisionConstants.TARGET_HEIGHT); double xVel = (distance * VisionConstants.GRAV) / (yVel); @@ -102,8 +102,8 @@ public class TrackTarget extends CommandBase { //END Equation Code //START CSV Code - fireVel = m_shooter.m_shooterTable.getVelocity(realDistance); - fireAngle = m_shooter.m_shooterTable.getHood(realDistance); //Note: Ensure to follow because units are different + fireVel = m_shooter.getVelocity(realDistance); + fireAngle = m_shooter.getHood(realDistance); //Note: Ensure to follow because units are different //fireAngle = 33; //END CSV Code diff --git a/src/main/java/frc4388/robot/subsystems/Shooter.java b/src/main/java/frc4388/robot/subsystems/Shooter.java index a45894e..add0f49 100644 --- a/src/main/java/frc4388/robot/subsystems/Shooter.java +++ b/src/main/java/frc4388/robot/subsystems/Shooter.java @@ -7,15 +7,25 @@ package frc4388.robot.subsystems; +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.regex.Pattern; +import java.util.stream.IntStream; + import com.ctre.phoenix.motorcontrol.NeutralMode; import com.ctre.phoenix.motorcontrol.TalonFXControlMode; import com.ctre.phoenix.motorcontrol.can.WPI_TalonFX; +import edu.wpi.first.wpilibj.Filesystem; import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import edu.wpi.first.wpilibj2.command.SubsystemBase; import frc4388.robot.Constants.ShooterConstants; +import frc4388.utility.CSV; import frc4388.utility.Gains; -import frc4388.utility.ShooterTables; import frc4388.utility.Trims; import frc4388.utility.controller.IHandController; @@ -26,12 +36,35 @@ public class Shooter extends SubsystemBase { public static Gains m_drumShooterGains = ShooterConstants.DRUM_SHOOTER_GAINS; public static Shooter m_shooter; public static IHandController m_controller; - + double velP; double input; - public ShooterTables m_shooterTable; - + private class ShooterDataEntry { + Double distance; + Double hoodExt; + Double drumVelocity; + Double centerDisplacement; + + public Double getDistance() { + return distance; + } + + public Double getHoodExt() { + return hoodExt; + } + + public Double getDrumVelocity() { + return drumVelocity; + } + + public Double getCenterDisplacement() { + return centerDisplacement; + } + } + + private ShooterDataEntry[] m_shooterTable; + public boolean m_isDrumReady = false; public double m_fireVel; @@ -39,25 +72,26 @@ public class Shooter extends SubsystemBase { public ShooterHood m_shooterHoodSubsystem; public ShooterAim m_shooterAimSubsystem; - + /* - * Creates a new Shooter subsystem, with the drum shooter and the angle adjsuter. + * Creates a new Shooter subsystem, with the drum shooter and the angle + * adjsuter. */ public Shooter() { - //Testing purposes reseting gyros - //resetGyroAngleAdj(); + // Testing purposes reseting gyros + // resetGyroAngleAdj(); shooterTrims = new Trims(0, 0); - //SmartDashboard.putNumber("Velocity Target", 10000); - //SmartDashboard.putNumber("Angle Target", 3); + // SmartDashboard.putNumber("Velocity Target", 10000); + // SmartDashboard.putNumber("Angle Target", 3); - //LEFT FALCON + // LEFT FALCON m_shooterFalconLeft.configFactoryDefault(); m_shooterFalconLeft.setNeutralMode(NeutralMode.Coast); m_shooterFalconLeft.setInverted(true); m_shooterFalconLeft.configOpenloopRamp(1, ShooterConstants.SHOOTER_TIMEOUT_MS); m_shooterFalconLeft.configClosedloopRamp(0.75, ShooterConstants.SHOOTER_TIMEOUT_MS); - //RIGHT FALCON + // RIGHT FALCON m_shooterFalconRight.configFactoryDefault(); m_shooterFalconRight.setNeutralMode(NeutralMode.Coast); m_shooterFalconRight.setInverted(false); @@ -66,41 +100,60 @@ public class Shooter extends SubsystemBase { setShooterGains(); int closedLoopTimeMs = 1; - //LEFT FALCON + // LEFT FALCON m_shooterFalconLeft.configPeakOutputReverse(0, ShooterConstants.SHOOTER_TIMEOUT_MS); - - m_shooterFalconLeft.setSelectedSensorPosition(0, ShooterConstants.SHOOTER_PID_LOOP_IDX, ShooterConstants.SHOOTER_TIMEOUT_MS); - + + m_shooterFalconLeft.setSelectedSensorPosition(0, ShooterConstants.SHOOTER_PID_LOOP_IDX, + ShooterConstants.SHOOTER_TIMEOUT_MS); + m_shooterFalconLeft.configClosedLoopPeriod(0, closedLoopTimeMs, ShooterConstants.SHOOTER_TIMEOUT_MS); - m_shooterFalconLeft.configSupplyCurrentLimit(ShooterConstants.SUPPLY_CURRENT_LIMIT_CONFIG, ShooterConstants.SHOOTER_TIMEOUT_MS); + m_shooterFalconLeft.configSupplyCurrentLimit(ShooterConstants.SUPPLY_CURRENT_LIMIT_CONFIG, + ShooterConstants.SHOOTER_TIMEOUT_MS); + + // RIGHT FALCON + // m_shooterFalconRight.configPeakOutputForward(0, + // ShooterConstants.SHOOTER_TIMEOUT_MS); + + m_shooterFalconRight.setSelectedSensorPosition(0, ShooterConstants.SHOOTER_PID_LOOP_IDX, + ShooterConstants.SHOOTER_TIMEOUT_MS); - - //RIGHT FALCON - //m_shooterFalconRight.configPeakOutputForward(0, ShooterConstants.SHOOTER_TIMEOUT_MS); - - m_shooterFalconRight.setSelectedSensorPosition(0, ShooterConstants.SHOOTER_PID_LOOP_IDX, ShooterConstants.SHOOTER_TIMEOUT_MS); - m_shooterFalconRight.configClosedLoopPeriod(0, closedLoopTimeMs, ShooterConstants.SHOOTER_TIMEOUT_MS); - m_shooterFalconRight.configSupplyCurrentLimit(ShooterConstants.SUPPLY_CURRENT_LIMIT_CONFIG, ShooterConstants.SHOOTER_TIMEOUT_MS); + m_shooterFalconRight.configSupplyCurrentLimit(ShooterConstants.SUPPLY_CURRENT_LIMIT_CONFIG, + ShooterConstants.SHOOTER_TIMEOUT_MS); - m_shooterTable = new ShooterTables(); - //SmartDashboard.putNumber("CSV 10", m_shooterTable.getVelocity(10)); - //SmartDashboard.putNumber("CSV 200", m_shooterTable.getVelocity(200)); - //SmartDashboard.putNumber("CSV 250", m_shooterTable.getVelocity(250)); - //SmartDashboard.putNumber("CSV 500", m_shooterTable.getVelocity(500)); + try { + m_shooterTable = new CSV<>(ShooterDataEntry::new) { + private final Pattern parentheses = Pattern.compile("\\([^\\)]*+\\)"); - //SmartDashboard.putNumber("CSV A -30", m_shooterTable.getAngleDisplacement(-30)); - //SmartDashboard.putNumber("CSV A 10", m_shooterTable.getAngleDisplacement(10)); - //SmartDashboard.putNumber("CSV A 5", m_shooterTable.getAngleDisplacement(5)); - //SmartDashboard.putNumber("CSV A 30", m_shooterTable.getAngleDisplacement(30)); + @Override + protected String headerSanitizer(final String header) { + return super.headerSanitizer(parentheses.matcher(header).replaceAll("")); + } + }.read(new File(Filesystem.getDeployDirectory(), "example_data.csv").toPath()); + System.out.println(CSV.ReflectionTable.create(m_shooterTable)); + } catch (IOException e) { + throw new RuntimeException(e); + } + // SmartDashboard.putNumber("CSV 10", m_shooterTable.getVelocity(10)); + // SmartDashboard.putNumber("CSV 200", m_shooterTable.getVelocity(200)); + // SmartDashboard.putNumber("CSV 250", m_shooterTable.getVelocity(250)); + // SmartDashboard.putNumber("CSV 500", m_shooterTable.getVelocity(500)); + + // SmartDashboard.putNumber("CSV A -30", + // m_shooterTable.getAngleDisplacement(-30)); + // SmartDashboard.putNumber("CSV A 10", + // m_shooterTable.getAngleDisplacement(10)); + // SmartDashboard.putNumber("CSV A 5", m_shooterTable.getAngleDisplacement(5)); + // SmartDashboard.putNumber("CSV A 30", + // m_shooterTable.getAngleDisplacement(30)); } @Override public void periodic() { // This method will be called once per scheduler run - try{ + try { SmartDashboard.putNumber("Drum Velocity", m_shooterFalconLeft.getSelectedSensorVelocity()); SmartDashboard.putNumber("Drum Velocity CSV", m_fireVel); @@ -109,17 +162,17 @@ public class Shooter extends SubsystemBase { SmartDashboard.putNumber("Shooter Current", m_shooterFalconLeft.getSupplyCurrent()); - SmartDashboard.putBoolean("Drum Ready" , m_isDrumReady); + SmartDashboard.putBoolean("Drum Ready", m_isDrumReady); } - catch(Exception e) - { - + catch (Exception e) { + } } /** * Passes subsystem needed. + * * @param subsystem Subsystem needed. */ public void passRequiredSubsystem(ShooterHood subsystem0, ShooterAim subsystem1) { @@ -133,12 +186,13 @@ public class Shooter extends SubsystemBase { /** * Runs drum shooter motor. + * * @param speed Speed to set the motor at */ public void runDrumShooter(double speed) { m_shooterFalconLeft.set(TalonFXControlMode.PercentOutput, speed); m_shooterFalconRight.follow(m_shooterFalconLeft); - //m_shooterFalconRight.set(TalonFXControlMode.PercentOutput, speed); + // m_shooterFalconRight.set(TalonFXControlMode.PercentOutput, speed); } /** @@ -146,19 +200,66 @@ public class Shooter extends SubsystemBase { */ public void setShooterGains() { m_shooterFalconLeft.selectProfileSlot(ShooterConstants.SHOOTER_SLOT_IDX, ShooterConstants.SHOOTER_PID_LOOP_IDX); - m_shooterFalconLeft.config_kF(ShooterConstants.SHOOTER_SLOT_IDX, m_drumShooterGains.m_kF, ShooterConstants.SHOOTER_TIMEOUT_MS); - m_shooterFalconLeft.config_kP(ShooterConstants.SHOOTER_SLOT_IDX, m_drumShooterGains.m_kP, ShooterConstants.SHOOTER_TIMEOUT_MS); - m_shooterFalconLeft.config_kI(ShooterConstants.SHOOTER_SLOT_IDX, m_drumShooterGains.m_kI, ShooterConstants.SHOOTER_TIMEOUT_MS); - m_shooterFalconLeft.config_kD(ShooterConstants.SHOOTER_SLOT_IDX, m_drumShooterGains.m_kD, ShooterConstants.SHOOTER_TIMEOUT_MS); + m_shooterFalconLeft.config_kF(ShooterConstants.SHOOTER_SLOT_IDX, m_drumShooterGains.m_kF, + ShooterConstants.SHOOTER_TIMEOUT_MS); + m_shooterFalconLeft.config_kP(ShooterConstants.SHOOTER_SLOT_IDX, m_drumShooterGains.m_kP, + ShooterConstants.SHOOTER_TIMEOUT_MS); + m_shooterFalconLeft.config_kI(ShooterConstants.SHOOTER_SLOT_IDX, m_drumShooterGains.m_kI, + ShooterConstants.SHOOTER_TIMEOUT_MS); + m_shooterFalconLeft.config_kD(ShooterConstants.SHOOTER_SLOT_IDX, m_drumShooterGains.m_kD, + ShooterConstants.SHOOTER_TIMEOUT_MS); } /** * Runs drum shooter velocity PID. + * * @param targetVel Target velocity to run motor at */ public void runDrumShooterVelocityPID(double targetVel) { - //System.out.println("Target Velocity" + targetVel); - m_shooterFalconLeft.set(TalonFXControlMode.Velocity, targetVel); //Init PID + // System.out.println("Target Velocity" + targetVel); + m_shooterFalconLeft.set(TalonFXControlMode.Velocity, targetVel); // Init PID m_shooterFalconRight.follow(m_shooterFalconLeft); } + + public Double getCenterDisplacement(Double distance) { + return linearInterpolate(distance, ShooterDataEntry::getDistance, ShooterDataEntry::getCenterDisplacement) + .doubleValue(); + } + + public Double getVelocity(Double distance) { + return linearInterpolate(distance, ShooterDataEntry::getDistance, ShooterDataEntry::getDrumVelocity).doubleValue(); + } + + public Double getHood(Double distance) { + return linearInterpolate(distance, ShooterDataEntry::getDistance, ShooterDataEntry::getHoodExt).doubleValue(); + } + + private Number linearInterpolate(final Number lookupValue, final Function lookupGetter, + final Function targetGetter) { + final Map.Entry closestEntry = lookup(lookupValue.doubleValue(), lookupGetter, false) + .orElse(Map.entry(m_shooterTable.length - 1, m_shooterTable[m_shooterTable.length - 1])); + final ShooterDataEntry closestRecord = closestEntry.getValue(); + final int closestRecordIndex = closestEntry.getKey(); + final ShooterDataEntry neighborRecord = m_shooterTable[lookupValue.doubleValue() <= lookupGetter + .apply(closestRecord).doubleValue() ? Math.max(closestRecordIndex == 0 ? 1 : 0, closestRecordIndex - 1) + : Math.min(closestRecordIndex + 1, + m_shooterTable.length - (closestRecordIndex == m_shooterTable.length - 1 ? 2 : 1))]; + return lerp2(lookupValue, lookupGetter.apply(closestRecord), targetGetter.apply(closestRecord), + lookupGetter.apply(neighborRecord), targetGetter.apply(neighborRecord)); + } + + private Optional> lookup(final Number value, + final Function valueGetter, boolean exactMatch) { + Optional> match = IntStream.range(0, m_shooterTable.length) + .mapToObj(i -> Map.entry(i, m_shooterTable[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(); + + } + + private 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(); + } } \ No newline at end of file diff --git a/src/main/java/frc4388/utility/CSV.java b/src/main/java/frc4388/utility/CSV.java new file mode 100644 index 0000000..cbe8f98 --- /dev/null +++ b/src/main/java/frc4388/utility/CSV.java @@ -0,0 +1,275 @@ +/*----------------------------------------------------------------------------*/ +/* Copyright (c) 2018-2019 FIRST. All Rights Reserved. */ +/* Open Source Software - may be modified and shared by FRC teams. The code */ +/* must be accompanied by the FIRST BSD license file in the root directory of */ +/* the project. */ +/*----------------------------------------------------------------------------*/ + +package frc4388.utility; + +import java.awt.Color; + +import java.io.BufferedReader; +import java.io.IOException; +import java.lang.invoke.MethodHandleProxies; +import java.lang.invoke.MethodHandles; +import java.lang.invoke.MethodType; +import java.lang.reflect.Array; +import java.lang.reflect.Field; +import java.lang.reflect.Modifier; +import java.nio.file.Files; +import java.nio.file.Path; +import java.text.MessageFormat; +import java.util.ArrayList; +import java.util.HashMap; +import java.util.List; +import java.util.Map; +import java.util.Objects; +import java.util.function.BiConsumer; +import java.util.function.Function; +import java.util.function.IntFunction; +import java.util.function.Predicate; +import java.util.function.Supplier; +import java.util.regex.Pattern; +import java.util.stream.Collectors; +import java.util.stream.IntStream; +import java.util.stream.Stream; + +public class CSV { + private static final Pattern SANITIZER = Pattern.compile("[^$\\w,]"); + + private final Supplier generator; + private final IntFunction arrayGenerator; + private final Map> setters; + + /** + * A binary string operator to be applied to the entire header of the CSV. + */ + protected String headerSanitizer(final String header) { + return SANITIZER.matcher(header).replaceAll(""); + } + + /** + * A binary string operator to be applied to each name in the header of the CSV. + */ + protected String nameProcessor(final String name) { + return Character.toLowerCase(name.charAt(0)) + name.substring(1); + } + + /** + * Creates a new {@code CSV} instance and prepares for populating the fields of + * objects created by the given generator. Fields of primitive types is not + * supported. + * + * @param generator a parameterless supplier which produces a new object with + * any number of fields corresponding to header names from a + * CSV file. The first character of the names from the header + * in the CSV file will be made lowercase and invalid + * characters will be removed to match Java naming conventions. + * @see #read(Path) + */ + @SuppressWarnings("unchecked") + public CSV(final Supplier generator) { + final Class clazz = generator.get().getClass(); + final Map, Function> fieldParsers = new HashMap<>(); + this.arrayGenerator = size -> (R[]) Array.newInstance(clazz, size); + this.generator = generator; + this.setters = new HashMap<>(); + for (final Field field : clazz.getFields()) { + final Function parser = Modifier.isStatic(field.getModifiers()) ? null + : fieldParsers.computeIfAbsent(field.getType(), CSV::getTypeParser); + if (parser != null) + this.setters.put(field.getName(), (final R obj, final String rawValue) -> { + try { + field.set(obj, rawValue.isEmpty() ? null : parser.apply(rawValue)); + } catch (final IllegalAccessException e) { + throw new RuntimeException(e); + } + }); + } + } + + /** + * Reads and parses the contents of the given CSV file, and returns an array + * filled with populated objects created with the previously given generator. + * Cells are parsed using their corresponding field's {@code valueOf(String)} + * function. + * + * @param path the path to a CSV file + * @return the parsed data from the CSV file + * @throws IOException if an I/O error occurs opening the file + */ + @SuppressWarnings("unchecked") + public R[] read(final Path path) throws IOException { + try (final BufferedReader reader = Files.newBufferedReader(path)) { + final BiConsumer[] fieldSetters = Stream.of(headerSanitizer(reader.readLine()).split(",")) + .map(this::nameProcessor).map(setters::get).toArray(BiConsumer[]::new); + final Stream lines = reader.lines(); + return lines.filter(Predicate.not(String::isBlank)) + .map(line -> deserializeRecordString(line, fieldSetters, generator.get())) + .toArray(this.arrayGenerator); + } + } + + @SuppressWarnings("unchecked") + private static Function getTypeParser(final Class type) { + try { + return type.isAssignableFrom(String.class) ? Function.identity() + : MethodHandleProxies.asInterfaceInstance(Function.class, MethodHandles.publicLookup() + .findStatic(type, "valueOf", MethodType.methodType(type, String.class))); + } catch (final NoSuchMethodException | IllegalAccessException e) { + return null; + } + } + + private static R deserializeRecordString(final String recordString, + final BiConsumer[] fieldParseSetters, final R object) { + final int recordStringLength = recordString.length(); + int fieldBeginIndex = 0, tryFieldEndFromIndex = 0, i = 0; + while (tryFieldEndFromIndex < recordStringLength && i < fieldParseSetters.length) { + final int tryFieldEndIndex = recordString.indexOf(',', tryFieldEndFromIndex); + String field = recordString + .substring(fieldBeginIndex, tryFieldEndIndex == -1 ? recordStringLength : tryFieldEndIndex).strip(); + if (!field.isEmpty() && (tryFieldEndFromIndex != fieldBeginIndex || field.charAt(0) == '"')) { + final int fieldLength = field.length(); + if (countTrailing(field, '"') % 2 == 0) { + tryFieldEndFromIndex = tryFieldEndIndex + 1; + continue; + } else + field = field.substring(1, fieldLength - 1).replace("\"\"", "\""); + } + final BiConsumer setter = fieldParseSetters[i]; + if (setter != null) + setter.accept(object, field); + tryFieldEndFromIndex = fieldBeginIndex = tryFieldEndIndex + 1; + i++; + } + return object; + } + + private static int countTrailing(final String str, final char c) { + final int l = str.length(); + int count = 0; + while (str.charAt(l - count - 1) == c && count < l) + count++; + return count; + } + + public static class ReflectionTable { + public static String create(final T[] objects) { + final Field[] fields = Stream.of(objects).flatMap(object -> Stream.of(object.getClass().getFields())) + .distinct().toArray(Field[]::new); + final List> rows = new ArrayList<>(); + rows.add(Stream.of(fields).map(ReflectionTable::new).collect(Collectors.toList())); + rows.addAll(Stream.of(objects).map( + obj -> Stream.of(fields).map(field -> new ReflectionTable(obj, field)).collect(Collectors.toList())) + .collect(Collectors.toList())); + final int[] columnWidths = rows.stream() + .map(row -> row.stream().map(cell -> cell.string).mapToInt(String::length).toArray()) + .reduce(new int[fields.length], (result, row) -> IntStream.range(0, row.length) + .map(i -> Math.max(result[i], row[i])).toArray()); + IntStream.range(0, fields.length).forEach(i -> { + final var columnSummaryStatistics = rows.stream().skip(1) + .mapToDouble(row -> row.get(i).getValue().doubleValue()).summaryStatistics(); + rows.stream().skip(1).forEach(row -> row.get(i).colorByValue(columnSummaryStatistics.getMin(), + columnSummaryStatistics.getMax())); + }); + return rows.stream() + .map(row -> IntStream.range(0, row.size()) + .mapToObj(i -> String.format( + MessageFormat.format("{0} %{1}{2}s {3}", row.get(i).escape, + row.get(i).padRight ? "-" : "", columnWidths[i], RESET_STYLE), + row.get(i).string)) + .collect(Collectors.joining("|"))) + .collect(Collectors.joining(LF)); + } + + private static final Color GRADIENT_MIN = new Color(0, 51, 0); + private static final Color GRADIENT_MAX = new Color(0, 255, 0); + private static final String CONTROL = "\033"; + private static final String CSI = "["; + private static final String LF = "\n"; + private static final String RESET = "0"; + private static final String BOLD = "1"; + private static final String ITALIC = "3"; + private static final String UNDERLINE = "4"; + private static final String BACKGROUND_RED = "41"; + private static final String FOREGROUND = "38"; + private static final String BACKGROUND = "48"; + private static final String TRUECOLOR = "2"; + private static final String SEPARATOR = ";"; + private static final String SGR = "m"; + private static final String HEADER_STYLE = CONTROL + CSI + BOLD + SEPARATOR + UNDERLINE + SGR; + private static final String NULL_STYLE = CONTROL + CSI + ITALIC + SGR; + private static final String ERROR_STYLE = CONTROL + CSI + ITALIC + SGR + CONTROL + CSI + BACKGROUND_RED + SGR; + private static final String RESET_STYLE = CONTROL + CSI + RESET + SGR; + private Object value; + private String string; + private boolean padRight; + private String escape; + + private ReflectionTable(final Object obj, final Field field) { + try { + value = field.get(obj); + string = Objects.toString(value); + padRight = !Number.class.isAssignableFrom(field.getType()); + escape = Objects.isNull(value) ? NULL_STYLE : ""; + } catch (final IllegalAccessException | IllegalArgumentException e) { + value = null; + string = e.getClass().getSimpleName(); + padRight = false; + escape = ERROR_STYLE; + } + } + + private ReflectionTable(final Field field) { + value = null; + string = field.getName(); + padRight = true; + escape = HEADER_STYLE; + } + + private Number getValue() { + return padRight ? Objects.hashCode(string) : Objects.requireNonNullElse((Number) value, 0); + } + + private void colorByValue(final Number min, final Number max) { + if (Objects.nonNull(value)) { + final double normal = (getValue().doubleValue() - min.doubleValue()) + / (max.doubleValue() - min.doubleValue()); + final Color color = new Color(range(normal, GRADIENT_MIN.getRed(), GRADIENT_MAX.getRed()), + range(normal, GRADIENT_MIN.getGreen(), GRADIENT_MAX.getGreen()), + range(normal, GRADIENT_MIN.getBlue(), GRADIENT_MAX.getBlue())); + escape += (contrastRatio(color, Color.BLACK) > contrastRatio(Color.WHITE, color) + ? colorTo24BitSGR(Color.BLACK, false) + : colorTo24BitSGR(Color.WHITE, false)) + colorTo24BitSGR(color, true); + } + } + + private static String colorTo24BitSGR(final Color color, final boolean background) { + return CONTROL + CSI + (background ? BACKGROUND : FOREGROUND) + SEPARATOR + TRUECOLOR + SEPARATOR + + color.getRed() + SEPARATOR + color.getGreen() + SEPARATOR + color.getBlue() + SGR; + } + + private static int range(final double normal, final int min, final int max) { + return (int) (normal * (max - min) + min); + } + + /* https://www.w3.org/TR/WCAG20/#contrast-ratiodef */ + private static float contrastRatio(final Color lighter, final Color darker) { + return (relativeLuminance(lighter) + 0.05f) / (relativeLuminance(darker) + 0.05f); + } + + /* https://www.w3.org/TR/2008/REC-WCAG20-20081211/#relativeluminancedef */ + private static float relativeLuminance(final Color color) { + final float[] components = color.getRGBComponents(null); + final float r = components[0] <= 0.03928f ? components[0] / 12.92f + : (float) Math.pow((components[0] + 0.055f) / 1.055f, 2.4f); + final float g = components[1] <= 0.03928f ? components[1] / 12.92f + : (float) Math.pow((components[1] + 0.055f) / 1.055f, 2.4f); + final float b = components[2] <= 0.03928f ? components[2] / 12.92f + : (float) Math.pow((components[2] + 0.055f) / 1.055f, 2.4f); + return 0.2126f * r + 0.7152f * g + 0.0722f * b; + } + } +} diff --git a/src/main/java/frc4388/utility/ShooterTables.java b/src/main/java/frc4388/utility/ShooterTables.java deleted file mode 100644 index 3e5dae3..0000000 --- a/src/main/java/frc4388/utility/ShooterTables.java +++ /dev/null @@ -1,171 +0,0 @@ -/*----------------------------------------------------------------------------*/ -/* Copyright (c) 2018-2019 FIRST. All Rights Reserved. */ -/* Open Source Software - may be modified and shared by FRC teams. The code */ -/* must be accompanied by the FIRST BSD license file in the root directory of */ -/* the project. */ -/*----------------------------------------------------------------------------*/ - -package frc4388.utility; - -import java.io.BufferedReader; -import java.io.File; -import java.io.FileNotFoundException; -import java.io.FileReader; -import java.io.IOException; - -import edu.wpi.first.wpilibj.Filesystem; -import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; - -/** - * Add your docs here. - */ -public class ShooterTables { - double[][] m_distance = new double[50][4]; - double[][] m_angle = new double[50][2]; - - final int m_columnDistance = 0; - final int m_columnHood = 1; - final int m_columnVel = 2; - final int m_columnCenterDisplacement = 3; - final int m_columnAngle = 0; - final int m_columnDisplacement = 1; - - int m_distanceLength; - int m_angleLength; - - public ShooterTables() { - int lineNum = 0; - - File distanceCSV = new File(Filesystem.getDeployDirectory().getAbsolutePath() + "/Robot Data - Distances.csv"); - File angleCSV = new File(Filesystem.getDeployDirectory().getAbsolutePath() + "/Robot Data - Angles.csv"); - - try { - - - BufferedReader distanceReader = new BufferedReader(new FileReader(distanceCSV)); - BufferedReader angleReader = new BufferedReader(new FileReader(angleCSV)); - String line = ""; - - while ((line = distanceReader.readLine()) != null) { - - if (lineNum == 0) { - lineNum ++; - } else { - String[] values = line.split(","); - - m_distance[lineNum - 1][m_columnDistance] = Double.parseDouble(values[0]); - m_distance[lineNum - 1][m_columnHood] = Double.parseDouble(values[1]); - m_distance[lineNum - 1][m_columnVel] = Double.parseDouble(values[2]); - m_distance[lineNum - 1][m_columnCenterDisplacement] = Double.parseDouble(values[3]); - - lineNum ++; - } - - } - - m_distanceLength = lineNum-1; - lineNum = 0; - - while ((line = angleReader.readLine()) != null) { - - if (lineNum == 0) { - lineNum ++; - } else { - String[] values = line.split(","); - - m_angle[lineNum - 1][m_columnAngle] = Double.parseDouble(values[0]); - m_angle[lineNum - 1][m_columnDisplacement] = Double.parseDouble(values[1]); - - lineNum ++; - } - } - - m_angleLength = lineNum-1; - - distanceReader.close(); - angleReader.close(); - - } catch (FileNotFoundException e) { - e.printStackTrace(); - } catch (IOException e) { - e.printStackTrace(); - } - //SmartDashboard.putNumber("Row 2 Column 1", m_angle[1][0]); - //SmartDashboard.putNumber("Row 4 Column 2", m_angle[3][1]); - //SmartDashboard.putNumber("m_distanceLength", m_distanceLength); - //SmartDashboard.putNumber("Distance last row 0", m_distance[m_distanceLength-1][0]); - //SmartDashboard.putNumber("Distance last row 1", m_distance[m_distanceLength-1][1]); - //SmartDashboard.putNumber("Distance last row 2", m_distance[m_distanceLength-1][2]); - } - - public double getHood(double distance) { //Rotations of motor - int i = 0; - while ((i < m_distanceLength) && (m_distance[i][m_columnDistance] < distance)) { - i ++; - } - if ((i < m_distanceLength) && (m_distance[i][m_columnDistance] == distance)) { - return m_distance[i][m_columnHood]; - } else { - if (i >= m_distanceLength) { - i = m_distanceLength - 1; - } - return linearInterpolation(i, distance, m_columnHood, m_distance); - } - } - - public double getVelocity(double distance) { //Units per 100ms - int i = 0; - while ((i < m_distanceLength) && (m_distance[i][m_columnDistance] < distance)) { - i ++; - } - if ((i < m_distanceLength) && (m_distance[i][m_columnDistance] == distance)) { - return m_distance[i][m_columnVel]; - } else { - if (i >= m_distanceLength) { - i = m_distanceLength - 1; - } - return linearInterpolation(i, distance, m_columnVel, m_distance); - } - } - - public double getCenterDisplacement(double distance) { //Degrees of Lime FOV - int i = 0; - while ((i < m_distanceLength) && (m_distance[i][m_columnDistance] < distance)) { - i ++; - } - if ((i < m_distanceLength) && (m_distance[i][m_columnDistance] == distance)) { - return m_distance[i][m_columnCenterDisplacement]; - } else { - if (i >= m_distanceLength) { - i = m_distanceLength - 1; - } - return linearInterpolation(i, distance, m_columnCenterDisplacement, m_distance); - } - } - - - - public double getAngleDisplacement(double angle) { - int i = 0; - while ((i < m_angleLength) && (m_angle[i][m_columnAngle] < angle)) { - i ++; - } - if ((i < m_angleLength) && (m_angle[i][m_columnAngle] == angle)) { - return m_angle[i][m_columnDisplacement]; - } else { - if (i >= m_angleLength) { - i = m_angleLength - 1; - } - return linearInterpolation(i, angle, m_columnDisplacement, m_angle); - } - } - - public double linearInterpolation(int i, double value, int column, double[][] table) { - if (i != 0) { - double slope = (table[i][column] - table[i-1][column]) / (table[i][0] - table[i-1][0]); - value = slope * (value - table[i-1][0]) + table[i-1][column]; - return value; - } - return 0.0; - } -} \ No newline at end of file