diff --git a/build.gradle b/build.gradle index 0c2f1feb..03cf3dc8 100644 --- a/build.gradle +++ b/build.gradle @@ -1,7 +1,6 @@ plugins { id "java" id "edu.wpi.first.GradleRIO" version "2024.3.1" - id "com.peterabeles.gversion" version "1.10" id "org.sonarqube" version "4.4.1.3373" } @@ -56,28 +55,6 @@ wpi.java.debugJni = false // Set this to true to enable desktop support. def includeDesktopSupport = true -// Configuration for AdvantageKit -repositories { - maven { - url = uri("https://maven.pkg.github.com/Mechanical-Advantage/AdvantageKit") - credentials { - username = "Mechanical-Advantage-Bot" - password = "\u0067\u0068\u0070\u005f\u006e\u0056\u0051\u006a\u0055\u004f\u004c\u0061\u0079\u0066\u006e\u0078\u006e\u0037\u0051\u0049\u0054\u0042\u0032\u004c\u004a\u006d\u0055\u0070\u0073\u0031\u006d\u0037\u004c\u005a\u0030\u0076\u0062\u0070\u0063\u0051" - } - } - mavenLocal() -} - -configurations.all { - exclude group: "edu.wpi.first.wpilibj" -} - -task(checkAkitInstall, dependsOn: "classes", type: JavaExec) { - mainClass = "org.littletonrobotics.junction.CheckInstall" - classpath = sourceSets.main.runtimeClasspath -} -compileJava.finalizedBy checkAkitInstall - // Defining my dependencies. In this case, WPILib (+ friends), and vendor libraries. // Also defines JUnit 4. dependencies { @@ -99,9 +76,6 @@ dependencies { simulationRelease wpi.sim.enableRelease() implementation 'gov.nist.math:jama:1.0.3' - - def akitJson = new groovy.json.JsonSlurper().parseText(new File(projectDir.getAbsolutePath() + "/vendordeps/AdvantageKit.json").text) - annotationProcessor "org.littletonrobotics.akit.junction:junction-autolog:$akitJson.version" } test { @@ -147,15 +121,4 @@ tasks.withType(JavaCompile) { } -// Create version file -project.compileJava.dependsOn(createVersionFile) -gversion { - srcDir = "src/main/java/" - classPackage = "frc.robot" - className = "BuildConstants" - dateFormat = "yyyy-MM-dd HH:mm:ss z" - timeZone = "America/New_York" - indent = " " -} - diff --git a/src/main/deploy/pathplanner/autos/1M Auto.auto b/src/main/deploy/pathplanner/autos/1M Auto.auto new file mode 100644 index 00000000..77afe835 --- /dev/null +++ b/src/main/deploy/pathplanner/autos/1M Auto.auto @@ -0,0 +1,19 @@ +{ + "version": 1.0, + "startingPose": null, + "command": { + "type": "sequential", + "data": { + "commands": [ + { + "type": "path", + "data": { + "pathName": "1M Path" + } + } + ] + } + }, + "folder": null, + "choreoAuto": false +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/New New Path.path b/src/main/deploy/pathplanner/paths/1M Path.path similarity index 87% rename from src/main/deploy/pathplanner/paths/New New Path.path rename to src/main/deploy/pathplanner/paths/1M Path.path index be983412..7630c6eb 100644 --- a/src/main/deploy/pathplanner/paths/New New Path.path +++ b/src/main/deploy/pathplanner/paths/1M Path.path @@ -16,12 +16,12 @@ }, { "anchor": { - "x": 4.0, - "y": 6.0 + "x": 2.34, + "y": 5.55 }, "prevControl": { - "x": 3.0, - "y": 6.0 + "x": 1.3399999999999999, + "y": 5.55 }, "nextControl": null, "isLocked": false, @@ -39,7 +39,7 @@ }, "goalEndState": { "velocity": 0, - "rotation": 0, + "rotation": 0.10655751049247228, "rotateFast": false }, "reversed": false, diff --git a/src/main/deploy/pathplanner/paths/New New New Path.path b/src/main/deploy/pathplanner/paths/New New New Path.path new file mode 100644 index 00000000..1d7579f5 --- /dev/null +++ b/src/main/deploy/pathplanner/paths/New New New Path.path @@ -0,0 +1,52 @@ +{ + "version": 1.0, + "waypoints": [ + { + "anchor": { + "x": 0.0, + "y": 0.0 + }, + "prevControl": null, + "nextControl": { + "x": 1.0000000000000013, + "y": 0.0 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 1.0, + "y": 0.0 + }, + "prevControl": { + "x": 0.0, + "y": 0.0 + }, + "nextControl": null, + "isLocked": false, + "linkedName": null + } + ], + "rotationTargets": [], + "constraintZones": [], + "eventMarkers": [], + "globalConstraints": { + "maxVelocity": 3.0, + "maxAcceleration": 3.0, + "maxAngularVelocity": 540.0, + "maxAngularAcceleration": 720.0 + }, + "goalEndState": { + "velocity": 0, + "rotation": 90.05397463575794, + "rotateFast": false + }, + "reversed": false, + "folder": null, + "previewStartingState": { + "rotation": 0, + "velocity": 0 + }, + "useDefaultConstraints": true +} \ No newline at end of file diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java index 339523e9..50676892 100644 --- a/src/main/java/frc/robot/Constants.java +++ b/src/main/java/frc/robot/Constants.java @@ -12,18 +12,17 @@ // GNU General Public License for more details. package frc.robot; -import edu.wpi.first.math.geometry.Pose3d; -import edu.wpi.first.math.geometry.Translation2d; -import edu.wpi.first.math.geometry.Translation3d; +import edu.wpi.first.math.geometry.*; import com.gos.lib.properties.GosDoubleProperty; import com.pathplanner.lib.path.PathConstraints; import com.pathplanner.lib.util.HolonomicPathFollowerConfig; import com.pathplanner.lib.util.PIDConstants; import com.pathplanner.lib.util.ReplanningConfig; -import edu.wpi.first.math.geometry.Rotation3d; -import edu.wpi.first.math.geometry.Transform3d; import edu.wpi.first.math.geometry.Translation2d; import edu.wpi.first.math.geometry.Translation3d; +import edu.wpi.first.math.trajectory.Trajectory; +import edu.wpi.first.math.trajectory.TrajectoryConfig; +import edu.wpi.first.math.trajectory.TrajectoryGenerator; import edu.wpi.first.math.util.Units; import edu.wpi.first.wpilibj.RobotBase; import edu.wpi.first.units.Unit; @@ -31,6 +30,8 @@ import frc.robot.subsystems.arm.ArmSubsystem; import frc.robot.subsystems.drive.module.ModuleConstants; +import java.util.List; + /** * The Constants class provides a convenient place for teams to hold robot-wide numerical or boolean * constants. This class should not be used for any other purpose. All constants should be declared @@ -92,12 +93,12 @@ private DriveConstants() { // new Rotation3d(0.0, Units.degreesToRadians(50), Units.degreesToRadians(18)) // ); - public static final Transform3d LEFT_CAMERA_TRANSFORMATION = new Transform3d( - new Translation3d(Units.inchesToMeters(-11.25), Units.inchesToMeters(9.0), Units.inchesToMeters(6.0)), - new Rotation3d(Units.degreesToRadians(5.0), Units.degreesToRadians(-28.125), Units.degreesToRadians(35.0 + 180)) + public static final Transform3d RIGHT_CAMERA_TRANSFORMATION = new Transform3d( + new Translation3d(Units.inchesToMeters(12.0), Units.inchesToMeters(6.0), Units.inchesToMeters(7.8)), + new Rotation3d(Units.degreesToRadians(0.0), Units.degreesToRadians(20.0), -Units.degreesToRadians(7.5)) ); - public static final Transform3d RIGHT_CAMERA_TRANSFORMATION = new Transform3d( + public static final Transform3d LEFT_CAMERA_TRANSFORMATION = new Transform3d( new Translation3d(Units.inchesToMeters(-11.25), Units.inchesToMeters(-9.0), Units.inchesToMeters(6.0)), new Rotation3d(Units.degreesToRadians(2.0), Units.degreesToRadians(-26.0), Units.degreesToRadians(-35.0 - 180)) ); @@ -172,8 +173,8 @@ private ArmConstants() {} /* Because the absolute encoders are on a 2/1 ratio, we have to move our offset down a little into a rotation lower * than the lowest point the arm and wrist will move to , and then compensate for that in our encoder reset code */ public static final double OFFSET_NUDGE = 45; - public static final double ARM_OFFSET = -0.604004 + Units.degreesToRotations(OFFSET_NUDGE); - public static final double WRIST_OFFSET = -0.108643 + Units.degreesToRotations(OFFSET_NUDGE); + public static final double ARM_OFFSET = -0.123779 + Units.degreesToRotations(OFFSET_NUDGE); + public static final double WRIST_OFFSET = -0.163330 + Units.degreesToRotations(OFFSET_NUDGE); public static final double ARM_SENSOR_MECHANISM_RATIO = (56.0 / 12.0) * (66.0 / 18.0) * (80.0 / 18.0) * (64.0 / 24.0); public static final double ARM_CANCODER_MECHANISM_RATIO = (26.0 / 36.0) * (64.0 / 24.0); @@ -196,17 +197,17 @@ private ArmConstants() {} public static final double ARM_KG = 0.375; public static final GosDoubleProperty WRIST_LOWER_LIMIT = - new GosDoubleProperty(false, "Arm/WristLowerLimit", 30); + new GosDoubleProperty(true, "Arm/WristLowerLimit", 0); public static final GosDoubleProperty WRIST_UPPER_LIMIT = - new GosDoubleProperty(false, "Arm/WristUpperLimit", 30); + new GosDoubleProperty(true, "Arm/WristUpperLimit", 180); public static final GosDoubleProperty ARM_LOWER_LIMIT = - new GosDoubleProperty(false, "Arm/ArmLowerLimit", 30); + new GosDoubleProperty(false, "Arm/ArmLowerLimit", -9); public static final GosDoubleProperty ARM_UPPER_LIMIT = - new GosDoubleProperty(false, "Arm/ArmUpperLimit", 30); + new GosDoubleProperty(true, "Arm/ArmUpperLimit", 180); public static final GosDoubleProperty WRIST_ARM_GAP = - new GosDoubleProperty(false, "Arm/Wrist Gap", 20); + new GosDoubleProperty(true, "Arm/Wrist Gap", 10); public static final Translation2d PIVOT_JOINT_TRANSLATION = new Translation2d(Units.inchesToMeters(9.27), @@ -223,20 +224,47 @@ private ArmConstants() {} } public static class ArmSetpoints { - public static final ArmPose AMP_INTERMEDIATE = new ArmPose("ArmPoses/Amp Intermediate", false, 60.0, 145.0); - private ArmSetpoints() { throw new IllegalStateException("Static classes should not be constructed"); } + public static final ArmPose AMP_INTERMEDIATE = new ArmPose("ArmPoses/Amp Intermediate", false, 60.0, 145.0); + public static final ArmPose STOW_SETPOINT = new - ArmPose("ArmPoses/Stow", false, 0.0, 45.0); + ArmPose("ArmPoses/Stow", true, 0.0, 45.0); public static final ArmPose INTAKE_SETPOINT = - new ArmPose("ArmPoses/Intake", false, 0.0, 35.0); + new ArmPose("ArmPoses/Intake", true, 0.0, 55.0); public static final ArmPose AMP_SETPOINT = - new ArmPose("ArmPoses/Amp", false, 90.0, 135.0); + new ArmPose("ArmPoses/Amp", true, 94.0, 145.0); public static final GosDoubleProperty WRIST_ANGLE = new GosDoubleProperty(false, "Wrist Angle", 45.0); + + public static final Trajectory STOW_AMP_TRAJ; + public static final Trajectory AMP_STOW_TRAJ; + + static { + // kinda janky(?) spline generation + // use Pose2d x for arm angle and y for wrist angle, ignore heading + var stowPose = new Pose2d(0.0, 45.0, new Rotation2d()); + var ampPose = new Pose2d(94.0, 145.0, new Rotation2d()); + var ampIntermediatePose = List.of(new Pose2d(60.0, 145.0, new Rotation2d()).getTranslation()); + + var trajConfig = new TrajectoryConfig(Units.degreesToRotations(30), Units.degreesToRotations(30)); + + STOW_AMP_TRAJ = TrajectoryGenerator.generateTrajectory( + stowPose, + ampIntermediatePose, + ampPose, + trajConfig + ); + + AMP_STOW_TRAJ = TrajectoryGenerator.generateTrajectory( + ampPose, + ampIntermediatePose, + stowPose, + trajConfig + ); + } } public static class ShooterConstants { diff --git a/src/main/java/frc/robot/Robot.java b/src/main/java/frc/robot/Robot.java index 21e239dd..cc114c87 100644 --- a/src/main/java/frc/robot/Robot.java +++ b/src/main/java/frc/robot/Robot.java @@ -15,16 +15,16 @@ import com.ctre.phoenix6.SignalLogger; import com.gos.lib.properties.PropertyManager; +import edu.wpi.first.networktables.NetworkTableInstance; +import edu.wpi.first.util.datalog.StringLogEntry; +import edu.wpi.first.wpilibj.DataLogManager; +import edu.wpi.first.wpilibj.DriverStation; import edu.wpi.first.wpilibj.PowerDistribution; +import edu.wpi.first.wpilibj.TimedRobot; import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.CommandScheduler; import lib.factories.TalonFXFactory; -import org.littletonrobotics.junction.LogFileUtil; -import org.littletonrobotics.junction.LoggedRobot; -import org.littletonrobotics.junction.Logger; -import org.littletonrobotics.junction.networktables.NT4Publisher; -import org.littletonrobotics.junction.wpilog.WPILOGReader; -import org.littletonrobotics.junction.wpilog.WPILOGWriter; +import lib.logger.DataLogUtil; /** * The VM is configured to automatically run this class, and to call the functions corresponding to @@ -32,7 +32,7 @@ * the package after creating this project, you must also update the build.gradle file in the * project. */ -public class Robot extends LoggedRobot { +public class Robot extends TimedRobot { private Command autonomousCommand; private RobotContainer robotContainer; private PowerDistribution pdp; @@ -45,60 +45,29 @@ public class Robot extends LoggedRobot { public void robotInit() { // Clear dead properties PropertyManager.purgeExtraKeys(); - // Record metadata - Logger.recordMetadata("ProjectName", BuildConstants.MAVEN_NAME); - Logger.recordMetadata("BuildDate", BuildConstants.BUILD_DATE); - Logger.recordMetadata("GitSHA", BuildConstants.GIT_SHA); - Logger.recordMetadata("GitDate", BuildConstants.GIT_DATE); - Logger.recordMetadata("GitBranch", BuildConstants.GIT_BRANCH); - String gitDirty = "GitDirty"; - switch (BuildConstants.DIRTY) { - case 0: - Logger.recordMetadata(gitDirty, "All changes committed"); - break; - case 1: - Logger.recordMetadata(gitDirty, "Uncomitted changes"); - break; - default: - Logger.recordMetadata(gitDirty, "Unknown"); - break; - } - - SignalLogger.start(); - // Set up data receivers & replay source - switch (Constants.currentMode) { - case REAL, PROTO_SHOOTER, PROTO_ARM: - // Running on a real robot, log to a USB stick ("/U/logs") - Logger.addDataReceiver(new WPILOGWriter("/media/sda1/aoede")); - Logger.addDataReceiver(new NT4Publisher()); - break; - - case SIM: - // Running a physics simulator, log to NT - Logger.addDataReceiver(new NT4Publisher()); - break; - - case REPLAY: - // Replaying a log, set up replay source - setUseTiming(false); // Run as fast as possible - String logPath = LogFileUtil.findReplayLog(); - Logger.setReplaySource(new WPILOGReader(logPath)); - Logger.addDataReceiver(new WPILOGWriter(LogFileUtil.addPathSuffix(logPath, "_sim"))); - break; - } + String logPath = "/media/sda1/aoide"; - // See http://bit.ly/3YIzFZ6 for more information on timestamps in AdvantageKit. - // Logger.disableDeterministicTimestamps() + DataLogManager.start(logPath); + DriverStation.startDataLog(DataLogManager.getLog()); - // Start AdvantageKit logger - Logger.start(); + SignalLogger.setPath(logPath); + SignalLogger.start(); // Instantiate our RobotContainer. This will perform all our button bindings, // and put our autonomous chooser on the dashboard. robotContainer = new RobotContainer(); pdp = new PowerDistribution(); + + final StringLogEntry entry = new StringLogEntry(DataLogManager.getLog(), "/ntlog"); + NetworkTableInstance.getDefault() + .addLogger( + 0, + 100, + event -> + entry.append( + event.logMessage.filename + ":" + event.logMessage.line + ":" + event.logMessage.message)); } /** This function is called periodically during all modes. */ @@ -111,8 +80,7 @@ public void robotPeriodic() { // the Command-based framework to work. CommandScheduler.getInstance().run(); TalonFXFactory.handleFaults(); - - Logger.recordOutput("Switchable On?", pdp.getSwitchableChannel()); + DataLogUtil.updateTables(); } /** This function is called once when the robot is disabled. */ diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 8cc3e123..6117e9ab 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -25,6 +25,7 @@ import edu.wpi.first.wpilibj.XboxController; import edu.wpi.first.wpilibj.shuffleboard.Shuffleboard; import edu.wpi.first.wpilibj.shuffleboard.ShuffleboardTab; +import edu.wpi.first.wpilibj.smartdashboard.SendableChooser; import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.Commands; import edu.wpi.first.wpilibj2.command.button.CommandXboxController; @@ -46,10 +47,9 @@ import frc.robot.subsystems.shooter.ShooterIOKraken; import frc.robot.subsystems.shooter.ShooterIOSim; import frc.robot.subsystems.shooter.ShooterSubsystem; +import frc.robot.subsystems.vision.VisionSubsystem; import lib.utils.AllianceFlipUtil; import lib.utils.FieldConstants; -import org.littletonrobotics.junction.Logger; -import org.littletonrobotics.junction.networktables.LoggedDashboardChooser; /** * This class is where the bulk of the robot should be declared. Since Command-based is a @@ -68,13 +68,11 @@ public class RobotContainer { private final CommandXboxController controller = new CommandXboxController(0); // Dashboard inputs - private final LoggedDashboardChooser autoChooser; + private final SendableChooser autoChooser; /** The container for the robot. Contains subsystems, OI devices, and commands. */ public RobotContainer() { DriverStation.silenceJoystickConnectionWarning(true); - Logger.recordOutput("Zero Pose3d", new Pose3d()); - Logger.recordOutput("Zeroed Pose3d Array", new Pose3d(), new Pose3d()); switch (Constants.currentMode) { case REAL -> { @@ -84,7 +82,10 @@ public RobotContainer() { new ModuleIOTalonFX(Constants.DriveConstants.FL_MOD_CONSTANTS), new ModuleIOTalonFX(Constants.DriveConstants.FR_MOD_CONSTANTS), new ModuleIOTalonFX(Constants.DriveConstants.BL_MOD_CONSTANTS), - new ModuleIOTalonFX(Constants.DriveConstants.BR_MOD_CONSTANTS)); + new ModuleIOTalonFX(Constants.DriveConstants.BR_MOD_CONSTANTS), + new VisionSubsystem[]{ + new VisionSubsystem("RightCamera", DriveConstants.RIGHT_CAMERA_TRANSFORMATION) + }); m_shooter = new ShooterSubsystem(new ShooterIOKraken()); m_armSubsystem = new ArmSubsystem(new ArmIOKraken(), m_driveSubsystem::getVisionPose); m_climber = new ClimberSubsystem(new ClimberIOKraken() {}); @@ -141,7 +142,7 @@ public RobotContainer() { } // Set up auto routines - autoChooser = new LoggedDashboardChooser<>("Auto Choices", AutoBuilder.buildAutoChooser()); + autoChooser = new SendableChooser<>(); // Set up feedforward characterization autoChooser.addOption( @@ -313,6 +314,6 @@ private void configureDashboard() { * @return the command to run in autonomous */ public Command getAutonomousCommand() { - return autoChooser.get(); + return autoChooser.getSelected(); } } diff --git a/src/main/java/frc/robot/subsystems/arm/ArmIO.java b/src/main/java/frc/robot/subsystems/arm/ArmIO.java index 6ccca449..3c9edf4d 100644 --- a/src/main/java/frc/robot/subsystems/arm/ArmIO.java +++ b/src/main/java/frc/robot/subsystems/arm/ArmIO.java @@ -1,9 +1,7 @@ package frc.robot.subsystems.arm; -import org.littletonrobotics.junction.AutoLog; public interface ArmIO { - @AutoLog class ArmIOInputs { public double armPositionDegs = 0.0; public double wristPositionDegs = 0.0; @@ -24,15 +22,15 @@ class ArmIOInputs { public double wristCurrentDraw = 0.0; } - default void updateInputs(ArmIOInputsAutoLogged inputs) {} + default void updateInputs(ArmIOInputs inputs) {} default void setArmVoltage(double voltage) {} - default void setArmAngle(double degrees) {} + default void setArmAngle(double degrees, double velocity) {} default void setWristVoltage(double voltage) {} - default void setWristAngle(double degrees, boolean track) {} + default void setWristAngle(double degrees, double velocity) {} default void resetPosition() {} default void stop() {} diff --git a/src/main/java/frc/robot/subsystems/arm/ArmIOKraken.java b/src/main/java/frc/robot/subsystems/arm/ArmIOKraken.java index 11604c99..a3493411 100644 --- a/src/main/java/frc/robot/subsystems/arm/ArmIOKraken.java +++ b/src/main/java/frc/robot/subsystems/arm/ArmIOKraken.java @@ -16,7 +16,6 @@ import lib.properties.phoenix6.Phoenix6PidPropertyBuilder; import lib.properties.phoenix6.PidPropertyPublic; import frc.robot.Constants.ArmConstants; -import org.littletonrobotics.junction.Logger; public class ArmIOKraken implements ArmIO { // Physical devices @@ -40,8 +39,8 @@ public class ArmIOKraken implements ArmIO { private final MotionMagicConfigs m_wristMMConfigs = new MotionMagicConfigs(); // Control outputs - private final DynamicMotionMagicVoltage m_dynMMRequest; - private final MotionMagicVoltage m_mmRequest; + private final DynamicMotionMagicVoltage m_wristDynMMRequest; + private final DynamicMotionMagicVoltage m_armDynMMRequest; private final Follower m_armFollowerRequest; private final Follower m_wristFollowerRequest; private final NeutralOut m_stopRequest; @@ -110,11 +109,11 @@ public ArmIOKraken() { m_wristEncoder.getConfigurator().apply(wristEncoderConfig); // config output requests - m_dynMMRequest = new DynamicMotionMagicVoltage(0, 0, 0, 0) + m_wristDynMMRequest = new DynamicMotionMagicVoltage(0, 0, 0, 0) .withEnableFOC(m_armMaster.getIsProLicensed().getValue() && m_wristMaster.getIsProLicensed().getValue()) .withSlot(0); - m_mmRequest = new MotionMagicVoltage(0) + m_armDynMMRequest = new DynamicMotionMagicVoltage(0, 0, 0, 0) .withEnableFOC(m_armMaster.getIsProLicensed().getValue() && m_wristMaster.getIsProLicensed().getValue()) .withSlot(0); @@ -137,22 +136,17 @@ public ArmIOKraken() { .addKG(ArmConstants.WRIST_KG, GravityTypeValue.Arm_Cosine) .build(); - m_armMaxVelDegS = new HeavyDoubleProperty((double maxVel) -> { - m_armMMConfigs.MotionMagicCruiseVelocity = Units.degreesToRotations(maxVel); - m_armMaster.getConfigurator().apply(m_armMMConfigs); - m_armFollower.getConfigurator().apply(m_armMMConfigs); - }, new GosDoubleProperty(false, "Arm/Arm Max Vel DegsS", 120)); + m_armMaxVelDegS = new HeavyDoubleProperty( + (double maxVel) -> m_armDynMMRequest.Velocity = Units.degreesToRotations(maxVel), + new GosDoubleProperty(false, "Arm/Arm Max Vel DegsS", 120)); - m_wristMaxVelDegS = new HeavyDoubleProperty((double maxVel) -> { - m_dynMMRequest.Velocity = Units.degreesToRotations(maxVel); - }, new GosDoubleProperty(false, "Arm/Wrist Max Vel DegsS", 120)); + m_wristMaxVelDegS = new HeavyDoubleProperty( + (double maxVel) -> m_wristDynMMRequest.Velocity = Units.degreesToRotations(maxVel), + new GosDoubleProperty(false, "Arm/Wrist Max Vel DegsS", 120)); m_accelTimeSecs = new HeavyDoubleProperty((double accel) -> { - m_armMMConfigs.MotionMagicAcceleration = m_armMMConfigs.MotionMagicCruiseVelocity / accel; - m_armMaster.getConfigurator().apply(m_armMMConfigs); - m_armFollower.getConfigurator().apply(m_armMMConfigs); - - m_dynMMRequest.Acceleration = m_dynMMRequest.Velocity / accel; + m_armDynMMRequest.Acceleration = m_armDynMMRequest.Velocity / accel; + m_wristDynMMRequest.Acceleration = m_wristDynMMRequest.Velocity / accel; }, new GosDoubleProperty(false, "Arm/Acceleration Time Secs", 1)); m_armMaxVelDegS.updateIfChanged(true); @@ -202,7 +196,7 @@ public ArmIOKraken() { } @Override - public void updateInputs(ArmIOInputsAutoLogged inputs) { + public void updateInputs(ArmIOInputs inputs) { BaseStatusSignal.refreshAll( m_armPositionSignal, m_wristPositionSignal, @@ -238,11 +232,6 @@ public void updateInputs(ArmIOInputsAutoLogged inputs) { inputs.armCurrentDraw = m_armCurrentDrawSignal.getValueAsDouble(); inputs.wristCurrentDraw = m_wristCurrentDrawSignal.getValueAsDouble(); - Logger.recordOutput("Arm Absolute Position", Units.rotationsToDegrees( - m_armEncoder.getAbsolutePosition().getValueAsDouble())); - Logger.recordOutput("Wrist Absolute Position", Units.rotationsToDegrees( - m_wristEncoder.getAbsolutePosition().getValueAsDouble())); - // update any pid properties m_wristProperty.updateIfChanged(); m_armProperty.updateIfChanged(); @@ -259,8 +248,11 @@ public void setArmVoltage(double voltage) { } @Override - public void setArmAngle(double degrees) { - m_armMaster.setControl(m_mmRequest.withPosition(degrees / 360)); + public void setArmAngle(double degrees, double velocityMult) { + m_armMaxVelDegS.updateIfChanged(true); + m_armDynMMRequest.Velocity = m_armDynMMRequest.Velocity * velocityMult; + + m_armMaster.setControl(m_armDynMMRequest.withPosition(degrees / 360)); m_armFollower.setControl(m_armFollowerRequest); } @@ -271,18 +263,11 @@ public void setWristVoltage(double voltage) { } @Override - public void setWristAngle(double degrees, boolean track) { - if (track && !m_tracking) { - m_tracking = true; - m_dynMMRequest.Acceleration = Double.MAX_VALUE; - Logger.recordOutput("Arm/Wrist Accel", m_dynMMRequest.Acceleration); - } else if (!track && !m_tracking) { - m_tracking = false; - m_accelTimeSecs.updateIfChanged(true); - Logger.recordOutput("Arm/Wrist Accel", m_dynMMRequest.Acceleration); - } - - m_wristMaster.setControl(m_dynMMRequest.withPosition(degrees / 360)); + public void setWristAngle(double degrees, double velocityMult) { + m_wristMaxVelDegS.updateIfChanged(true); + m_wristDynMMRequest.Velocity = m_wristDynMMRequest.Velocity * velocityMult; + + m_wristMaster.setControl(m_wristDynMMRequest.withPosition(degrees / 360)); m_wristFollower.setControl(m_wristFollowerRequest); } diff --git a/src/main/java/frc/robot/subsystems/arm/ArmIOPrototype.java b/src/main/java/frc/robot/subsystems/arm/ArmIOPrototype.java index 501bd041..2fe078b1 100644 --- a/src/main/java/frc/robot/subsystems/arm/ArmIOPrototype.java +++ b/src/main/java/frc/robot/subsystems/arm/ArmIOPrototype.java @@ -8,7 +8,6 @@ import edu.wpi.first.math.util.Units; import lib.properties.phoenix6.Phoenix6PidPropertyBuilder; import lib.properties.phoenix6.PidPropertyPublic; -import org.littletonrobotics.junction.Logger; import frc.robot.Constants.ArmConstants; @@ -81,7 +80,7 @@ public ArmIOPrototype() { } @Override - public void updateInputs(ArmIOInputsAutoLogged inputs) { + public void updateInputs(ArmIOInputs inputs) { m_shoulderPID.updateIfChanged(); m_wristPID.updateIfChanged(); @@ -102,12 +101,6 @@ public void updateInputs(ArmIOInputsAutoLogged inputs) { inputs.armCurrentDraw = m_shoulder.getSupplyCurrent().getValueAsDouble(); inputs.wristCurrentDraw = m_wrist.getSupplyCurrent().getValueAsDouble(); - - Logger.recordOutput("Arm/Should PID Output", m_shoulder.getClosedLoopOutput().getValueAsDouble()); - Logger.recordOutput("Arm/Wrist PID Output", m_wrist.getClosedLoopOutput().getValueAsDouble()); - - Logger.recordOutput("Arm/Shoulder PID Setpoint", m_shoulder.getClosedLoopReference().getValueAsDouble()); - Logger.recordOutput("Arm/Wrist PID Setpoint", m_wrist.getClosedLoopReference().getValueAsDouble()); } @Override @@ -116,7 +109,7 @@ public void setArmVoltage(double voltage){ } @Override - public void setArmAngle(double degrees) { + public void setArmAngle(double degrees, double velocity) { m_shoulder.setControl(m_shoulderReqMM.withPosition(degrees / 360.0)); } @@ -126,11 +119,7 @@ public void setWristVoltage(double voltage){ } @Override - public void setWristAngle(double degrees, boolean useMM) { - if (useMM) { + public void setWristAngle(double degrees, double velocity) { m_wrist.setControl(m_wristReqMM.withPosition(degrees / 360.0)); - } else { - m_wrist.setControl(m_wristReqPID.withPosition(degrees / 360.0)); - } } } diff --git a/src/main/java/frc/robot/subsystems/arm/ArmIOSim.java b/src/main/java/frc/robot/subsystems/arm/ArmIOSim.java index 905a4cd9..3f89b78f 100644 --- a/src/main/java/frc/robot/subsystems/arm/ArmIOSim.java +++ b/src/main/java/frc/robot/subsystems/arm/ArmIOSim.java @@ -27,7 +27,7 @@ public class ArmIOSim implements ArmIO{ private static final double LOOP_PERIOD_SECS = 0.02; @Override - public void updateInputs(ArmIOInputsAutoLogged inputs) { + public void updateInputs(ArmIOInputs inputs) { m_armSim.update(LOOP_PERIOD_SECS); m_wristSim.update(LOOP_PERIOD_SECS); @@ -45,7 +45,7 @@ public void setArmVoltage(double voltage) { } @Override - public void setArmAngle(double degrees) { + public void setArmAngle(double degrees, double velocity) { double setpointRots = Units.degreesToRotations(degrees); m_armAppliedOutput = m_armController.calculate(Units.radiansToRotations(m_armSim.getAngleRads()), setpointRots); m_armAppliedOutput = MathUtil.clamp(m_armAppliedOutput, -12.0, 12.0); @@ -59,7 +59,7 @@ public void setWristVoltage(double voltage) { } @Override - public void setWristAngle(double degrees, boolean track) { + public void setWristAngle(double degrees, double velocity) { double setpointRots = Units.degreesToRotations(degrees); m_wristAppliedOutput = m_wristController.calculate(Units.radiansToRotations(m_wristSim.getAngleRads()), setpointRots); m_wristAppliedOutput = MathUtil.clamp(m_wristAppliedOutput, -12.0, 12.0); diff --git a/src/main/java/frc/robot/subsystems/arm/ArmSubsystem.java b/src/main/java/frc/robot/subsystems/arm/ArmSubsystem.java index 8984ff98..ed971e87 100644 --- a/src/main/java/frc/robot/subsystems/arm/ArmSubsystem.java +++ b/src/main/java/frc/robot/subsystems/arm/ArmSubsystem.java @@ -2,15 +2,13 @@ import edu.wpi.first.math.MathUtil; import edu.wpi.first.math.geometry.Pose2d; -import edu.wpi.first.math.geometry.Pose3d; import edu.wpi.first.wpilibj.DriverStation; import edu.wpi.first.wpilibj.util.Color; import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.SubsystemBase; import frc.robot.Constants.ArmConstants; import frc.robot.Constants.ArmSetpoints; -import lib.utils.AimbotUtils; -import org.littletonrobotics.junction.Logger; +import lib.logger.DataLogUtil; import java.util.function.Supplier; @@ -29,15 +27,22 @@ public enum ArmState { } private final ArmIO m_io; - private final ArmIOInputsAutoLogged m_inputs; + private final ArmIO.ArmIOInputs m_inputs; private double m_desiredArmPoseDegs; + private double m_armVelocityMult = 0; private double m_desiredWristPoseDegs; + private double m_wristGap; + private double m_wristVelocityMult = 0; + private double m_startTime; private ArmState m_desiredState = ArmState.DISABLED; private ArmState m_currentState = ArmState.DISABLED; -// private final ArmVisualizer m_setpointVisualizer; -// private final ArmVisualizer m_poseVisualizer; + + private final DataLogUtil.DataLogTable logUtil = DataLogUtil.getTable("Arm"); + + private final ArmVisualizer m_setpointVisualizer; + private final ArmVisualizer m_poseVisualizer; private final Supplier m_poseSupplier; @@ -47,7 +52,7 @@ public ArmSubsystem(ArmIO io) { public ArmSubsystem(ArmIO io, Supplier supplier) { m_io = io; - m_inputs = new ArmIOInputsAutoLogged(); + m_inputs = new ArmIO.ArmIOInputs(); m_desiredWristPoseDegs = Double.NEGATIVE_INFINITY; m_desiredArmPoseDegs = Double.NEGATIVE_INFINITY; @@ -56,17 +61,16 @@ public ArmSubsystem(ArmIO io, Supplier supplier) { m_poseSupplier = supplier; -// m_poseVisualizer = new ArmVisualizer("Current Arm Pose", Color.kFirstBlue); -// m_setpointVisualizer = new ArmVisualizer("Current Arm Setpoint", Color.kFirstRed); + setupLogging(); + m_poseVisualizer = new ArmVisualizer("Current Arm Pose", Color.kFirstBlue); + m_setpointVisualizer = new ArmVisualizer("Current Arm Setpoint", Color.kFirstRed); } @Override public void periodic() { m_io.updateInputs(m_inputs); - Logger.processInputs("Arm", m_inputs); handleState(); -// Logger.recordOutput("Arm/Arm Desired State", m_desiredState.toString()); // clamp values for PID in between acceptable ranges m_desiredWristPoseDegs = m_desiredWristPoseDegs > Double.NEGATIVE_INFINITY ? @@ -88,50 +92,39 @@ public void periodic() { m_io.enableBrakeMode(m_desiredState == ArmState.DISABLED); if (m_desiredState != ArmState.DISABLED) { - - // check to see if the wrist is currently too close to the rest of the arm - double wristGap = m_inputs.wristPositionDegs + m_inputs.armPositionDegs; - if (wristGap < ArmConstants.WRIST_ARM_GAP.getValue()) { - double underGap = ArmConstants.WRIST_ARM_GAP.getValue() - wristGap; - m_desiredArmPoseDegs += underGap; - m_io.setWristAngle(m_desiredWristPoseDegs + underGap, true); - Logger.recordOutput("Arm/Wrist Setpoint Degs", m_inputs.wristPositionDegs); - } else { - m_io.setWristAngle(m_desiredWristPoseDegs, false); - Logger.recordOutput("Arm/Wrist Setpoint Degs", m_desiredWristPoseDegs); - } + m_io.setWristAngle(m_desiredWristPoseDegs, m_wristVelocityMult); + + double underGap = MathUtil.clamp(ArmConstants.WRIST_ARM_GAP.getValue() + - (m_inputs.armPositionDegs + m_inputs.wristPositionDegs), 0, 180); // set the arms angle - m_io.setArmAngle(m_desiredArmPoseDegs); - Logger.recordOutput("Arm/Arm Setpoint Degs", m_desiredArmPoseDegs); + m_io.setArmAngle(m_desiredArmPoseDegs + underGap, m_armVelocityMult); } -// Logger.recordOutput("Arm/Wrist Gap", m_inputs.wristPositionDegs + m_inputs.armPositionDegs); - // Update arm visualizers -// m_poseVisualizer.update(m_inputs.armPositionDegs, m_inputs.wristPositionDegs); -// m_setpointVisualizer.update(m_desiredArmPoseDegs, m_desiredWristPoseDegs); + m_poseVisualizer.update(m_inputs.armPositionDegs, m_inputs.wristPositionDegs); + m_setpointVisualizer.update(m_desiredArmPoseDegs, m_desiredWristPoseDegs); } public void handleState() { switch(m_desiredState) { case STOW -> { - if (m_currentState == ArmState.AMP - && m_inputs.armPositionDegs < ArmSetpoints.AMP_INTERMEDIATE.armAngle() + 5 - && m_inputs.wristPositionDegs < ArmSetpoints.AMP_INTERMEDIATE.wristAngle() + 5) { - m_currentState = ArmState.STOW; - } else if (m_currentState == ArmState.AMP) { - m_desiredArmPoseDegs = ArmSetpoints.AMP_INTERMEDIATE.armAngle(); - m_desiredWristPoseDegs = ArmSetpoints.AMP_INTERMEDIATE.wristAngle(); + if (m_inputs.armPositionDegs > 60 && m_currentState == ArmState.AMP) { + m_wristVelocityMult = 0.0; + m_armVelocityMult = 1.0; } else { - m_desiredArmPoseDegs = ArmSetpoints.STOW_SETPOINT.armAngle(); - m_desiredWristPoseDegs = ArmSetpoints.STOW_SETPOINT.wristAngle(); + m_currentState = ArmState.STOW; + m_armVelocityMult = 1.0; + m_wristVelocityMult = 1.0; } + + m_desiredArmPoseDegs = ArmSetpoints.STOW_SETPOINT.armAngle(); + m_desiredWristPoseDegs = ArmSetpoints.STOW_SETPOINT.wristAngle(); } case AUTO_AIM -> { -// ArmPose aimSetpoint = AimbotUtils.aimbotCalculate( -// new Pose3d(m_poseSupplier.get()), m_inputs.armPositionDegs); + m_armVelocityMult = 1.0; + m_wristVelocityMult = 1.0; m_desiredArmPoseDegs = ArmConstants.WRIST_ARM_GAP.getValue() - m_desiredWristPoseDegs; m_desiredArmPoseDegs = m_desiredArmPoseDegs >= ArmConstants.ARM_LOWER_LIMIT.getValue() ? m_desiredArmPoseDegs @@ -139,24 +132,31 @@ public void handleState() { m_desiredWristPoseDegs = ArmSetpoints.WRIST_ANGLE.getValue(); } case INTAKE -> { + m_armVelocityMult = 1.0; + m_wristVelocityMult = 1.0; + m_currentState = ArmState.INTAKE; m_desiredArmPoseDegs = ArmSetpoints.INTAKE_SETPOINT.armAngle(); m_desiredWristPoseDegs = ArmSetpoints.INTAKE_SETPOINT.wristAngle(); } case AMP -> { - - if (m_inputs.armPositionDegs > ArmSetpoints.AMP_INTERMEDIATE.armAngle() - 5 - && m_inputs.wristPositionDegs > ArmSetpoints.AMP_INTERMEDIATE.wristAngle() - 5) { - m_currentState = ArmState.AMP; - m_desiredArmPoseDegs = ArmSetpoints.AMP_SETPOINT.armAngle(); - m_desiredWristPoseDegs = ArmSetpoints.AMP_SETPOINT.wristAngle(); + if (Math.abs(m_inputs.wristPositionDegs - m_desiredWristPoseDegs) > 5) { + m_armVelocityMult = 0.5; + m_wristVelocityMult = 1.0; } else { - m_currentState = ArmState.TRANSITION_AMP; - m_desiredArmPoseDegs = ArmSetpoints.AMP_INTERMEDIATE.armAngle(); - m_desiredWristPoseDegs = ArmSetpoints.AMP_INTERMEDIATE.wristAngle(); + m_armVelocityMult = 1.0; + m_wristVelocityMult = 1.0; } + + m_currentState = ArmState.AMP; + + m_desiredArmPoseDegs = ArmSetpoints.AMP_SETPOINT.armAngle(); + m_desiredWristPoseDegs = ArmSetpoints.AMP_SETPOINT.wristAngle(); } default -> { + m_armVelocityMult = 1.0; + m_wristVelocityMult = 1.0; + m_desiredArmPoseDegs = m_inputs.armPositionDegs; m_desiredWristPoseDegs = m_inputs.wristPositionDegs; } @@ -223,6 +223,25 @@ public Command setWristPowerFactory(double power) { () -> m_io.setWristVoltage(0.0)); } + /** Logging util */ + public void setupLogging() { + logUtil.addDouble("ArmAngleDegs", () -> m_inputs.armPositionDegs, true); + logUtil.addDouble("WristAngleDegs", () -> m_inputs.wristPositionDegs, true); + logUtil.addDouble("ArmSetpointDegs", () -> m_desiredArmPoseDegs, true); + logUtil.addDouble("WristSetpointDegs", () -> m_desiredWristPoseDegs, true); + + logUtil.addDouble("ArmAppliedOutput", () -> m_inputs.armAppliedOutput, false); + logUtil.addDouble("WristAppliedOutput", () -> m_inputs.wristAppliedOutput, false); + + logUtil.addDouble("ArmClosedLoopOutput", () -> m_inputs.armClosedLoopOutput, false); + logUtil.addDouble("WristClosedLoopOutput", () -> m_inputs.wristClosedLoopOutput, false); + + logUtil.addDouble("ArmAppliedOutput", () -> m_inputs.armAppliedOutput, false); + logUtil.addDouble("WristAppliedOutput", () -> m_inputs.wristAppliedOutput, false); + + logUtil.addDouble("WristArmGap", () -> m_wristGap, true); + logUtil.addString("Arm Current State", () -> m_desiredState.toString(), true); + } } diff --git a/src/main/java/frc/robot/subsystems/arm/ArmVisualizer.java b/src/main/java/frc/robot/subsystems/arm/ArmVisualizer.java index 76657d29..dc8ebaf7 100644 --- a/src/main/java/frc/robot/subsystems/arm/ArmVisualizer.java +++ b/src/main/java/frc/robot/subsystems/arm/ArmVisualizer.java @@ -1,5 +1,6 @@ package frc.robot.subsystems.arm; +import edu.wpi.first.math.geometry.Pose2d; import edu.wpi.first.math.geometry.Pose3d; import edu.wpi.first.math.geometry.Rotation2d; import edu.wpi.first.math.geometry.Rotation3d; @@ -10,8 +11,8 @@ import edu.wpi.first.wpilibj.util.Color; import edu.wpi.first.wpilibj.util.Color8Bit; import frc.robot.Constants.ArmConstants; +import lib.logger.DataLogUtil; import lib.utils.AimbotUtils; -import org.littletonrobotics.junction.Logger; public class ArmVisualizer { private final Mechanism2d mechanism; @@ -19,6 +20,9 @@ public class ArmVisualizer { private final MechanismLigament2d wrist; private final String key; + Pose3d pivotArm = new Pose3d(); + Pose3d pivotWrist = new Pose3d(); + public ArmVisualizer(String key, Color color) { this.key = key; mechanism = new Mechanism2d(3.0, 3.0, new Color8Bit(Color.kWhite)); @@ -29,6 +33,8 @@ public ArmVisualizer(String key, Color color) { wrist = new MechanismLigament2d("wrist", ArmConstants.WRIST_LENGTH_METERS, 45.0, 5, new Color8Bit(color)); root.append(arm); arm.append(wrist); + + DataLogUtil.getTable("Arm").addPose3dArray(key, () -> new Pose3d[]{pivotArm, pivotWrist}, true); } /** Update arm visualizer with current arm angle */ @@ -36,16 +42,14 @@ public void update(double armAngleDegs, double wristAngleDegs) { // Log Mechanism2d arm.setAngle(Rotation2d.fromDegrees(armAngleDegs)); wrist.setAngle(Rotation2d.fromDegrees(wristAngleDegs)); - Logger.recordOutput("Arm/Mechanism2d/" + key, mechanism); // Log 3D poses - Pose3d pivotArm = + pivotArm = new Pose3d(ArmConstants.PIVOT_JOINT_TRANSLATION.getX(), 0.0, ArmConstants.PIVOT_JOINT_TRANSLATION.getY(), new Rotation3d(0.0, Units.degreesToRadians(armAngleDegs), 0.0)); - Pose3d pivotWrist = new Pose3d(AimbotUtils.getShooterTransformation(armAngleDegs).getTranslation(), + pivotWrist = new Pose3d(AimbotUtils.getShooterTransformation(armAngleDegs).getTranslation(), new Rotation3d(0.0, Units.degreesToRadians(-wristAngleDegs), 0.0)); - Logger.recordOutput("Arm/Mechanism3d/" + key, pivotArm, pivotWrist); } } diff --git a/src/main/java/frc/robot/subsystems/climber/ClimberIO.java b/src/main/java/frc/robot/subsystems/climber/ClimberIO.java index 04f22d2c..ae59d9aa 100644 --- a/src/main/java/frc/robot/subsystems/climber/ClimberIO.java +++ b/src/main/java/frc/robot/subsystems/climber/ClimberIO.java @@ -1,9 +1,6 @@ package frc.robot.subsystems.climber; -import org.littletonrobotics.junction.AutoLog; - public interface ClimberIO { - @AutoLog class ClimberIOInputs { protected double leftClimberPosition = 0.0; protected double rightClimberPosition = 0.0; @@ -105,6 +102,6 @@ default void setRightVoltage(double volts) {} default void setLeftVoltage(double volts) {} default void setRightPosition(double degrees) {} default void setLeftPosition(double degrees) {} - default void updateInputs(ClimberIOInputsAutoLogged inputs) {} + default void updateInputs(ClimberIOInputs inputs) {} default void stop() {} } diff --git a/src/main/java/frc/robot/subsystems/climber/ClimberIOKraken.java b/src/main/java/frc/robot/subsystems/climber/ClimberIOKraken.java index a586cb86..45fbca9d 100644 --- a/src/main/java/frc/robot/subsystems/climber/ClimberIOKraken.java +++ b/src/main/java/frc/robot/subsystems/climber/ClimberIOKraken.java @@ -98,7 +98,7 @@ public ClimberIOKraken() { } @Override - public void updateInputs(ClimberIOInputsAutoLogged inputs) { + public void updateInputs(ClimberIOInputs inputs) { inputs.leftClimberPosition = m_leftPositionSignal.getValueAsDouble(); inputs.rightClimberPosition = m_rightPositionSignal.getValueAsDouble(); diff --git a/src/main/java/frc/robot/subsystems/climber/ClimberIOPrototype.java b/src/main/java/frc/robot/subsystems/climber/ClimberIOPrototype.java index 2330b0e3..1ebf0d5e 100644 --- a/src/main/java/frc/robot/subsystems/climber/ClimberIOPrototype.java +++ b/src/main/java/frc/robot/subsystems/climber/ClimberIOPrototype.java @@ -66,7 +66,7 @@ public void setRightPosition(double degrees) { } @Override - public void updateInputs(ClimberIOInputsAutoLogged inputs) { + public void updateInputs(ClimberIOInputs inputs) { m_leftClimberPid.updateIfChanged(); m_rightClimberPid.updateIfChanged(); diff --git a/src/main/java/frc/robot/subsystems/climber/ClimberSubsystem.java b/src/main/java/frc/robot/subsystems/climber/ClimberSubsystem.java index f5d05e36..56924cdb 100644 --- a/src/main/java/frc/robot/subsystems/climber/ClimberSubsystem.java +++ b/src/main/java/frc/robot/subsystems/climber/ClimberSubsystem.java @@ -3,20 +3,18 @@ import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.SubsystemBase; -import org.littletonrobotics.junction.Logger; public class ClimberSubsystem extends SubsystemBase { private final ClimberIO m_io; - private final ClimberIOInputsAutoLogged m_inputs; + private final ClimberIO.ClimberIOInputs m_inputs; public ClimberSubsystem(ClimberIO io) { m_io = io; - m_inputs = new ClimberIOInputsAutoLogged(); + m_inputs = new ClimberIO.ClimberIOInputs(); } @Override public void periodic() { m_io.updateInputs(m_inputs); -// Logger.processInputs("Climber", m_inputs); } public void setClimberPower(double power) { diff --git a/src/main/java/frc/robot/subsystems/drive/DriveSubsystem.java b/src/main/java/frc/robot/subsystems/drive/DriveSubsystem.java index 80134095..3eeae2f7 100644 --- a/src/main/java/frc/robot/subsystems/drive/DriveSubsystem.java +++ b/src/main/java/frc/robot/subsystems/drive/DriveSubsystem.java @@ -16,8 +16,8 @@ import com.gos.lib.properties.pid.PidProperty; import com.gos.lib.properties.pid.WpiPidPropertyBuilder; import com.pathplanner.lib.auto.AutoBuilder; +import com.pathplanner.lib.pathfinding.LocalADStar; import com.pathplanner.lib.pathfinding.Pathfinding; -import com.pathplanner.lib.util.PathPlannerLogging; import edu.wpi.first.math.controller.PIDController; import edu.wpi.first.math.estimator.SwerveDrivePoseEstimator; import edu.wpi.first.math.geometry.*; @@ -25,40 +25,41 @@ import edu.wpi.first.math.geometry.Pose2d; import edu.wpi.first.math.geometry.Rotation2d; import edu.wpi.first.math.geometry.Translation2d; -import edu.wpi.first.math.geometry.Twist2d; import edu.wpi.first.math.kinematics.*; import edu.wpi.first.math.util.Units; import edu.wpi.first.wpilibj.DriverStation; import edu.wpi.first.wpilibj.DriverStation.Alliance; import edu.wpi.first.wpilibj.Timer; +import edu.wpi.first.wpilibj.smartdashboard.Field2d; +import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.SubsystemBase; import frc.robot.Constants.DriveConstants; import frc.robot.subsystems.drive.module.Module; import frc.robot.subsystems.drive.module.ModuleIO; import frc.robot.subsystems.vision.VisionSubsystem; -import lib.utils.LocalADStarAK; -import java.util.List; -import java.util.Optional; import java.util.concurrent.locks.Lock; import java.util.concurrent.locks.ReentrantLock; +import lib.logger.DataLogUtil; +import lib.logger.DataLogUtil.DataLogTable; import lib.utils.PoseEstimator; -import org.littletonrobotics.junction.AutoLogOutput; -import org.littletonrobotics.junction.Logger; public class DriveSubsystem extends SubsystemBase { public static final Lock odometryLock = new ReentrantLock(); private final GyroIO gyroIO; - private final GyroIOInputsAutoLogged gyroInputs = new GyroIOInputsAutoLogged(); + private final GyroIO.GyroIOInputs gyroInputs = new GyroIO.GyroIOInputs(); private final Module[] modules = new Module[4]; // FL, FR, BL, BR private final SwerveDriveKinematics kinematics = new SwerveDriveKinematics(getModuleTranslations()); private final PoseEstimator m_poseEstimator; private final SwerveDrivePoseEstimator m_wpiPoseEstimator; + + private final Field2d m_field = new Field2d(); + private Pose2d pose = new Pose2d(); private Rotation2d lastGyroRotation = new Rotation2d(); @@ -67,6 +68,28 @@ public class DriveSubsystem extends SubsystemBase { private final PIDController m_thetaPid; private final PidProperty m_thetaPidProperty; + private final DataLogTable m_logTable = DataLogUtil.getTable("Swerve"); + + private final SwerveModuleState[] m_measureStates = new SwerveModuleState[] { + new SwerveModuleState(), + new SwerveModuleState(), + new SwerveModuleState(), + new SwerveModuleState() + }; + private SwerveModuleState[] m_setpointStates = new SwerveModuleState[] { + new SwerveModuleState(), + new SwerveModuleState(), + new SwerveModuleState(), + new SwerveModuleState() + }; + private final SwerveModuleState[] m_optimizedStates = new SwerveModuleState[] { + new SwerveModuleState(), + new SwerveModuleState(), + new SwerveModuleState(), + new SwerveModuleState() + }; + + public DriveSubsystem( GyroIO gyroIO, ModuleIO flModuleIO, @@ -129,12 +152,11 @@ public DriveSubsystem( DriverStation.getAlliance().isPresent() && DriverStation.getAlliance().get() == Alliance.Red, this); - Pathfinding.setPathfinder(new LocalADStarAK()); - PathPlannerLogging.setLogActivePathCallback( - (List activePath) -> Logger.recordOutput( - "Odometry/Trajectory", activePath.toArray(new Pose2d[0]))); - PathPlannerLogging.setLogTargetPoseCallback( - (Pose2d targetPose) -> Logger.recordOutput("Odometry/TrajectorySetpoint", targetPose)); + Pathfinding.setPathfinder(new LocalADStar()); + + // turn on logging + setupLogging(); + SmartDashboard.putData("Field",m_field); } @Override @@ -145,7 +167,6 @@ public void periodic() { module.updateInputs(); } odometryLock.unlock(); -// Logger.processInputs("Drive/Gyro", gyroInputs); for (var module : modules) { module.periodic(); } @@ -156,11 +177,6 @@ public void periodic() { module.stop(); } } - // Log empty setpoint states when disabled - if (DriverStation.isDisabled()) { -// Logger.recordOutput("SwerveStates/Setpoints"); -// Logger.recordOutput("SwerveStates/SetpointsOptimized"); - } // Update odometry // int deltaCount = @@ -194,16 +210,20 @@ public void periodic() { // } // List visionUpdates = new java.util.ArrayList<>(List.of()); -// for (VisionSubsystem camera : m_cameras) { -// camera.updateInputs(); -// Optional update = camera.getPose(m_poseEstimator.getLatestPose()); -// update.ifPresent(visionUpdates::add); -// } + for (VisionSubsystem camera : m_cameras) { + camera.updateInputs(); + camera.getPose(m_wpiPoseEstimator.getEstimatedPosition()).ifPresent( + (PoseEstimator.TimestampedVisionUpdate pose) -> + m_wpiPoseEstimator.addVisionMeasurement(pose.pose(), pose.timestamp(), pose.stdDevs()) + ); + } // m_poseEstimator.addVisionData(visionUpdates); m_wpiPoseEstimator.update(gyroInputs.yawPosition, getModulePositions()); m_thetaPidProperty.updateIfChanged(); + + m_field.setRobotPose(getVisionPose()); } /** @@ -214,19 +234,14 @@ public void periodic() { public void runVelocity(ChassisSpeeds speeds) { // Calculate module setpoints ChassisSpeeds discreteSpeeds = ChassisSpeeds.discretize(speeds, 0.02); - SwerveModuleState[] setpointStates = kinematics.toSwerveModuleStates(discreteSpeeds); - SwerveDriveKinematics.desaturateWheelSpeeds(setpointStates, DriveConstants.MAX_LINEAR_SPEED); + m_setpointStates = kinematics.toSwerveModuleStates(discreteSpeeds); + SwerveDriveKinematics.desaturateWheelSpeeds(m_setpointStates, DriveConstants.MAX_LINEAR_SPEED); // Send setpoints to modules - SwerveModuleState[] optimizedSetpointStates = new SwerveModuleState[4]; for (int i = 0; i < 4; i++) { // The module returns the optimized state, useful for logging - optimizedSetpointStates[i] = modules[i].runSetpoint(setpointStates[i]); + m_optimizedStates[i] = modules[i].runSetpoint(m_setpointStates[i]); } - - // Log setpoint states -// Logger.recordOutput("SwerveStates/Setpoints", setpointStates); -// Logger.recordOutput("SwerveStates/SetpointsOptimized", optimizedSetpointStates); } /** @@ -242,10 +257,7 @@ public double alignToPoint(Pose3d point) { double desiredRotation = Math.PI * 2 - (Math.atan2(robotToPoint.getX(), robotToPoint.getY()) + Units.degreesToRadians(270)); - double output = m_thetaPid.calculate(getRotation().getRadians(), desiredRotation); -// Logger.recordOutput("Drive/Alignment output", output); -// Logger.recordOutput("Drive/Desired Alignment", desiredRotation); - return output; + return m_thetaPid.calculate(getRotation().getRadians(), desiredRotation); } /** Stops the drive. */ @@ -346,4 +358,24 @@ public static Translation2d[] getModuleTranslations() { new Translation2d(-DriveConstants.TRACK_WIDTH_X / 2.0, -DriveConstants.TRACK_WIDTH_Y / 2.0) }; } -} + + public void setupLogging() { + for (int i = 0; i < modules.length; i++) { + Module module = modules[i]; + String moduleName = "Module" + i; + + m_logTable.addDouble(moduleName + "/AzimuthPose", () -> module.getAngle().getDegrees(), true); + m_logTable.addDouble(moduleName + "/DrivePose", module::getPositionMeters, true); + + m_logTable.addDouble(moduleName + "/VelocityMPS", module::getVelocityMetersPerSec, true); + } + + m_logTable.addPose2d("Vision Pose", this::getVisionPose, true); + + m_logTable.addSwerveModuleStateArray("MeasuredStates", () -> m_measureStates, true); + m_logTable.addSwerveModuleStateArray("SetpointStates", () -> m_setpointStates, true); + m_logTable.addSwerveModuleStateArray("OptimizedStates", () -> m_optimizedStates, true); + + m_logTable.addDouble("GyroHeading", () -> gyroInputs.yawPosition.getDegrees(), true); + } + } diff --git a/src/main/java/frc/robot/subsystems/drive/GyroIO.java b/src/main/java/frc/robot/subsystems/drive/GyroIO.java index 1d386ae1..b11c6ccc 100644 --- a/src/main/java/frc/robot/subsystems/drive/GyroIO.java +++ b/src/main/java/frc/robot/subsystems/drive/GyroIO.java @@ -14,10 +14,8 @@ package frc.robot.subsystems.drive; import edu.wpi.first.math.geometry.Rotation2d; -import org.littletonrobotics.junction.AutoLog; public interface GyroIO { - @AutoLog class GyroIOInputs { protected boolean connected = false; protected Rotation2d yawPosition = new Rotation2d(); @@ -57,7 +55,7 @@ public void setYawVelocityRadPerSec(double yawVelocityRadPerSec) { } } - default void updateInputs(GyroIOInputsAutoLogged inputs) {} + default void updateInputs(GyroIOInputs inputs) {} default void resetGyro(double degrees) {} } diff --git a/src/main/java/frc/robot/subsystems/drive/GyroIOPigeon1.java b/src/main/java/frc/robot/subsystems/drive/GyroIOPigeon1.java index dea3555a..183004ef 100644 --- a/src/main/java/frc/robot/subsystems/drive/GyroIOPigeon1.java +++ b/src/main/java/frc/robot/subsystems/drive/GyroIOPigeon1.java @@ -21,7 +21,7 @@ public GyroIOPigeon1(int id) { } @Override - public void updateInputs(GyroIOInputsAutoLogged inputs) { + public void updateInputs(GyroIOInputs inputs) { inputs.setConnected(m_pigeon.getState() == PigeonIMU.PigeonState.Ready); inputs.setYawPosition(Rotation2d.fromDegrees(m_pigeon.getYaw())); inputs.setYawVelocityRadPerSec((m_pigeon.getYaw() - prevYaw) / Units.millisecondsToSeconds(20)); diff --git a/src/main/java/frc/robot/subsystems/drive/GyroIOPigeon2.java b/src/main/java/frc/robot/subsystems/drive/GyroIOPigeon2.java index 1b359561..2476c75f 100644 --- a/src/main/java/frc/robot/subsystems/drive/GyroIOPigeon2.java +++ b/src/main/java/frc/robot/subsystems/drive/GyroIOPigeon2.java @@ -50,7 +50,7 @@ public GyroIOPigeon2(boolean phoenixDrive) { } @Override - public void updateInputs(GyroIOInputsAutoLogged inputs) { + public void updateInputs(GyroIOInputs inputs) { inputs.setConnected(BaseStatusSignal.refreshAll(yaw, yawVelocity).equals(StatusCode.OK)); inputs.setYawPosition(Rotation2d.fromDegrees(yaw.getValueAsDouble())); inputs.setYawVelocityRadPerSec(Units.degreesToRadians(yawVelocity.getValueAsDouble())); diff --git a/src/main/java/frc/robot/subsystems/drive/module/Module.java b/src/main/java/frc/robot/subsystems/drive/module/Module.java index a360ddda..25046e59 100644 --- a/src/main/java/frc/robot/subsystems/drive/module/Module.java +++ b/src/main/java/frc/robot/subsystems/drive/module/Module.java @@ -18,14 +18,14 @@ import edu.wpi.first.math.kinematics.SwerveModuleState; import edu.wpi.first.wpilibj.Timer; import frc.robot.Constants; -import org.littletonrobotics.junction.Logger; +import lib.logger.DataLogUtil; public class Module { private static final double WHEEL_RADIUS = Constants.DriveConstants.WHEEL_RADIUS_METERS; public static final double ODOMETRY_FREQUENCY = 250.0; private final ModuleIO m_io; - private final ModuleIOInputsAutoLogged m_inputs = new ModuleIOInputsAutoLogged(); + private final ModuleIO.ModuleIOInputs m_inputs = new ModuleIO.ModuleIOInputs(); private final int m_index; private Rotation2d m_angleSetpoint = null; // Setpoint for closed loop control, null for open loop @@ -42,6 +42,12 @@ public Module(ModuleIO io) { Timer.delay(0.5); setBrakeMode(true); + + DataLogUtil.getTable("Swerve").addDoubleArray("Module" + m_index + "/DriveCurrentDraw", + () -> m_inputs.driveCurrentAmps, false); + DataLogUtil.getTable("Swerve").addDoubleArray("Module" + m_index + "/TurnCurrentDraw", + () -> m_inputs.turnCurrentAmps, false); + } /** @@ -53,11 +59,9 @@ public void updateInputs() { } public void periodic() { -// Logger.processInputs("Drive/Module" + Integer.toString(m_index), m_inputs); - // On first cycle, reset relative turn encoder // Wait until absolute angle is nonzero in case it wasn't initialized yet - if (m_turnRelativeOffset == null) {// && m_inputs.getTurnAbsolutePosition().getRadians() != 0.0) { + if (m_turnRelativeOffset == null) { m_turnRelativeOffset = m_inputs.getTurnAbsolutePosition().minus(m_inputs.getTurnPosition()); } diff --git a/src/main/java/frc/robot/subsystems/drive/module/ModuleIO.java b/src/main/java/frc/robot/subsystems/drive/module/ModuleIO.java index f812aadc..4a3167bf 100644 --- a/src/main/java/frc/robot/subsystems/drive/module/ModuleIO.java +++ b/src/main/java/frc/robot/subsystems/drive/module/ModuleIO.java @@ -14,10 +14,8 @@ package frc.robot.subsystems.drive.module; import edu.wpi.first.math.geometry.Rotation2d; -import org.littletonrobotics.junction.AutoLog; public interface ModuleIO { - @AutoLog class ModuleIOInputs { protected double drivePositionRad = 0.0; protected double driveVelocityRadPerSec = 0.0; diff --git a/src/main/java/frc/robot/subsystems/drive/module/ModuleIOSim.java b/src/main/java/frc/robot/subsystems/drive/module/ModuleIOSim.java index 34d6851e..e735141e 100644 --- a/src/main/java/frc/robot/subsystems/drive/module/ModuleIOSim.java +++ b/src/main/java/frc/robot/subsystems/drive/module/ModuleIOSim.java @@ -21,7 +21,6 @@ import edu.wpi.first.wpilibj.Timer; import edu.wpi.first.wpilibj.simulation.DCMotorSim; import frc.robot.Constants; -import org.littletonrobotics.junction.Logger; /** * Physics sim implementation of module IO. diff --git a/src/main/java/frc/robot/subsystems/drive/module/ModuleIOTalonFX.java b/src/main/java/frc/robot/subsystems/drive/module/ModuleIOTalonFX.java index 1d2b9c4a..82aa4b77 100644 --- a/src/main/java/frc/robot/subsystems/drive/module/ModuleIOTalonFX.java +++ b/src/main/java/frc/robot/subsystems/drive/module/ModuleIOTalonFX.java @@ -170,7 +170,7 @@ public ModuleIOTalonFX(ModuleConstants moduleConstants) { BaseStatusSignal.setUpdateFrequencyForAll( Module.ODOMETRY_FREQUENCY, m_drivePosition, m_turnPosition); BaseStatusSignal.setUpdateFrequencyForAll( - 50.0, + 5.0, m_driveVelocity, m_driveAppliedVolts, m_driveCurrent, diff --git a/src/main/java/frc/robot/subsystems/shooter/ShooterIO.java b/src/main/java/frc/robot/subsystems/shooter/ShooterIO.java index cf7cc79b..15dd4e85 100644 --- a/src/main/java/frc/robot/subsystems/shooter/ShooterIO.java +++ b/src/main/java/frc/robot/subsystems/shooter/ShooterIO.java @@ -1,9 +1,6 @@ package frc.robot.subsystems.shooter; -import org.littletonrobotics.junction.AutoLog; - public interface ShooterIO { - @AutoLog class ShooterIOInputs { protected double tlVelocityRPM = 0.0; protected double trVelocityRPM = 0.0; @@ -248,7 +245,7 @@ default void setKickerVoltage(double voltage) {} default void setIntakeVoltage(double voltage) {} default void setLeftVelocityRpm(double rpm) {} default void setRightVelocityRpm(double rpm) {} - default void updateInputs(ShooterIOInputsAutoLogged inputs) {} + default void updateInputs(ShooterIOInputs inputs) {} default void stop() {} } diff --git a/src/main/java/frc/robot/subsystems/shooter/ShooterIOKraken.java b/src/main/java/frc/robot/subsystems/shooter/ShooterIOKraken.java index 8f5ed1bc..77c6b107 100644 --- a/src/main/java/frc/robot/subsystems/shooter/ShooterIOKraken.java +++ b/src/main/java/frc/robot/subsystems/shooter/ShooterIOKraken.java @@ -147,7 +147,7 @@ public ShooterIOKraken() { } @Override - public void updateInputs(ShooterIOInputsAutoLogged inputs) { + public void updateInputs(ShooterIOInputs inputs) { BaseStatusSignal.refreshAll( m_leftVelSignal, m_rightVelSignal, diff --git a/src/main/java/frc/robot/subsystems/shooter/ShooterIOPrototype.java b/src/main/java/frc/robot/subsystems/shooter/ShooterIOPrototype.java index e0b2aeb5..32d714d1 100644 --- a/src/main/java/frc/robot/subsystems/shooter/ShooterIOPrototype.java +++ b/src/main/java/frc/robot/subsystems/shooter/ShooterIOPrototype.java @@ -123,7 +123,7 @@ public void setRightVelocityRpm(double rpm) { } @Override - public void updateInputs(ShooterIOInputsAutoLogged inputs) { + public void updateInputs(ShooterIOInputs inputs) { m_topLeftPidProperty.updateIfChanged(); m_topRightPidProperty.updateIfChanged(); m_bottomLeftPidProperty.updateIfChanged(); diff --git a/src/main/java/frc/robot/subsystems/shooter/ShooterIOSim.java b/src/main/java/frc/robot/subsystems/shooter/ShooterIOSim.java index 7b56cb18..a91dcea5 100644 --- a/src/main/java/frc/robot/subsystems/shooter/ShooterIOSim.java +++ b/src/main/java/frc/robot/subsystems/shooter/ShooterIOSim.java @@ -18,7 +18,7 @@ public class ShooterIOSim implements ShooterIO { private double kickerAppliedVolts = 0.0; @Override - public void updateInputs(ShooterIOInputsAutoLogged inputs) { + public void updateInputs(ShooterIOInputs inputs) { m_simLeft.update(LOOP_PERIOD_SECS); m_simRight.update(LOOP_PERIOD_SECS); diff --git a/src/main/java/frc/robot/subsystems/shooter/ShooterSubsystem.java b/src/main/java/frc/robot/subsystems/shooter/ShooterSubsystem.java index 55e88ae0..9793763c 100644 --- a/src/main/java/frc/robot/subsystems/shooter/ShooterSubsystem.java +++ b/src/main/java/frc/robot/subsystems/shooter/ShooterSubsystem.java @@ -3,29 +3,28 @@ import edu.wpi.first.wpilibj.Timer; import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.SubsystemBase; -import org.littletonrobotics.junction.Logger; -import org.littletonrobotics.junction.networktables.LoggedDashboardNumber; + +import lib.logger.DataLogUtil; +import lib.logger.DataLogUtil.DataLogTable; public class ShooterSubsystem extends SubsystemBase { private final ShooterIO m_io; - private final ShooterIOInputsAutoLogged m_inputs; + private final ShooterIO.ShooterIOInputs m_inputs; - private final LoggedDashboardNumber m_leftSetpoint; - private final LoggedDashboardNumber m_rightSetpoint; + private final DataLogTable m_logTable = DataLogUtil.getTable("Shooter"); public ShooterSubsystem(ShooterIO io) { m_io = io; - m_inputs = new ShooterIOInputsAutoLogged(); + m_inputs = new ShooterIO.ShooterIOInputs(); - m_leftSetpoint = new LoggedDashboardNumber("Shooter/Left Flywheel Setpoint RPM", 5000); - m_rightSetpoint = new LoggedDashboardNumber("Shooter/Right Flywheel Setpoint RPM", 5000); + // turn on logging + setupLogging(); } @Override public void periodic() { m_io.updateInputs(m_inputs); - Logger.processInputs("Shooter", m_inputs); } public void setShooterPowerLeft(double power) { @@ -48,12 +47,10 @@ public void setIntakePower(double power) { public Command runShooterVelocity(boolean runKicker) { return runEnd(() -> { - m_io.setLeftVelocityRpm(m_leftSetpoint.get()); - m_io.setRightVelocityRpm(m_rightSetpoint.get()); + m_io.setLeftVelocityRpm(0.0); + m_io.setRightVelocityRpm(0.0); - if ( /*(Math.abs(m_leftSetpoint.get() - m_inputs.tlVelocityRPM) < 15 - || Math.abs(m_rightSetpoint.get() - m_inputs.trVelocityRPM) < 15) - && */ runKicker) { + if (runKicker) { m_io.setKickerVoltage(3.0); m_io.setIntakeVoltage(0.05); } else { @@ -109,5 +106,18 @@ public Command setShooterPowerFactory(double left, double right, double intake) setIntakePower(intake); }); } -} + public void setupLogging() { + m_logTable.addDouble("LeftVelocity", () -> m_inputs.tlVelocityRPM, true); + m_logTable.addDouble("RightVelocity", () -> m_inputs.trVelocityRPM, true); + + m_logTable.addDouble("LeftAppliedVolts", () -> m_inputs.tlAppliedVolts, true); + m_logTable.addDouble("RightAppliedVolts", () -> m_inputs.trAppliedVolts, true); + + m_logTable.addDouble("LeftCurrentDraw", () -> m_inputs.tlCurrentDraw, false); + m_logTable.addDouble("RightCurrentDraw", () -> m_inputs.trCurrentDraw, false); + + m_logTable.addDouble("LeftTemperature", () -> m_inputs.tlTemperature, false); + m_logTable.addDouble("RightTemperature", () -> m_inputs.trTemperature, false); + } +} diff --git a/src/main/java/frc/robot/subsystems/vision/PhotonVisionIOInputs.java b/src/main/java/frc/robot/subsystems/vision/PhotonVisionIOInputs.java index 56163aa0..92d2619c 100644 --- a/src/main/java/frc/robot/subsystems/vision/PhotonVisionIOInputs.java +++ b/src/main/java/frc/robot/subsystems/vision/PhotonVisionIOInputs.java @@ -1,9 +1,5 @@ package frc.robot.subsystems.vision; -import edu.wpi.first.math.geometry.Pose3d; -import org.littletonrobotics.junction.AutoLog; - -@AutoLog public class PhotonVisionIOInputs { protected boolean camConnected = false; protected double camLatency = 0.0; diff --git a/src/main/java/frc/robot/subsystems/vision/VisionSubsystem.java b/src/main/java/frc/robot/subsystems/vision/VisionSubsystem.java index 83b2d543..b16d3eb2 100644 --- a/src/main/java/frc/robot/subsystems/vision/VisionSubsystem.java +++ b/src/main/java/frc/robot/subsystems/vision/VisionSubsystem.java @@ -13,7 +13,6 @@ import edu.wpi.first.math.numbers.N1; import edu.wpi.first.math.numbers.N3; import lib.utils.PoseEstimator; -import org.littletonrobotics.junction.Logger; import org.photonvision.EstimatedRobotPose; import org.photonvision.PhotonCamera; import org.photonvision.PhotonPoseEstimator; @@ -34,7 +33,7 @@ public class VisionSubsystem { private final double xyStdDevCoefficient = 0.01; private final double thetaStdDevCoefficient = 0.01; - private final PhotonVisionIOInputsAutoLogged inputs = new PhotonVisionIOInputsAutoLogged(); + private final PhotonVisionIOInputs inputs = new PhotonVisionIOInputs(); public VisionSubsystem(String camName, Transform3d camPose) { m_camera = new PhotonCamera(camName); @@ -44,9 +43,11 @@ public VisionSubsystem(String camName, Transform3d camPose) { m_aprilTagFieldLayout = AprilTagFieldLayout.loadFromResource(AprilTagFields.k2024Crescendo.m_resourceFile); m_photonPoseEstimator = new PhotonPoseEstimator( m_aprilTagFieldLayout, - PhotonPoseEstimator.PoseStrategy.LOWEST_AMBIGUITY, + PhotonPoseEstimator.PoseStrategy.MULTI_TAG_PNP_ON_COPROCESSOR, m_camera, robotToCam); + + m_photonPoseEstimator.setMultiTagFallbackStrategy(PhotonPoseEstimator.PoseStrategy.LOWEST_AMBIGUITY); } catch (IOException e){ throw new IllegalStateException(e); } @@ -58,9 +59,6 @@ public void updateInputs() { inputs.setTagIds(getTargetIDs()); inputs.setNumTagsFound(m_camera.getLatestResult().targets.size()); inputs.setTagsFound(m_camera.getLatestResult().hasTargets()); - - Logger.recordOutput("PhotonVision/" + m_name + "/Last Estimated Pose", m_lastEstimatedPose); - Logger.processInputs("PhotonVision/" + m_name, inputs); } public Optional getPose(Pose2d prevEstimatedRobotPose) { diff --git a/src/main/java/lib/logger/BooleanLoggers.java b/src/main/java/lib/logger/BooleanLoggers.java new file mode 100644 index 00000000..1d6f23b6 --- /dev/null +++ b/src/main/java/lib/logger/BooleanLoggers.java @@ -0,0 +1,38 @@ +package lib.logger; + +import edu.wpi.first.networktables.NetworkTableEntry; +import edu.wpi.first.util.datalog.BooleanLogEntry; +import edu.wpi.first.wpilibj.DataLogManager; + +import java.util.function.BooleanSupplier; + +public class BooleanLoggers { + public static class BooleanLogger { + private final NetworkTableEntry m_networkTableEntry; + private final boolean m_updateNT; + private final BooleanSupplier m_booleanSupplier; + private final BooleanLogEntry m_logEntry; + private boolean m_lastValue; + + public BooleanLogger(NetworkTableEntry networkTableEntry, BooleanSupplier booleanSupplier, boolean updateNT) { + m_networkTableEntry = networkTableEntry; + m_booleanSupplier = booleanSupplier; + + m_updateNT = updateNT; + + m_lastValue = m_booleanSupplier.getAsBoolean(); + m_logEntry = new BooleanLogEntry(DataLogManager.getLog(), m_networkTableEntry.getName()); + } + + public void updateBooleanEntry() { + if (m_updateNT) { + m_networkTableEntry.setBoolean(m_booleanSupplier.getAsBoolean()); + } + + if (m_booleanSupplier.getAsBoolean() != m_lastValue) { + m_lastValue = m_booleanSupplier.getAsBoolean(); + m_logEntry.append(m_lastValue); + } + } + } +} diff --git a/src/main/java/lib/logger/ComplexLoggers.java b/src/main/java/lib/logger/ComplexLoggers.java new file mode 100644 index 00000000..d026e0be --- /dev/null +++ b/src/main/java/lib/logger/ComplexLoggers.java @@ -0,0 +1,39 @@ +package lib.logger; + +import edu.wpi.first.networktables.NetworkTableEntry; +import edu.wpi.first.util.datalog.StringLogEntry; +import edu.wpi.first.wpilibj.DataLogManager; + +import java.util.Objects; +import java.util.function.Supplier; + +public class ComplexLoggers { + // quick and lazy loggers for complex datatypes + public static class StringLogger { + private final NetworkTableEntry m_networkTableEntry; + private final boolean m_updateNT; + private final Supplier m_stringSupplier; + private final StringLogEntry m_logEntry; + private String m_lastValue; + + public StringLogger(NetworkTableEntry networkTableEntry, Supplier stringSupplier, boolean updateNT) { + m_networkTableEntry = networkTableEntry; + m_stringSupplier = stringSupplier; + m_updateNT = updateNT; + + m_lastValue = m_stringSupplier.get(); + m_logEntry = new StringLogEntry(DataLogManager.getLog(), m_networkTableEntry.getName()); + } + + public void updateStingEntry() { + if (m_updateNT) { + m_networkTableEntry.setString(m_stringSupplier.get()); + } + + if (!Objects.equals(m_stringSupplier.get(), m_lastValue)) { + m_lastValue = m_stringSupplier.get(); + m_logEntry.append(m_lastValue); + } + } + } +} diff --git a/src/main/java/lib/logger/DataLogUtil.java b/src/main/java/lib/logger/DataLogUtil.java new file mode 100644 index 00000000..5a67af64 --- /dev/null +++ b/src/main/java/lib/logger/DataLogUtil.java @@ -0,0 +1,145 @@ +package lib.logger; + +import edu.wpi.first.math.geometry.Pose2d; +import edu.wpi.first.math.geometry.Pose3d; +import edu.wpi.first.math.kinematics.SwerveModuleState; +import edu.wpi.first.networktables.NetworkTable; +import edu.wpi.first.networktables.NetworkTableInstance; + +import java.util.ArrayList; +import java.util.List; +import java.util.Objects; +import java.util.function.BooleanSupplier; +import java.util.function.DoubleSupplier; +import java.util.function.Supplier; + +import lib.logger.DoubleLoggers.DoubleLogger; +import lib.logger.DoubleLoggers.DoubleArrayLogger; +import lib.logger.BooleanLoggers.BooleanLogger; +import lib.logger.ComplexLoggers.StringLogger; + +public class DataLogUtil { + private static final List logTables = new ArrayList<>(); + + public static DataLogTable getTable(String name) { + for (DataLogTable table : logTables) { + if (Objects.equals(table.name, name)) { + return table; + } + } + + DataLogTable table = new DataLogTable(name); + logTables.add(table); + return table; + } + + public static void updateTables() { + for (DataLogTable table : logTables) { + table.updateLogs(); + } + } + + public static class DataLogTable { + public final String name; + private final NetworkTable m_loggingTable; + + private final List m_doubleLogs = new ArrayList<>(); + private final List m_doubleArrayLogs = new ArrayList<>(); + private final List m_booleanLogs = new ArrayList<>(); + private final List m_stringLogs = new ArrayList<>(); + + public DataLogTable(String loggingTableName) { + this(NetworkTableInstance.getDefault().getTable(loggingTableName)); + } + + public DataLogTable(NetworkTable loggingTable) { + m_loggingTable = loggingTable; + name = m_loggingTable.toString(); + } + + public void addDouble(String logName, DoubleSupplier updateChecker, boolean updateNt) { + m_doubleLogs.add(new DoubleLogger(m_loggingTable.getEntry(logName), updateChecker, updateNt)); + } + + public void addDoubleArray(String logName, Supplier updateChecker, boolean updateNt) { + m_doubleArrayLogs.add(new DoubleArrayLogger(m_loggingTable.getEntry(logName), updateChecker, updateNt)); + } + + public void addBoolean(String logName, BooleanSupplier updateChecker, boolean updateNT) { + m_booleanLogs.add(new BooleanLogger(m_loggingTable.getEntry(logName), updateChecker, updateNT)); + } + + public void addString(String logName, Supplier updateChecker, boolean updateNT) { + m_stringLogs.add(new StringLogger(m_loggingTable.getEntry(logName), updateChecker, updateNT)); + } + + public void addPose2d(String logName, Supplier updateChecker, boolean updateNT) { + addDoubleArray(logName, + () -> new double[]{ + updateChecker.get().getX(), + updateChecker.get().getY(), + updateChecker.get().getRotation().getRadians() + }, + updateNT); + } + + public void addPose2dArray(String logName, Supplier updateChecker, boolean updateNT) { + for (int i = 0; i < updateChecker.get().length; i++) { + int finalI = i; + addPose2d(logName + "/" + i, + () -> updateChecker.get()[finalI], + updateNT); + } + } + + public void addPose3d(String logName, Supplier updateChecker, boolean updateNT) { + addDoubleArray(logName, + () -> new double[] { + updateChecker.get().getX(), + updateChecker.get().getY(), + updateChecker.get().getZ(), + updateChecker.get().getRotation().getQuaternion().getW(), + updateChecker.get().getRotation().getQuaternion().getX(), + updateChecker.get().getRotation().getQuaternion().getY(), + updateChecker.get().getRotation().getQuaternion().getZ() + }, + updateNT); + } + + public void addPose3dArray(String logName, Supplier updateChecker, boolean updateNT) { + for (int i = 0; i < updateChecker.get().length; i++) { + int finalI = i; + addPose3d(logName + "/" + i, + () -> updateChecker.get()[finalI], + updateNT); + } + } + + public void addSwerveModuleStateArray(String logName, Supplier updateChecker, boolean updateNT) { + addDoubleArray(logName, () -> { + double[] stateArray = new double[8]; + for (int i = 0; i < updateChecker.get().length * 2; i += 2) { + stateArray[i] = updateChecker.get()[(int) (i / 2.0)].angle.getRadians(); + stateArray[i + 1] = updateChecker.get()[(int) (i / 2.0)].speedMetersPerSecond; + } + return stateArray; + }, + updateNT); + } + + public void updateLogs() { + for (BooleanLogger booleanLogger : m_booleanLogs) { + booleanLogger.updateBooleanEntry(); + } + for (DoubleLogger doubleLogger : m_doubleLogs) { + doubleLogger.updateDoubleEntry(); + } + for (DoubleArrayLogger doubleArrayLogger : m_doubleArrayLogs) { + doubleArrayLogger.updateDoubleArrayEntry(); + } + for (StringLogger stringLogger : m_stringLogs) { + stringLogger.updateStingEntry(); + } + } + } +} diff --git a/src/main/java/lib/logger/DoubleLoggers.java b/src/main/java/lib/logger/DoubleLoggers.java new file mode 100644 index 00000000..75a36977 --- /dev/null +++ b/src/main/java/lib/logger/DoubleLoggers.java @@ -0,0 +1,67 @@ +package lib.logger; + +import edu.wpi.first.networktables.NetworkTableEntry; +import edu.wpi.first.util.datalog.DoubleArrayLogEntry; +import edu.wpi.first.util.datalog.DoubleLogEntry; +import edu.wpi.first.wpilibj.DataLogManager; + +import java.util.function.DoubleSupplier; +import java.util.function.Supplier; + +public class DoubleLoggers { + public static class DoubleLogger { + private final NetworkTableEntry m_networkTableEntry; + private final boolean m_updateNT; + private final DoubleSupplier m_doubleSupplier; + private final DoubleLogEntry m_logEntry; + private double m_lastValue; + + public DoubleLogger(NetworkTableEntry networkTableEntry, DoubleSupplier doubleSupplier, boolean updateNT) { + m_networkTableEntry = networkTableEntry; + m_doubleSupplier = doubleSupplier; + m_updateNT = updateNT; + + m_lastValue = m_doubleSupplier.getAsDouble(); + m_logEntry = new DoubleLogEntry(DataLogManager.getLog(), m_networkTableEntry.getName()); + } + + public void updateDoubleEntry() { + if (m_updateNT) { + m_networkTableEntry.setNumber(m_doubleSupplier.getAsDouble()); + } + + if (m_doubleSupplier.getAsDouble() != m_lastValue) { + m_lastValue = m_doubleSupplier.getAsDouble(); + m_logEntry.append(m_lastValue); + } + } + } + + public static class DoubleArrayLogger { + private final NetworkTableEntry m_networkTableEntry; + private final boolean m_updateNT; + private final Supplier m_doubleSupplier; + private final DoubleArrayLogEntry m_logEntry; + private double[] m_lastValue; + + public DoubleArrayLogger(NetworkTableEntry networkTableEntry, Supplier doubleSupplier, boolean updateNT) { + m_networkTableEntry = networkTableEntry; + m_doubleSupplier = doubleSupplier; + m_updateNT = updateNT; + + m_lastValue = m_doubleSupplier.get(); + m_logEntry = new DoubleArrayLogEntry(DataLogManager.getLog(), m_networkTableEntry.getName()); + } + + public void updateDoubleArrayEntry() { + if (m_updateNT) { + m_networkTableEntry.setDoubleArray(m_doubleSupplier.get()); + } + + if (m_doubleSupplier.get() != m_lastValue) { + m_lastValue = m_doubleSupplier.get(); + m_logEntry.append(m_lastValue); + } + } + } +} diff --git a/src/main/java/lib/utils/AimbotUtils.java b/src/main/java/lib/utils/AimbotUtils.java index 1983e65a..44766f2d 100644 --- a/src/main/java/lib/utils/AimbotUtils.java +++ b/src/main/java/lib/utils/AimbotUtils.java @@ -4,9 +4,7 @@ import edu.wpi.first.math.interpolation.InterpolatingDoubleTreeMap; import edu.wpi.first.math.util.Units; import frc.robot.Constants.ArmConstants; -import frc.robot.Constants.ArmSetpoints; import frc.robot.subsystems.arm.ArmPose; -import org.littletonrobotics.junction.Logger; public class AimbotUtils { diff --git a/src/main/java/lib/utils/ArmTrajectoryGenerator.java b/src/main/java/lib/utils/ArmTrajectoryGenerator.java new file mode 100644 index 00000000..f6d5d3d1 --- /dev/null +++ b/src/main/java/lib/utils/ArmTrajectoryGenerator.java @@ -0,0 +1,168 @@ +package lib.utils; + +import java.util.ArrayList; +import java.util.List; + +public class ArmTrajectoryGenerator { + + private static double lerp(double startValue, double endValue, double t) { + return startValue + (endValue - startValue) * t; + } + + public static ArmTrajectory generateTrajectory(List setpoints, ArmTrajectoryConfig config) { + List m_states = new ArrayList<>(); + // forwards pass for ramp up + double timestamp = 0.0; + for (int i = 0; i < setpoints.size() - 1; i++) { + ArmTrajectoryPose current = setpoints.get(i); + ArmTrajectoryPose next = setpoints.get(i + 1); + + // look ahead a consistent time step (20 ms) and calculate the state + ArmTrajectory.ArmTrajectoryState prevState = null; + while (true) { + if (prevState == null) { + // inital state + prevState = new ArmTrajectory.ArmTrajectoryState( + timestamp, + current.armPoseDegs, + current.armVelocityDegsPerSecond, + config.armMaxAccelerationDegsPerSecondSq, + current.wristPoseDegs, + current.wristVelocityDegsPerSecond, + config.wristMaxAccelerationDegsPerSecondSq); + + m_states.add(prevState); + } + + // check to see if we're past the setpoint trying to move to, if we are then continue to the next set of + // setpoints + if (((prevState.armPoseDegs > next.armPoseDegs && prevState.armVelocityDegsPerSecond > 0.0) + || (prevState.armPoseDegs < next.armPoseDegs && prevState.armVelocityDegsPerSecond < 0.0)) + && ((prevState.wristPoseDegs > next.wristPoseDegs && prevState.wristVelocityDegsPerSecond > 0.0) + || (prevState.wristPoseDegs < next.wristPoseDegs && prevState.wristVelocityDegsPerSecond < 0.0))) { + break; + } + + timestamp += 0.02; + + // calculate new joint positions + // dx = x + vt + 1/2at^2 + double newArmPose = prevState.armPoseDegs + + (1.0 / 2.0) * config.armMaxAccelerationDegsPerSecondSq * Math.pow(0.02, 2); + double newWristPose = prevState.wristPoseDegs + + (1.0 / 2.0) * config.wristMaxAccelerationDegsPerSecondSq * Math.pow(0.02, 2); + + // calculate new joint velocities + // vf = sqrt(vi^2 + 2at) + double newArmVel = Math.min(Math.sqrt(Math.pow(prevState.armVelocityDegsPerSecond, 2) + + 2 * config.armMaxAccelerationDegsPerSecondSq + 0.02), + config.armMaxVelocityDegsPerSecond); + double newWristVel = Math.min(Math.sqrt(Math.pow(prevState.wristVelocityDegsPerSecond, 2) + + 2 * config.wristMaxAccelerationDegsPerSecondSq + 0.02), + config.wristMaxVelocityDegsPerSecond); + + // create new point at time step + prevState = new ArmTrajectory.ArmTrajectoryState( + timestamp, + newArmPose, + newArmVel, + config.armMaxAccelerationDegsPerSecondSq, + newWristPose, + newWristVel, + config.wristMaxAccelerationDegsPerSecondSq); + + m_states.add(prevState); + } + } + + return new ArmTrajectory(m_states); + } + + public record ArmTrajectoryPose(double armPoseDegs, double armVelocityDegsPerSecond, + double wristPoseDegs, double wristVelocityDegsPerSecond) {} + + public record ArmTrajectoryConfig(double armMaxVelocityDegsPerSecond, double armMaxAccelerationDegsPerSecondSq, + double wristMaxVelocityDegsPerSecond, double wristMaxAccelerationDegsPerSecondSq) {} + + public static class ArmTrajectory { + private final List states; + + public ArmTrajectory(List states) { + this.states = states; + } + + public record ArmTrajectoryState(double timeSeconds, + double armPoseDegs, + double armVelocityDegsPerSecond, + double armAccelerationDegsPerSecondSq, + double wristPoseDegs, + double wristVelocityDegsPerSecond, + double wristAccelerationDegsPerSecondSq) { + ArmTrajectoryState interpolate(ArmTrajectoryState endValue, double i) { + // Find the new t value. + final double newT = lerp(timeSeconds, endValue.timeSeconds, i); + + // Find the delta time between the current state and the interpolated state. + final double deltaT = newT - timeSeconds; + + // If delta time is negative, flip the order of interpolation. + if (deltaT < 0) { + return endValue.interpolate(this, 1 - i); + } + + // run interpolation on arm positions and velocities + // Check whether the robot is reversing at this stage. + final boolean armReversing = armVelocityDegsPerSecond < 0; + + // Calculate the new velocity + // v_f = v_0 + at + final double newArmV = armVelocityDegsPerSecond + (armAccelerationDegsPerSecondSq * deltaT); + + // Calculate the change in position. + // delta_s = v_0 t + 0.5at² + final double newArmS = + (armVelocityDegsPerSecond * deltaT + + 0.5 * armAccelerationDegsPerSecondSq * Math.pow(deltaT, 2)) + * (armReversing ? -1.0 : 1.0); + + // To find the new position for the new state, we need + // to interpolate between the two endpoint poses. The fraction for + // interpolation is the change in position (delta s) divided by the total + // distance between the two endpoints. + final double armInterpolationFrac = + newArmS / endValue.armPoseDegs() - armPoseDegs; + + // run interpolation on wrist positions and velocities + // Check whether the robot is reversing at this stage. + final boolean wristReversing = wristVelocityDegsPerSecond < 0; + + // Calculate the new velocity + // v_f = v_0 + at + final double newWristV = wristVelocityDegsPerSecond + (wristAccelerationDegsPerSecondSq * deltaT); + + // Calculate the change in position. + // delta_s = v_0 t + 0.5at² + final double newWristS = + (wristVelocityDegsPerSecond * deltaT + + 0.5 * wristAccelerationDegsPerSecondSq * Math.pow(deltaT, 2)) + * (wristReversing ? -1.0 : 1.0); + + // Return the new state. To find the new position for the new state, we need + // to interpolate between the two endpoint poses. The fraction for + // interpolation is the change in position (delta s) divided by the total + // distance between the two endpoints. + final double wristInterpolationFrac = + newWristS / endValue.armPoseDegs() - wristPoseDegs; + + return new ArmTrajectoryState( + newT, + lerp(armPoseDegs, endValue.armPoseDegs, armInterpolationFrac), + newArmV, + armAccelerationDegsPerSecondSq, + lerp(wristPoseDegs, endValue.wristPoseDegs, wristInterpolationFrac), + newWristV, + wristAccelerationDegsPerSecondSq); + } + } + } +} diff --git a/src/main/java/lib/utils/LocalADStarAK.java b/src/main/java/lib/utils/LocalADStarAK.java deleted file mode 100644 index 39b1d665..00000000 --- a/src/main/java/lib/utils/LocalADStarAK.java +++ /dev/null @@ -1,153 +0,0 @@ -package lib.utils; - -import com.pathplanner.lib.path.GoalEndState; -import com.pathplanner.lib.path.PathConstraints; -import com.pathplanner.lib.path.PathPlannerPath; -import com.pathplanner.lib.path.PathPoint; -import com.pathplanner.lib.pathfinding.LocalADStar; -import com.pathplanner.lib.pathfinding.Pathfinder; -import edu.wpi.first.math.Pair; -import edu.wpi.first.math.geometry.Translation2d; -import java.util.ArrayList; -import java.util.Collections; -import java.util.List; -import org.littletonrobotics.junction.LogTable; -import org.littletonrobotics.junction.Logger; -import org.littletonrobotics.junction.inputs.LoggableInputs; - -// NOTE: This file is available at -// https://gist.github.com/mjansen4857/a8024b55eb427184dbd10ae8923bd57d - -public class LocalADStarAK implements Pathfinder { - private final ADStarIO io = new ADStarIO(); - - /** - * Get if a new path has been calculated since the last time a path was retrieved - * - * @return True if a new path is available - */ - @Override - public boolean isNewPathAvailable() { - if (!Logger.hasReplaySource()) { - io.updateIsNewPathAvailable(); - } - - Logger.processInputs("LocalADStarAK", io); - - return io.isNewPathAvailable; - } - - /** - * Get the most recently calculated path - * - * @param constraints The path constraints to use when creating the path - * @param goalEndState The goal end state to use when creating the path - * @return The PathPlannerPath created from the points calculated by the pathfinder - */ - @Override - public PathPlannerPath getCurrentPath(PathConstraints constraints, GoalEndState goalEndState) { - if (!Logger.hasReplaySource()) { - io.updateCurrentPathPoints(constraints, goalEndState); - } - - Logger.processInputs("LocalADStarAK", io); - - if (io.currentPathPoints.isEmpty()) { - return null; - } - - return PathPlannerPath.fromPathPoints(io.currentPathPoints, constraints, goalEndState); - } - - /** - * Set the start position to pathfind from - * - * @param startPosition Start position on the field. If this is within an obstacle it will be - * moved to the nearest non-obstacle node. - */ - @Override - public void setStartPosition(Translation2d startPosition) { - if (!Logger.hasReplaySource()) { - io.adStar.setStartPosition(startPosition); - } - } - - /** - * Set the goal position to pathfind to - * - * @param goalPosition Goal position on the field. f this is within an obstacle it will be moved - * to the nearest non-obstacle node. - */ - @Override - public void setGoalPosition(Translation2d goalPosition) { - if (!Logger.hasReplaySource()) { - io.adStar.setGoalPosition(goalPosition); - } - } - - /** - * Set the dynamic obstacles that should be avoided while pathfinding. - * - * @param obs A List of Translation2d pairs representing obstacles. Each Translation2d represents - * opposite corners of a bounding box. - * @param currentRobotPos The current position of the robot. This is needed to change the start - * position of the path to properly avoid obstacles - */ - @Override - public void setDynamicObstacles( - List> obs, Translation2d currentRobotPos) { - if (!Logger.hasReplaySource()){ - io.adStar.setDynamicObstacles(obs, currentRobotPos); - } - } - - private static class ADStarIO implements LoggableInputs { - protected LocalADStar adStar = new LocalADStar(); - protected boolean isNewPathAvailable = false; - protected List currentPathPoints = Collections.emptyList(); - - @Override - public void toLog(LogTable table) { - table.put("IsNewPathAvailable", isNewPathAvailable); - - double[] pointsLogged = new double[currentPathPoints.size() * 2]; - int idx = 0; - for (PathPoint point : currentPathPoints) { - pointsLogged[idx] = point.position.getX(); - pointsLogged[idx + 1] = point.position.getY(); - idx += 2; - } - - table.put("CurrentPathPoints", pointsLogged); - } - - @Override - public void fromLog(LogTable table) { - isNewPathAvailable = table.get("IsNewPathAvailable", false); - - double[] pointsLogged = table.get("CurrentPathPoints", new double[0]); - - List pathPoints = new ArrayList<>(); - for (int i = 0; i < pointsLogged.length; i += 2) { - pathPoints.add( - new PathPoint(new Translation2d(pointsLogged[i], pointsLogged[i + 1]), null)); - } - - currentPathPoints = pathPoints; - } - - public void updateIsNewPathAvailable() { - isNewPathAvailable = adStar.isNewPathAvailable(); - } - - public void updateCurrentPathPoints(PathConstraints constraints, GoalEndState goalEndState) { - PathPlannerPath currentPath = adStar.getCurrentPath(constraints, goalEndState); - - if (currentPath != null) { - currentPathPoints = currentPath.getAllPathPoints(); - } else { - currentPathPoints = Collections.emptyList(); - } - } - } -} diff --git a/vendordeps/AdvantageKit.json b/vendordeps/AdvantageKit.json deleted file mode 100644 index 623d1572..00000000 --- a/vendordeps/AdvantageKit.json +++ /dev/null @@ -1,42 +0,0 @@ -{ - "fileName": "AdvantageKit.json", - "name": "AdvantageKit", - "version": "3.1.0", - "uuid": "d820cc26-74e3-11ec-90d6-0242ac120003", - "frcYear": "2024", - "mavenUrls": [], - "jsonUrl": "https://github.com/Mechanical-Advantage/AdvantageKit/releases/latest/download/AdvantageKit.json", - "javaDependencies": [ - { - "groupId": "org.littletonrobotics.akit.junction", - "artifactId": "wpilib-shim", - "version": "3.1.0" - }, - { - "groupId": "org.littletonrobotics.akit.junction", - "artifactId": "junction-core", - "version": "3.1.0" - }, - { - "groupId": "org.littletonrobotics.akit.conduit", - "artifactId": "conduit-api", - "version": "3.1.0" - } - ], - "jniDependencies": [ - { - "groupId": "org.littletonrobotics.akit.conduit", - "artifactId": "conduit-wpilibio", - "version": "3.1.0", - "skipInvalidPlatforms": false, - "isJar": false, - "validPlatforms": [ - "linuxathena", - "windowsx86-64", - "linuxx86-64", - "osxuniversal" - ] - } - ], - "cppDependencies": [] -} \ No newline at end of file