From 416d81d348b07d2ba765e5846542009bd9d49371 Mon Sep 17 00:00:00 2001 From: GearBoxFox <98189655+GearBoxFox@users.noreply.github.com> Date: Sat, 20 Jan 2024 14:27:08 -0800 Subject: [PATCH] another mass update of files --- src/main/java/frc/robot/RobotContainer.java | 2 +- .../commands/FeedForwardCharacterization.java | 13 +++--- .../subsystems/drive/DriveSubsystem.java | 21 +++++---- .../frc/robot/subsystems/drive/GyroIO.java | 4 +- .../robot/subsystems/drive/GyroIOPigeon1.java | 45 +++++++++---------- 5 files changed, 42 insertions(+), 43 deletions(-) diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 9b0ee378..82d9ea5a 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -57,7 +57,7 @@ public RobotContainer() { case REAL -> { // Real robot, instantiate hardware IO implementations m_driveSubsystem = new DriveSubsystem( - new GyroIOPigeon1(12, true), + new GyroIOPigeon1(12), new ModuleIOTalonFX(Constants.DriveConstants.FL_MOD_CONSTANTS), new ModuleIOTalonFX(Constants.DriveConstants.FR_MOD_CONSTANTS), new ModuleIOTalonFX(Constants.DriveConstants.BL_MOD_CONSTANTS), diff --git a/src/main/java/frc/robot/commands/FeedForwardCharacterization.java b/src/main/java/frc/robot/commands/FeedForwardCharacterization.java index d7ac7a7a..8081bd68 100644 --- a/src/main/java/frc/robot/commands/FeedForwardCharacterization.java +++ b/src/main/java/frc/robot/commands/FeedForwardCharacterization.java @@ -13,6 +13,7 @@ package frc.robot.commands; +import edu.wpi.first.wpilibj.DriverStation; import edu.wpi.first.wpilibj.Timer; import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.Subsystem; @@ -86,7 +87,7 @@ public void add(double velocity, double voltage) { } public void print() { - if (velocityData.size() == 0 || voltageData.size() == 0) { + if (velocityData.isEmpty() || voltageData.isEmpty()) { return; } @@ -96,11 +97,11 @@ public void print() { voltageData.stream().mapToDouble(Double::doubleValue).toArray(), 1); - System.out.println("FF Characterization Results:"); - System.out.println("\tCount=" + Integer.toString(velocityData.size()) + ""); - System.out.println(String.format("\tR2=%.5f", regression.R2())); - System.out.println(String.format("\tkS=%.5f", regression.beta(0))); - System.out.println(String.format("\tkV=%.5f", regression.beta(1))); + DriverStation.reportWarning("FF Characterization Results:", false); + DriverStation.reportWarning("\tCount=" + Integer.toString(velocityData.size()) + "", false); + DriverStation.reportWarning(String.format("\tR2=%.5f", regression.R2()), false); + DriverStation.reportWarning(String.format("\tkS=%.5f", regression.beta(0)), false); + DriverStation.reportWarning(String.format("\tkV=%.5f", regression.beta(1)), false); } } } diff --git a/src/main/java/frc/robot/subsystems/drive/DriveSubsystem.java b/src/main/java/frc/robot/subsystems/drive/DriveSubsystem.java index e369e57f..59f063e1 100644 --- a/src/main/java/frc/robot/subsystems/drive/DriveSubsystem.java +++ b/src/main/java/frc/robot/subsystems/drive/DriveSubsystem.java @@ -33,6 +33,8 @@ import frc.robot.subsystems.drive.module.Module; import frc.robot.subsystems.drive.module.ModuleIO; import frc.robot.util.LocalADStarAK; + +import java.util.List; import java.util.concurrent.locks.Lock; import java.util.concurrent.locks.ReentrantLock; import org.littletonrobotics.junction.AutoLogOutput; @@ -51,7 +53,7 @@ public class DriveSubsystem extends SubsystemBase { private final GyroIOInputsAutoLogged gyroInputs = new GyroIOInputsAutoLogged(); private final Module[] modules = new Module[4]; // FL, FR, BL, BR - private SwerveDriveKinematics kinematics = new SwerveDriveKinematics(getModuleTranslations()); + private final SwerveDriveKinematics kinematics = new SwerveDriveKinematics(getModuleTranslations()); private Pose2d pose = new Pose2d(); private Rotation2d lastGyroRotation = new Rotation2d(); @@ -81,16 +83,13 @@ MAX_LINEAR_SPEED, DRIVE_BASE_RADIUS, new ReplanningConfig()), this); Pathfinding.setPathfinder(new LocalADStarAK()); PathPlannerLogging.setLogActivePathCallback( - (activePath) -> { - Logger.recordOutput( - "Odometry/Trajectory", activePath.toArray(new Pose2d[activePath.size()])); - }); + (List activePath) -> Logger.recordOutput( + "Odometry/Trajectory", activePath.toArray(new Pose2d[0]))); PathPlannerLogging.setLogTargetPoseCallback( - (targetPose) -> { - Logger.recordOutput("Odometry/TrajectorySetpoint", targetPose); - }); + (Pose2d targetPose) -> Logger.recordOutput("Odometry/TrajectorySetpoint", targetPose)); } + @Override public void periodic() { odometryLock.lock(); // Prevents odometry updates while reading data gyroIO.updateInputs(gyroInputs); @@ -111,8 +110,8 @@ public void periodic() { } // Log empty setpoint states when disabled if (DriverStation.isDisabled()) { - Logger.recordOutput("SwerveStates/Setpoints", new SwerveModuleState[] {}); - Logger.recordOutput("SwerveStates/SetpointsOptimized", new SwerveModuleState[] {}); + Logger.recordOutput("SwerveStates/Setpoints"); + Logger.recordOutput("SwerveStates/SetpointsOptimized"); } // Update odometry @@ -203,7 +202,7 @@ public double getCharacterizationVelocity() { return driveVelocityAverage / 4.0; } - /** Returns the module states (turn angles and drive velocities) for all of the modules. */ + /** Returns the module states (turn angles and drive velocities) for all the modules. */ @AutoLogOutput(key = "SwerveStates/Measured") private SwerveModuleState[] getModuleStates() { SwerveModuleState[] states = new SwerveModuleState[4]; diff --git a/src/main/java/frc/robot/subsystems/drive/GyroIO.java b/src/main/java/frc/robot/subsystems/drive/GyroIO.java index 38ccec1f..bcf122ed 100644 --- a/src/main/java/frc/robot/subsystems/drive/GyroIO.java +++ b/src/main/java/frc/robot/subsystems/drive/GyroIO.java @@ -18,12 +18,12 @@ public interface GyroIO { @AutoLog - public static class GyroIOInputs { + class GyroIOInputs { public boolean connected = false; public Rotation2d yawPosition = new Rotation2d(); public Rotation2d[] odometryYawPositions = new Rotation2d[] {}; public double yawVelocityRadPerSec = 0.0; } - public default void updateInputs(GyroIOInputs inputs) {} + default void updateInputs(GyroIOInputs inputs) {} } diff --git a/src/main/java/frc/robot/subsystems/drive/GyroIOPigeon1.java b/src/main/java/frc/robot/subsystems/drive/GyroIOPigeon1.java index 5d127d11..dee1c14c 100644 --- a/src/main/java/frc/robot/subsystems/drive/GyroIOPigeon1.java +++ b/src/main/java/frc/robot/subsystems/drive/GyroIOPigeon1.java @@ -3,35 +3,34 @@ import com.ctre.phoenix.sensors.PigeonIMU; import edu.wpi.first.math.geometry.Rotation2d; import edu.wpi.first.math.util.Units; -import frc.robot.subsystems.drive.module.PhoenixOdometryThread; import frc.robot.subsystems.drive.module.SparkMaxOdometryThread; import java.util.Queue; public class GyroIOPigeon1 implements GyroIO { - private final PigeonIMU m_pigeon; - private double prevYaw; - private Queue yawPositionQueue; + private final PigeonIMU m_pigeon; + private double prevYaw; + private Queue yawPositionQueue; - public GyroIOPigeon1(int id, boolean pheonixDrive) { - m_pigeon = new PigeonIMU(id); - m_pigeon.configFactoryDefault(); - m_pigeon.setYaw(0.0); - yawPositionQueue = - SparkMaxOdometryThread.getInstance().registerSignal(m_pigeon::getYaw); - } + public GyroIOPigeon1(int id) { + m_pigeon = new PigeonIMU(id); + m_pigeon.configFactoryDefault(); + m_pigeon.setYaw(0.0); + yawPositionQueue = + SparkMaxOdometryThread.getInstance().registerSignal(m_pigeon::getYaw); + } - @Override - public void updateInputs(GyroIOInputs inputs) { - inputs.connected = m_pigeon.getState() == PigeonIMU.PigeonState.Ready; - inputs.yawPosition = Rotation2d.fromDegrees(m_pigeon.getYaw()); - inputs.yawVelocityRadPerSec = (m_pigeon.getYaw() - prevYaw) / Units.millisecondsToSeconds(20); - prevYaw = m_pigeon.getYaw(); + @Override + public void updateInputs(GyroIOInputs inputs) { + inputs.connected = m_pigeon.getState() == PigeonIMU.PigeonState.Ready; + inputs.yawPosition = Rotation2d.fromDegrees(m_pigeon.getYaw()); + inputs.yawVelocityRadPerSec = (m_pigeon.getYaw() - prevYaw) / Units.millisecondsToSeconds(20); + prevYaw = m_pigeon.getYaw(); - inputs.odometryYawPositions = - yawPositionQueue.stream() - .map(Rotation2d::fromDegrees) - .toArray(Rotation2d[]::new); - yawPositionQueue.clear(); - } + inputs.odometryYawPositions = + yawPositionQueue.stream() + .map(Rotation2d::fromDegrees) + .toArray(Rotation2d[]::new); + yawPositionQueue.clear(); + } }