mirror of
https://github.com/Team4388/RiseOfRidgebotics2020.git
synced 2026-06-09 00:38:00 -06:00
Implement new CSV system
This commit is contained in:
nathanrsxtn
@@ -1,3 +0,0 @@
|
||||
Angle (deg),Displacement (deg)
|
||||
0,0
|
||||
0,0
|
||||
|
@@ -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,
|
||||
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
|
||||
|
@@ -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
|
||||
|
||||
|
||||
@@ -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();
|
||||
}
|
||||
}
|
||||
@@ -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;
|
||||
}
|
||||
}
|
||||
}
|
||||
@@ -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;
|
||||
}
|
||||
}
|
||||
Reference in New Issue
Block a user