diff --git a/src/main/deploy/Robot Data - Distances.csv b/src/main/deploy/Robot Data - Distances.csv new file mode 100644 index 0000000..99413bc --- /dev/null +++ b/src/main/deploy/Robot Data - Distances.csv @@ -0,0 +1,3 @@ +Distance (in) ,Hood Ext. (u) ,Drum Velocity (u/ds) +0 ,16 ,12000 +999 ,28.8 ,12000 \ No newline at end of file diff --git a/src/main/java/frc4388/robot/commands/AimToCenter.java b/src/main/java/frc4388/robot/commands/AimToCenter.java index 723bd88..9a90cb5 100644 --- a/src/main/java/frc4388/robot/commands/AimToCenter.java +++ b/src/main/java/frc4388/robot/commands/AimToCenter.java @@ -53,12 +53,12 @@ public class AimToCenter extends CommandBase { } + /** + * Checks if in deadzone. + * @return True if deadzone. + */ public boolean isDeadzone() { - if ((ShooterConstants.DEADZONE_LEFT < m_targetAngle) && (m_targetAngle < ShooterConstants.DEADZONE_RIGHT)) { - return true; - } else { - return false; - } + return ((ShooterConstants.DEADZONE_LEFT < m_targetAngle) && (m_targetAngle < ShooterConstants.DEADZONE_RIGHT)); } // Called once the command ends or is interrupted. diff --git a/src/main/java/frc4388/robot/commands/Shoot.java b/src/main/java/frc4388/robot/commands/Shoot.java new file mode 100644 index 0000000..2f8c5ef --- /dev/null +++ b/src/main/java/frc4388/robot/commands/Shoot.java @@ -0,0 +1,58 @@ +// 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.nio.file.SecureDirectoryStream; + +import edu.wpi.first.wpilibj2.command.CommandBase; +import frc4388.robot.subsystems.BoomBoom; +import frc4388.robot.subsystems.Hood; +import frc4388.robot.subsystems.SwerveDrive; + +public class Shoot extends CommandBase { + + public SwerveDrive m_swerve; + public BoomBoom m_boomBoom; + public Hood m_hood; + + public double m_targetVel; + public double m_targetHood; + public double m_distance; + + /** Creates a new Shoot. */ + public Shoot(SwerveDrive sDrive, BoomBoom sShooter, Hood sHood) { + // Use addRequirements() here to declare subsystem dependencies. + m_swerve = sDrive; + m_boomBoom = sShooter; + m_hood = sHood; + + addRequirements(m_swerve, m_boomBoom, m_hood); + } + + // Called when the command is initially scheduled. + @Override + public void initialize() { + m_distance = 0; //TODO: get this value using odometry + m_targetVel = m_boomBoom.getVelocity(m_distance); + m_targetHood = m_boomBoom.getHood(m_distance); + } + + // Called every time the scheduler runs while the command is scheduled. + @Override + public void execute() { + m_hood.runAngleAdjustPID(m_targetHood); + m_boomBoom.runDrumShooterVelocityPID(m_targetVel); + } + + // 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/subsystems/BoomBoom.java b/src/main/java/frc4388/robot/subsystems/BoomBoom.java index 01c850c..9dfd332 100644 --- a/src/main/java/frc4388/robot/subsystems/BoomBoom.java +++ b/src/main/java/frc4388/robot/subsystems/BoomBoom.java @@ -4,28 +4,29 @@ package frc4388.robot.subsystems; -import java.io.Console; -import java.util.Base64.Encoder; +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.simulation.JoystickSim; -import edu.wpi.first.wpilibj.smartdashboard.*; -import edu.wpi.first.math.controller.BangBangController; -import edu.wpi.first.math.controller.SimpleMotorFeedforward; +import edu.wpi.first.wpilibj.Filesystem; +import edu.wpi.first.wpilibj.RobotBase; import edu.wpi.first.wpilibj2.command.SubsystemBase; import frc4388.robot.Constants.ShooterConstants; -import frc4388.utility.ShooterTables; +import frc4388.utility.CSV; import frc4388.utility.Gains; import frc4388.utility.controller.IHandController; -import com.revrobotics.RelativeEncoder; public class BoomBoom extends SubsystemBase { public WPI_TalonFX m_shooterFalconLeft; public WPI_TalonFX m_shooterFalconRight; -public ShooterTables m_shooterTable; public static Gains m_drumShooterGains = ShooterConstants.DRUM_SHOOTER_GAINS; public static BoomBoom m_boomBoom; public static IHandController m_driverController; //not sure if driverController in 2022 = m_controller in 2020 @@ -42,7 +43,11 @@ public Turret m_turretSubsystem; // SimpleMotorFeedforward feedforward = new SimpleMotorFeedforward(69, 42, 0); //get real values later +public static class ShooterTableEntry { + public Double distance, hoodExt, drumVelocity; +} +private ShooterTableEntry[] m_shooterTable; /* * Creates new BoomBoom subsystem, has drum shooter and angle adjuster @@ -51,27 +56,55 @@ public Turret m_turretSubsystem; public BoomBoom(WPI_TalonFX shooterFalconLeft, WPI_TalonFX shooterFalconRight) { m_shooterFalconLeft = shooterFalconLeft; m_shooterFalconRight = shooterFalconRight; - m_shooterTable = new ShooterTables(); + + + try { + CSV csv = new CSV<>(ShooterTableEntry::new) { + private final Pattern parentheses = Pattern.compile("\\([^\\)]*+\\)"); + + @Override + protected String headerSanitizer(final String header) { + return super.headerSanitizer(parentheses.matcher(header).replaceAll("")); + } + + }; + m_shooterTable = csv.read(new File(Filesystem.getDeployDirectory(), "Robot Data - Distances.csv").toPath()); + new Thread(() -> System.out.println(CSV.ReflectionTable.create(m_shooterTable, RobotBase.isSimulation()))).start(); + } catch (final IOException e) { + e.printStackTrace(); + // throw new RuntimeException(e); + } +} + +public Double getVelocity(final Double distance) { + return linearInterpolate(m_shooterTable, distance, e -> e.distance, e -> e.drumVelocity).doubleValue(); +} + +public Double getHood(final Double distance) { + return linearInterpolate(m_shooterTable, distance, e -> e.distance, e -> e.hoodExt).doubleValue(); +} + +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)); +} + +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(); +} + +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(); } - @Override public void periodic() { // This method will be called once per scheduler run - -try { - // SmartDashboard.putNumber("Drum Velocity", m_shooterFalconLeft.getSelectedSensorVelocity()); - - // SmartDashboard.putNumber("Drum Velocity CSV", m_fireVel); - - // SmartDashboard.putNumber("Shooter Temp C", m_shooterFalconLeft.getTemperature()); //all these values should be "defined" elsewhere, fix this - - // SmartDashboard.putNumber("Shooter Current", m_shooterFalconLeft.getSupplyCurrent()); - - // SmartDashboard.putNumber("Drum Ready", m_isDrumReady); -} catch (Exception e) { - //TODO: handle exception -} } diff --git a/src/main/java/frc4388/robot/subsystems/Hood.java b/src/main/java/frc4388/robot/subsystems/Hood.java index 046d402..72bb3f8 100644 --- a/src/main/java/frc4388/robot/subsystems/Hood.java +++ b/src/main/java/frc4388/robot/subsystems/Hood.java @@ -23,13 +23,13 @@ import frc4388.utility.Gains; public class Hood extends SubsystemBase { public BoomBoom m_shooterSubsystem; - //public CANSparkMax m_angleAdjusterMotor = new CANSparkMax(ShooterConstants.SHOOTER_ANGLE_ADJUST_ID, MotorType.kBrushless); + public CANSparkMax m_angleAdjusterMotor = new CANSparkMax(ShooterConstants.SHOOTER_ANGLE_ADJUST_ID, MotorType.kBrushless); public SparkMaxLimitSwitch m_hoodUpLimitSwitch; public SparkMaxLimitSwitch m_hoodDownLimitSwitch; public static Gains m_angleAdjusterGains = ShooterConstants.SHOOTER_ANGLE_GAINS; - // public RelativeEncoder m_angleEncoder = m_angleAdjusterMotor.getEncoder(); + public RelativeEncoder m_angleEncoder = m_angleAdjusterMotor.getEncoder(); - // public SparkMaxPIDController m_angleAdjusterPIDController = m_angleAdjusterMotor.getPIDController(); + public SparkMaxPIDController m_angleAdjusterPIDController = m_angleAdjusterMotor.getPIDController(); //public boolean m_isHoodReady = false; @@ -39,12 +39,12 @@ public double m_fireAngle; /** Creates a new Hood. */ public Hood() { - // m_angleAdjusterMotor.setIdleMode(IdleMode.kBrake); + m_angleAdjusterMotor.setIdleMode(IdleMode.kBrake); - // m_hoodUpLimitSwitch = m_angleAdjusterMotor.getForwardLimitSwitch(SparkMaxLimitSwitch.Type.kNormallyOpen); - // m_hoodDownLimitSwitch = m_angleAdjusterMotor.getReverseLimitSwitch(SparkMaxLimitSwitch.Type.kNormallyOpen); - // m_hoodUpLimitSwitch.enableLimitSwitch(true); - // m_hoodDownLimitSwitch.enableLimitSwitch(true); + m_hoodUpLimitSwitch = m_angleAdjusterMotor.getForwardLimitSwitch(SparkMaxLimitSwitch.Type.kNormallyOpen); + m_hoodDownLimitSwitch = m_angleAdjusterMotor.getReverseLimitSwitch(SparkMaxLimitSwitch.Type.kNormallyOpen); + m_hoodUpLimitSwitch.enableLimitSwitch(true); + m_hoodDownLimitSwitch.enableLimitSwitch(true); } @@ -55,14 +55,14 @@ public double m_fireAngle; public void runAngleAdjustPID(double targetAngle) { //Set PID Coefficients - // m_angleAdjusterPIDController.setP(m_angleAdjusterGains.m_kP); - // m_angleAdjusterPIDController.setI(m_angleAdjusterGains.m_kI); - // m_angleAdjusterPIDController.setD(m_angleAdjusterGains.m_kD); - // m_angleAdjusterPIDController.setIZone(m_angleAdjusterGains.m_kIzone); - // m_angleAdjusterPIDController.setFF(m_angleAdjusterGains.m_kF); - // m_angleAdjusterPIDController.setOutputRange(ShooterConstants.SHOOTER_TURRET_MIN, m_angleAdjusterGains.m_kPeakOutput); + m_angleAdjusterPIDController.setP(m_angleAdjusterGains.m_kP); + m_angleAdjusterPIDController.setI(m_angleAdjusterGains.m_kI); + m_angleAdjusterPIDController.setD(m_angleAdjusterGains.m_kD); + m_angleAdjusterPIDController.setIZone(m_angleAdjusterGains.m_kIzone); + m_angleAdjusterPIDController.setFF(m_angleAdjusterGains.m_kF); + m_angleAdjusterPIDController.setOutputRange(ShooterConstants.SHOOTER_TURRET_MIN, m_angleAdjusterGains.m_kPeakOutput); - // m_angleAdjusterPIDController.setReference(targetAngle, ControlType.kPosition); + m_angleAdjusterPIDController.setReference(targetAngle, ControlType.kPosition); } diff --git a/src/main/java/frc4388/robot/subsystems/Turret.java b/src/main/java/frc4388/robot/subsystems/Turret.java index 4d7ac99..28c9583 100644 --- a/src/main/java/frc4388/robot/subsystems/Turret.java +++ b/src/main/java/frc4388/robot/subsystems/Turret.java @@ -51,10 +51,12 @@ public class Turret extends SubsystemBase { m_boomBoomRightLimit = m_boomBoomRotateMotor.getForwardLimitSwitch(SparkMaxLimitSwitch.Type.kNormallyOpen); m_boomBoomRightLimit.enableLimitSwitch(true); m_boomBoomLeftLimit.enableLimitSwitch(true); + SmartDashboard.putBoolean("Right Limit Switch Enabled", m_boomBoomRightLimit.isLimitSwitchEnabled()); + SmartDashboard.putBoolean("Left Limit Switch Enabled", m_boomBoomLeftLimit.isLimitSwitchEnabled()); - m_boomBoomRotateMotor.enableSoftLimit(SoftLimitDirection.kForward, false); - m_boomBoomRotateMotor.enableSoftLimit(SoftLimitDirection.kReverse, true); - m_boomBoomRotateMotor.setSoftLimit(SoftLimitDirection.kForward, ShooterConstants.TURRET_RIGHT_SOFT_LIMIT); // Set + // m_boomBoomRotateMotor.enableSoftLimit(SoftLimitDirection.kForward, false); + // m_boomBoomRotateMotor.enableSoftLimit(SoftLimitDirection.kReverse, true); + // m_boomBoomRotateMotor.setSoftLimit(SoftLimitDirection.kForward, ShooterConstants.TURRET_RIGHT_SOFT_LIMIT); // Set // second // soft // limit diff --git a/src/main/java/frc4388/utility/CSV.java b/src/main/java/frc4388/utility/CSV.java new file mode 100644 index 0000000..522980a --- /dev/null +++ b/src/main/java/frc4388/utility/CSV.java @@ -0,0 +1,240 @@ +/*----------------------------------------------------------------------------*/ +/* 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. Private fields and fields of primitive types are 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 final class ReflectionTable { + public static String create(final T[] objects, boolean colored) { + 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()); + if (colored) + 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())); + }); + MessageFormat formatFormat = new MessageFormat(colored ? "{2} %{0}{1}s {3}" : " %{0}{1}s "); + return rows.stream().map(row -> IntStream.range(0, row.size()).mapToObj(i -> String.format(formatFormat.format(new Object[] { row.get(i).padRight ? "-" : "", columnWidths[i], row.get(i).escape, RESET_STYLE }), row.get(i).string)).collect(Collectors.joining("|"))).collect(Collectors.joining(LF)); + } + + private static final Color GRADIENT_MIN = new Color(0x00, 0x99, 0x00); + private static final Color GRADIENT_MAX = new Color(0x66, 0xFF, 0x66); + 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 range = max.doubleValue() - min.doubleValue(); + final double normal = range == 0 ? 0 : (getValue().doubleValue() - min.doubleValue()) / range; + 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 2a5ce92..0000000 --- a/src/main/java/frc4388/utility/ShooterTables.java +++ /dev/null @@ -1,164 +0,0 @@ -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