mirror of
https://github.com/Team4388/2022NoWayHome.git
synced 2026-06-09 08:48:07 -06:00
Merge branch 'Shooter' of https://github.com/Team4388/2022NoWayHome into Shooter
This commit is contained in:
Ryan Manley
@@ -49,9 +49,12 @@ public class AimToCenter extends CommandBase {
|
||||
//m_turret.runshooterRotatePID(m_targetAngle);
|
||||
}
|
||||
|
||||
/**
|
||||
* Checks if in deadzone.
|
||||
* @return True if deadzone.
|
||||
*/
|
||||
public static boolean isDeadzone(double angle) {
|
||||
if ((ShooterConstants.DEADZONE_LEFT > angle) || (angle > ShooterConstants.DEADZONE_RIGHT)) return true;
|
||||
else return false;
|
||||
return ((ShooterConstants.DEADZONE_LEFT > angle) || (angle > ShooterConstants.DEADZONE_RIGHT));
|
||||
}
|
||||
|
||||
// Called once the command ends or is interrupted.
|
||||
|
||||
@@ -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;
|
||||
}
|
||||
}
|
||||
@@ -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<ShooterTableEntry> 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 <E> Number linearInterpolate(final E[] table, final Number lookupValue, final Function<E, Number> lookupGetter, final Function<E, Number> targetGetter) {
|
||||
final Map.Entry<Integer, E> closestEntry = lookup(table, lookupValue.doubleValue(), lookupGetter, false).orElse(Map.entry(table.length - 1, table[table.length - 1]));
|
||||
final E closestRecord = closestEntry.getValue();
|
||||
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 <E> Optional<Map.Entry<Integer, E>> lookup(final E[] table, final Number value, final Function<E, Number> valueGetter, final boolean exactMatch) {
|
||||
final Optional<Map.Entry<Integer, E>> match = IntStream.range(0, table.length).mapToObj(i -> Map.entry(i, table[i])).min(Comparator.comparingDouble(e -> Math.abs(valueGetter.apply(e.getValue()).doubleValue() - value.doubleValue())));
|
||||
return !exactMatch || match.map(e -> valueGetter.apply(e.getValue()).equals(value)).orElse(false) ? match : Optional.empty();
|
||||
}
|
||||
|
||||
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
|
||||
}
|
||||
|
||||
}
|
||||
|
||||
|
||||
@@ -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);
|
||||
}
|
||||
|
||||
|
||||
|
||||
@@ -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
|
||||
|
||||
@@ -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<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. 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<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 final class ReflectionTable {
|
||||
public static <T> 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<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());
|
||||
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;
|
||||
}
|
||||
}
|
||||
}
|
||||
@@ -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;
|
||||
}
|
||||
}
|
||||
Reference in New Issue
Block a user