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

Add the correct version of the code this time

Browse Source
This commit is contained in:
nathanrsxtn
2021-10-07 17:32:50 -06:00
parent a17427f6c8
commit 2f89edb04c
2 changed files with 61 additions and 179 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/main/java/frc4388/robot/subsystems/Shooter.java
+36 -112
View File
@@ -25,45 +25,18 @@ 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.Trims;
import frc4388.utility.controller.IHandController;
public class Shooter extends SubsystemBase {
public WPI_TalonFX m_shooterFalconLeft = new WPI_TalonFX(ShooterConstants.SHOOTER_FALCON_BALLER_ID);
public WPI_TalonFX m_shooterFalconRight = new WPI_TalonFX(ShooterConstants.SHOOTER_FALCON_BALLER_FOLLOWER_ID);
public static Gains m_drumShooterGains = ShooterConstants.DRUM_SHOOTER_GAINS;
public static Shooter m_shooter;
public static IHandController m_controller;
public final WPI_TalonFX m_shooterFalconLeft = new WPI_TalonFX(ShooterConstants.SHOOTER_FALCON_BALLER_ID);
public final WPI_TalonFX m_shooterFalconRight = new WPI_TalonFX(ShooterConstants.SHOOTER_FALCON_BALLER_FOLLOWER_ID);
double velP;
double input;
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;
}
public static class ShooterTableEntry {
public Double distance, hoodExt, drumVelocity, centerDisplacement;
}
private ShooterDataEntry[] m_shooterTable;
private ShooterTableEntry[] m_shooterTable;
public boolean m_isDrumReady = false;
public double m_fireVel;
@@ -74,8 +47,7 @@ public class Shooter extends SubsystemBase {
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 adjuster.
*/
public Shooter() {
// Testing purposes reseting gyros
@@ -99,55 +71,32 @@ public class Shooter extends SubsystemBase {
m_shooterFalconRight.configClosedloopRamp(0.75, ShooterConstants.SHOOTER_TIMEOUT_MS);
setShooterGains();
int closedLoopTimeMs = 1;
final int closedLoopTimeMs = 1;
// 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);
// 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);
try {
m_shooterTable = new CSV<>(ShooterDataEntry::new) {
m_shooterTable = new CSV<>(ShooterTableEntry::new) {
private final Pattern parentheses = Pattern.compile("\\([^\\)]*+\\)");
@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) {
}.read(new File(Filesystem.getDeployDirectory(), "Robot Data - Distances.csv").toPath());
new Thread(() -> System.out.println(CSV.ReflectionTable.create(m_shooterTable))).start();
} catch (final 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
@@ -155,24 +104,17 @@ public class Shooter extends SubsystemBase {
// 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());
SmartDashboard.putNumber("Shooter Current", m_shooterFalconLeft.getSupplyCurrent());
SmartDashboard.putBoolean("Drum Ready", m_isDrumReady);
}
catch (Exception e) {
} catch (final Exception e) {
}
}
/**
* Passes subsystem needed.
*
* @param subsystem Subsystem needed.
*/
public void passRequiredSubsystem(ShooterHood subsystem0, ShooterAim subsystem1) {
@@ -186,7 +128,6 @@ public class Shooter extends SubsystemBase {
/**
* Runs drum shooter motor.
*
* @param speed Speed to set the motor at
*/
public void runDrumShooter(double speed) {
@@ -200,19 +141,14 @@ 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, ShooterConstants.DRUM_SHOOTER_GAINS.m_kF, ShooterConstants.SHOOTER_TIMEOUT_MS);
m_shooterFalconLeft.config_kP(ShooterConstants.SHOOTER_SLOT_IDX, ShooterConstants.DRUM_SHOOTER_GAINS.m_kP, ShooterConstants.SHOOTER_TIMEOUT_MS);
m_shooterFalconLeft.config_kI(ShooterConstants.SHOOTER_SLOT_IDX, ShooterConstants.DRUM_SHOOTER_GAINS.m_kI, ShooterConstants.SHOOTER_TIMEOUT_MS);
m_shooterFalconLeft.config_kD(ShooterConstants.SHOOTER_SLOT_IDX, ShooterConstants.DRUM_SHOOTER_GAINS.m_kD, ShooterConstants.SHOOTER_TIMEOUT_MS);
}
/**
* Runs drum shooter velocity PID.
*
* @param targetVel Target velocity to run motor at
*/
public void runDrumShooterVelocityPID(double targetVel) {
@@ -221,44 +157,32 @@ public class Shooter extends SubsystemBase {
m_shooterFalconRight.follow(m_shooterFalconLeft);
}
public Double getCenterDisplacement(Double distance) {
return linearInterpolate(distance, ShooterDataEntry::getDistance, ShooterDataEntry::getCenterDisplacement)
.doubleValue();
public Double getCenterDisplacement(final Double distance) {
return linearInterpolate(m_shooterTable, distance, e -> e.distance, e -> e.centerDisplacement).doubleValue();
}
public Double getVelocity(Double distance) {
return linearInterpolate(distance, ShooterDataEntry::getDistance, ShooterDataEntry::getDrumVelocity).doubleValue();
public Double getVelocity(final Double distance) {
return linearInterpolate(m_shooterTable, distance, e -> e.distance, e -> e.drumVelocity).doubleValue();
}
public Double getHood(Double distance) {
return linearInterpolate(distance, ShooterDataEntry::getDistance, ShooterDataEntry::getHoodExt).doubleValue();
public Double getHood(final Double distance) {
return linearInterpolate(m_shooterTable, distance, e -> e.distance, e -> e.hoodExt).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();
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 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));
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 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 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 Number lerp2(final Number x, final Number x0, final Number y0, final Number x1, final Number y1) {
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();
}