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

Implement new CSV system

Browse Source
This commit is contained in:
nathanrsxtn
2021-10-07 17:02:09 -06:00
parent 8a3db9133f
commit a17427f6c8
6 changed files with 441 additions and 239 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/main/java/frc4388/robot/commands/shooter/TrackTarget.java
+4 -4
View File
@@ -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
src/main/java/frc4388/robot/subsystems/Shooter.java
+146 -45
View File
@@ -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<ShooterDataEntry, Number> lookupGetter,
final Function<ShooterDataEntry, Number> targetGetter) {
final Map.Entry<Integer, ShooterDataEntry> 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<Map.Entry<Integer, ShooterDataEntry>> lookup(final Number value,
final Function<ShooterDataEntry, Number> valueGetter, boolean exactMatch) {
Optional<Map.Entry<Integer, ShooterDataEntry>> 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();
}
}
src/main/java/frc4388/utility/CSV.java
+275
View File
@@ -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<R> {
private static final Pattern SANITIZER = Pattern.compile("[^$\\w,]");
private final Supplier<R> generator;
private final IntFunction<R[]> arrayGenerator;
private final Map<String, BiConsumer<R, String>> 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<R> generator) {
final Class<?> clazz = generator.get().getClass();
final Map<Class<?>, Function<String, ?>> 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<String, ?> 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<R, String>[] fieldSetters = Stream.of(headerSanitizer(reader.readLine()).split(","))
.map(this::nameProcessor).map(setters::get).toArray(BiConsumer[]::new);
final Stream<String> 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<String, ?> 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> R deserializeRecordString(final String recordString,
final BiConsumer<R, String>[] 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<R, String> 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 <T> String create(final T[] objects) {
final Field[] fields = Stream.of(objects).flatMap(object -> Stream.of(object.getClass().getFields()))
.distinct().toArray(Field[]::new);
final List<List<ReflectionTable>> 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;
}
}
}
src/main/java/frc4388/utility/ShooterTables.java
-171
View File
@@ -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;
}
}