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

Rewrite shooter tuner

Browse Source
This commit is contained in:
nathanrsxtn
2022-03-25 08:32:58 -06:00
parent 042a03a58f
commit ba60f44e67
4 changed files with 130 additions and 43 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/main/java/frc4388/robot/Robot.java
+3 -3
View File
@@ -96,9 +96,9 @@ public class Robot extends TimedRobot {
Vector2D secondBallPosition = new Vector2D(-(40.44 * (Math.sqrt(2.00) / 2.00)) - ((82.83 - 7.58) * (Math.sqrt(2.00) / 2.00)) - (82.83 / 2.00), -(40.44 * (Math.sqrt(2.00) / 2.00)) + ((82.83 - 7.58) * (Math.sqrt(2.00) / 2.00)) - (219.25 / 2.00)); // * position of second ball, relative to hub.
Vector2D firstToSecond = Vector2D.subtract(secondBallPosition, firstBallPosition);
System.out.println("First Ball (FEET): " + Vector2D.divide(firstBallPosition, 12).toString());
System.out.println("Second Ball (FEET): " + Vector2D.divide(secondBallPosition, 12).toString());
System.out.println("First To Second (FEET): " + Vector2D.divide(firstToSecond, 12).toString());
// System.out.println("First Ball (FEET): " + Vector2D.divide(firstBallPosition, 12).toString());
// System.out.println("Second Ball (FEET): " + Vector2D.divide(secondBallPosition, 12).toString());
// System.out.println("First To Second (FEET): " + Vector2D.divide(firstToSecond, 12).toString());
// current =
// m_robotContainer.m_robotBoomBoom.getCurrent() +
src/main/java/frc4388/robot/RobotContainer.java
+4 -34
View File
@@ -43,6 +43,7 @@ import frc4388.robot.Constants.StorageConstants;
import frc4388.robot.Constants.SwerveDriveConstants;
import frc4388.robot.commands.PathRecorder;
import frc4388.robot.commands.RunCommandForTime;
import frc4388.robot.commands.ShooterTuner;
import frc4388.robot.commands.DriveCommands.DriveWithInputForTime;
import frc4388.robot.commands.ExtenderIntakeCommands.ExtenderIntakeGroup;
import frc4388.robot.commands.ShooterCommands.TrackTarget;
@@ -91,6 +92,8 @@ public class RobotContainer {
/* Autonomous */
private final PathRecorder m_pathChooser = new PathRecorder(m_robotSwerveDrive);
private final ShooterTuner m_shooterTuner = new ShooterTuner(m_robotBoomBoom);
// Controllers
private final static XboxController m_driverXbox = new DeadbandedXboxController(OIConstants.XBOX_DRIVER_ID);
private final static XboxController m_operatorXbox = new DeadbandedXboxController(OIConstants.XBOX_OPERATOR_ID);
@@ -273,40 +276,7 @@ public class RobotContainer {
m_robotBoomBoom.setDefaultCommand(
new RunCommand(() -> m_robotBoomBoom.runDrumShooter(0.45), m_robotBoomBoom)
);
if (m_robotBoomBoom.readManualData) {
var tab = Shuffleboard.getTab("Manual Shooter Data");
var manual = tab.getLayout("Manual Shooter Data", BuiltInLayouts.kList).withSize(2, 3);
manual.add("Manual Shooter Data", new Sendable() {
private double manualHoodExt;
private double manualDrumVel;
@Override
public void initSendable(SendableBuilder builder) {
builder.setSmartDashboardType("RobotPreferences");
builder.addBooleanProperty("Disable CSV", () -> m_robotBoomBoom.readManualData, b -> m_robotBoomBoom.readManualData = b);
builder.addDoubleProperty("Drum Velocity", () -> manualDrumVel, d -> manualDrumVel = d);
builder.addDoubleProperty("Hood Extension", () -> manualHoodExt, d -> manualHoodExt = d);
builder.addDoubleProperty("Measured Distance", () -> SmartDashboard.getNumber("Distance to Target", -1), System.out::println);
}
});
manual.add("Shooter Table Appender", new InstantCommand(() -> Errors.log().run(() -> Files.write(new File(Filesystem.getDeployDirectory(), "ShooterData.csv").toPath(), String.format("%s,%s,%s%n", SmartDashboard.getNumber("Manual Shooter Data/Distance to Target", -1), SmartDashboard.getNumber("Manual Shooter Data/Hood Extension", -1), SmartDashboard.getNumber("Manual Shooter Data/Drum Velocity", -1)).getBytes(), StandardOpenOption.WRITE, StandardOpenOption.APPEND))) {
@Override
public boolean runsWhenDisabled() {
return true;
}
}.withName("Append"));
var csv = tab.getLayout("Shooter Data", BuiltInLayouts.kList).withPosition(2,0).withSize(4, 3);
csv.add("Initial Shooter Data", builder -> Arrays.stream(m_robotBoomBoom.m_shooterTable).forEach(e -> builder.addDoubleArrayProperty(Double.toString(e.distance), () -> new double[] {e.hoodExt, e.drumVelocity}, a -> {})));
csv.add("Reload Data", new InstantCommand(m_robotBoomBoom::updateShooterTable) {
@Override
public boolean runsWhenDisabled() {
return true;
}
}.withName("Reload"));
}
SmartDashboard.putData("Shooter Tuner", m_shooterTuner);
}
/**
src/main/java/frc4388/robot/commands/ShooterTuner.java
+117
View File
@@ -0,0 +1,117 @@
package frc4388.robot.commands;
import java.io.File;
import java.io.OutputStream;
import java.nio.file.Files;
import java.nio.file.StandardOpenOption;
import java.util.Arrays;
import com.diffplug.common.base.Errors;
import edu.wpi.first.util.sendable.Sendable;
import edu.wpi.first.util.sendable.SendableBuilder;
import edu.wpi.first.wpilibj.Filesystem;
import edu.wpi.first.wpilibj.shuffleboard.BuiltInLayouts;
import edu.wpi.first.wpilibj.shuffleboard.Shuffleboard;
import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
import edu.wpi.first.wpilibj2.command.CommandBase;
import edu.wpi.first.wpilibj2.command.InstantCommand;
import edu.wpi.first.wpilibj2.command.Subsystem;
import frc4388.robot.subsystems.BoomBoom;
import static frc4388.robot.subsystems.BoomBoom.ShooterTableEntry;
public class ShooterTuner extends CommandBase {
private final BoomBoom m_boomBoom;
private final Sendable m_shotEditor;
private final Sendable m_shotCsvAppender;
private final Sendable m_shooterTableView;
// private final Sendable m_shooterTableUpdater;
private OutputStream csvOutputStream;
public ShooterTuner(BoomBoom boomBoom) {
m_boomBoom = boomBoom;
addRequirements(boomBoom);
setName("Enable");
m_shotEditor = new ShotEditor();
m_shotCsvAppender = new PersistentInstantCommand(this::appendCsv).withName("Append");
m_shooterTableView = new ShooterTableEditor();
// m_shooterTableUpdater = new PersistentInstantCommand(m_boomBoom::loadShooterTable).withName("Reload");
csvOutputStream = Errors.rethrow().get(() -> Files.newOutputStream(new File(Filesystem.getDeployDirectory(), "ShooterData.csv").toPath(), StandardOpenOption.WRITE, StandardOpenOption.APPEND));
}
private class ShotEditor implements Sendable {
@Override
public void initSendable(SendableBuilder builder) {
builder.setSmartDashboardType("RobotPreferences");
builder.addBooleanProperty("[Enabled]", ShooterTuner.this::isScheduled, b -> {
if (!b) cancel();
});
builder.addDoubleProperty("Drum Velocity", () -> m_boomBoom.m_shooterTable[0].drumVelocity, d -> m_boomBoom.m_shooterTable[0].drumVelocity = d);
builder.addDoubleProperty("Hood Extension", () -> m_boomBoom.m_shooterTable[0].hoodExt, d -> m_boomBoom.m_shooterTable[0].hoodExt = d);
builder.addDoubleProperty("Measured Distance", () -> SmartDashboard.getNumber("Distance to Target", -1), System.out::println);
}
}
private class ShooterTableEditor implements Sendable {
@Override
public void initSendable(SendableBuilder builder) {
Arrays.stream(m_boomBoom.m_shooterTable).forEach(e -> builder.addDoubleArrayProperty(Double.toString(e.distance), () -> new double[] { e.hoodExt, e.drumVelocity }, a -> {
}));
}
}
private class PersistentInstantCommand extends InstantCommand {
public PersistentInstantCommand(Runnable toRun, Subsystem... requirements) {
super(toRun, requirements);
}
@Override
public boolean runsWhenDisabled() {
return true;
}
}
private void appendCsv() {
String s = String.format("%s,%s,%s%n",
m_boomBoom.m_shooterTable[0].distance,
m_boomBoom.m_shooterTable[0].hoodExt,
m_boomBoom.m_shooterTable[0].drumVelocity
);
byte[] b = s.getBytes();
Errors.log().run(() -> csvOutputStream.write(b));
}
@Override
public void initialize() {
setName("Disable");
var tab = Shuffleboard.getTab("Shooter Tuner");
var manual = tab.getLayout("Manual Shooter Data", BuiltInLayouts.kList).withPosition(0, 0).withSize(2, 3);
manual.add("Manual Shooter Data", m_shotEditor);
manual.add("Shooter Table Appender", m_shotCsvAppender);
var csv = tab.getLayout("Shooter Data", BuiltInLayouts.kList).withPosition(2, 0).withSize(4, 3);
csv.add("Initial Shooter Data", m_shooterTableView);
// csv.add("Reload Data", m_shooterTableUpdater);
ShooterTableEntry dummyEntry = new ShooterTableEntry();
dummyEntry.distance = 0.0;
dummyEntry.hoodExt = 0.0;
dummyEntry.drumVelocity = 0.0;
m_boomBoom.m_shooterTable = new ShooterTableEntry[] { dummyEntry };
Shuffleboard.selectTab("Shooter Tuner");
}
@Override
public void execute() {
m_boomBoom.m_shooterTable[0].distance = SmartDashboard.getNumber("Distance to Target", -1);
}
@Override
public void end(boolean interrupted) {
Errors.log().run(csvOutputStream::close);
m_boomBoom.loadShooterTable();
}
@Override
public boolean runsWhenDisabled() {
return true;
}
}
src/main/java/frc4388/robot/subsystems/BoomBoom.java
+6 -6
View File
@@ -6,6 +6,7 @@ package frc4388.robot.subsystems;
import java.io.File;
import java.io.IOException;
import java.util.Arrays;
import java.util.Comparator;
import java.util.Map;
import java.util.Objects;
@@ -31,7 +32,6 @@ import frc4388.utility.Gains;
import frc4388.utility.NumericData;
public class BoomBoom extends SubsystemBase {
public boolean readManualData = true;
private static final Logger LOGGER = Logger.getLogger(BoomBoom.class.getSimpleName());
public WPI_TalonFX m_shooterFalconLeft;
public WPI_TalonFX m_shooterFalconRight;
@@ -64,7 +64,7 @@ public class BoomBoom extends SubsystemBase {
setShooterGains();
updateShooterTable();
loadShooterTable();
// Run a helper method that logs the contents of the table on a new thread.
new Thread(() -> LOGGER.fine(() -> CSV.ReflectionTable.create(m_shooterTable, RobotBase.isSimulation()))).start();
}
@@ -77,7 +77,7 @@ public class BoomBoom extends SubsystemBase {
* @return Drum Velocity in units per 100 ms
*/
public Double getVelocity(final Double distance) {
return readManualData ? SmartDashboard.getNumber("Manual Shooter Data/Drum Velocity", 0) : NumericData.linearInterpolate(m_shooterTable, distance, e -> e.distance, e -> e.drumVelocity).doubleValue();
return NumericData.linearInterpolate(m_shooterTable, distance, e -> e.distance, e -> e.drumVelocity).doubleValue();
}
/**
@@ -88,7 +88,7 @@ public class BoomBoom extends SubsystemBase {
* @return Hood extension in units
*/
public Double getHood(final Double distance) {
return readManualData ? SmartDashboard.getNumber("Manual Shooter Data/Hood Extension", 0) : NumericData.linearInterpolate(m_shooterTable, distance, e -> e.distance, e -> e.hoodExt).doubleValue();
return NumericData.linearInterpolate(m_shooterTable, distance, e -> e.distance, e -> e.hoodExt).doubleValue();
}
@Override
@@ -140,7 +140,7 @@ public class BoomBoom extends SubsystemBase {
// m_shooterFalconLeft.set(m_controller.calculate(m_shooterFalconLeft.get(), targetVel));
}
public void updateShooterTable() {
public void loadShooterTable() {
try {
// This is a helper class that allows us to read a CSV file into a Java array.
@@ -164,7 +164,7 @@ public class BoomBoom extends SubsystemBase {
}
};
// This is reading the CSV file into a Java array.
m_shooterTable = csv.read(new File(Filesystem.getDeployDirectory(), "ShooterData.csv").toPath());
m_shooterTable = Arrays.stream(csv.read(new File(Filesystem.getDeployDirectory(), "ShooterData.csv").toPath())).sorted(Comparator.comparingDouble(e -> e.distance)).toArray(ShooterTableEntry[]::new);
} catch (IOException exception) {
ShooterTableEntry dummyEntry = new ShooterTableEntry();
dummyEntry.distance = 0.0;