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

shooter tables and shoot command

Browse Source
This commit is contained in:
aarav18
2022-02-25 20:18:21 -07:00
parent 31346ff646
commit 6e55d6fabd
8 changed files with 385 additions and 213 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/main/java/frc4388/robot/subsystems/BoomBoom.java
+59 -26
View File
@@ -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
}
}
src/main/java/frc4388/robot/subsystems/Hood.java
+15 -15
View File
@@ -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);
}
src/main/java/frc4388/robot/subsystems/Turret.java
+5 -3
View File
@@ -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