From 8a80d221b16139e209888da364de26eec64353d4 Mon Sep 17 00:00:00 2001 From: AquaShock211 <143662909+AquaShock211@users.noreply.github.com> Date: Sat, 18 Jan 2025 10:16:58 -0700 Subject: [PATCH 01/15] PID Tuning --- src/main/java/frc/robot/RobotContainer.java | 91 ++++++++++++------- .../java/frc/robot/commands/OnTheFly.java | 5 + .../frc/robot/subsystems/drive/Drive.java | 2 +- 3 files changed, 64 insertions(+), 34 deletions(-) create mode 100644 src/main/java/frc/robot/commands/OnTheFly.java diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 7cd9608..90005bf 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -36,7 +36,6 @@ import edu.wpi.first.wpilibj2.command.Commands; import edu.wpi.first.wpilibj2.command.button.CommandXboxController; import edu.wpi.first.wpilibj2.command.button.RobotModeTriggers; -import edu.wpi.first.wpilibj2.command.sysid.SysIdRoutine; import frc.robot.Constants.AprilTagConstants.AprilTagLayoutType; import frc.robot.Constants.OperatorConstants; import frc.robot.commands.DriveCommands; @@ -193,6 +192,32 @@ public RobotContainer() { definesysIdRoutines(); } + // // Create a list of waypoints from poses. Each pose represents one waypoint. + // // The rotation component of the pose should be the direction of travel. Do not use holonomic + // rotation. + // List waypoints = PathPlannerPath.waypointsFromPoses( + // new Pose2d(1.0, 1.0, Rotation2d.fromDegrees(0)), + // new Pose2d(5.0, 3.0, Rotation2d.fromDegrees(90)) + // ); + + // PathConstraints constraints = new PathConstraints(3.0, 3.0, 2 * Math.PI, 4 * Math.PI); // The + // constraints for this path. + // // PathConstraints constraints = PathConstraints.unlimitedConstraints(12.0); // You can also + // use unlimited constraints, only limited by motor torque and nominal battery voltage + + // // Create the path using the waypoints created above + // PathPlannerPath path = new PathPlannerPath( + // waypoints, + // constraints, + // null, // The ideal starting state, this is only relevant for pre-planned paths, so can be + // null for on-the-fly paths. + // new GoalEndState(0.0, Rotation2d.fromDegrees(-90)) // Goal end state. You can set a holonomic + // rotation here. If using a differential drivetrain, the rotation will have no effect. + // ); + + // // Prevent the path from being flipped if the coordinates are already correct + // path.preventFlipping = true; + /** Use this method to define your Autonomous commands for use with PathPlanner / Choreo */ private void defineAutoCommands() { @@ -325,38 +350,38 @@ public void updateAlerts() { private void definesysIdRoutines() { if (Constants.getAutoType() == RBSIEnum.AutoType.PATHPLANNER) { // Drivebase characterization - autoChooserPathPlanner.addOption( - "Drive Wheel Radius Characterization", - DriveCommands.wheelRadiusCharacterization(m_drivebase)); - autoChooserPathPlanner.addOption( - "Drive Simple FF Characterization", - DriveCommands.feedforwardCharacterization(m_drivebase)); - autoChooserPathPlanner.addOption( - "Drive SysId (Quasistatic Forward)", - m_drivebase.sysIdQuasistatic(SysIdRoutine.Direction.kForward)); - autoChooserPathPlanner.addOption( - "Drive SysId (Quasistatic Reverse)", - m_drivebase.sysIdQuasistatic(SysIdRoutine.Direction.kReverse)); - autoChooserPathPlanner.addOption( - "Drive SysId (Dynamic Forward)", - m_drivebase.sysIdDynamic(SysIdRoutine.Direction.kForward)); - autoChooserPathPlanner.addOption( - "Drive SysId (Dynamic Reverse)", - m_drivebase.sysIdDynamic(SysIdRoutine.Direction.kReverse)); - - // Example Flywheel SysId Characterization - autoChooserPathPlanner.addOption( - "Flywheel SysId (Quasistatic Forward)", - m_flywheel.sysIdQuasistatic(SysIdRoutine.Direction.kForward)); - autoChooserPathPlanner.addOption( - "Flywheel SysId (Quasistatic Reverse)", - m_flywheel.sysIdQuasistatic(SysIdRoutine.Direction.kReverse)); - autoChooserPathPlanner.addOption( - "Flywheel SysId (Dynamic Forward)", - m_flywheel.sysIdDynamic(SysIdRoutine.Direction.kForward)); - autoChooserPathPlanner.addOption( - "Flywheel SysId (Dynamic Reverse)", - m_flywheel.sysIdDynamic(SysIdRoutine.Direction.kReverse)); + // autoChooserPathPlanner.addOption( + // "Drive Wheel Radius Characterization", + // DriveCommands.wheelRadiusCharacterization(m_drivebase)); + // autoChooserPathPlanner.addOption( + // "Drive Simple FF Characterization", + // DriveCommands.feedforwardCharacterization(m_drivebase)); + // autoChooserPathPlanner.addOption( + // "Drive SysId (Quasistatic Forward)", + // m_drivebase.sysIdQuasistatic(SysIdRoutine.Direction.kForward)); + // autoChooserPathPlanner.addOption( + // "Drive SysId (Quasistatic Reverse)", + // m_drivebase.sysIdQuasistatic(SysIdRoutine.Direction.kReverse)); + // autoChooserPathPlanner.addOption( + // "Drive SysId (Dynamic Forward)", + // m_drivebase.sysIdDynamic(SysIdRoutine.Direction.kForward)); + // autoChooserPathPlanner.addOption( + // "Drive SysId (Dynamic Reverse)", + // m_drivebase.sysIdDynamic(SysIdRoutine.Direction.kReverse)); + + // // Example Flywheel SysId Characterization + // autoChooserPathPlanner.addOption( + // "Flywheel SysId (Quasistatic Forward)", + // m_flywheel.sysIdQuasistatic(SysIdRoutine.Direction.kForward)); + // autoChooserPathPlanner.addOption( + // "Flywheel SysId (Quasistatic Reverse)", + // m_flywheel.sysIdQuasistatic(SysIdRoutine.Direction.kReverse)); + // autoChooserPathPlanner.addOption( + // "Flywheel SysId (Dynamic Forward)", + // m_flywheel.sysIdDynamic(SysIdRoutine.Direction.kForward)); + // autoChooserPathPlanner.addOption( + // "Flywheel SysId (Dynamic Reverse)", + // m_flywheel.sysIdDynamic(SysIdRoutine.Direction.kReverse)); } } diff --git a/src/main/java/frc/robot/commands/OnTheFly.java b/src/main/java/frc/robot/commands/OnTheFly.java new file mode 100644 index 0000000..1a9b015 --- /dev/null +++ b/src/main/java/frc/robot/commands/OnTheFly.java @@ -0,0 +1,5 @@ +package frc.robot.commands; + +import edu.wpi.first.wpilibj2.command.Command; + +public class OnTheFly extends Command {} diff --git a/src/main/java/frc/robot/subsystems/drive/Drive.java b/src/main/java/frc/robot/subsystems/drive/Drive.java index 7d468d9..1716362 100644 --- a/src/main/java/frc/robot/subsystems/drive/Drive.java +++ b/src/main/java/frc/robot/subsystems/drive/Drive.java @@ -169,7 +169,7 @@ public Drive() { this::getChassisSpeeds, this::runVelocity, new PPHolonomicDriveController( - new PIDConstants(5.0, 0.0, 0.0), new PIDConstants(5.0, 0.0, 0.0)), + new PIDConstants(2.0, 0.0, 0.0), new PIDConstants(0.6, 0.0, 0.0)), PP_CONFIG, () -> DriverStation.getAlliance().orElse(Alliance.Blue) == Alliance.Red, this); From 16e74f5a7e623604e8d242dda7f688d74cae5d9b Mon Sep 17 00:00:00 2001 From: AquaShock211 <143662909+AquaShock211@users.noreply.github.com> Date: Sat, 18 Jan 2025 13:27:36 -0700 Subject: [PATCH 02/15] Test Autos and more PID changes --- .../pathplanner/autos/Back and Forth.auto | 25 +++++++++ .../deploy/pathplanner/autos/Rotate 180.auto | 19 +++++++ .../deploy/pathplanner/autos/Rotate 90.auto | 19 +++++++ .../autos/{Test.auto => Test 4ft.auto} | 0 .../pathplanner/paths/Consistancy Test.path | 18 +++---- .../paths/Copy of Consistancy Test.path | 54 +++++++++++++++++++ .../pathplanner/paths/Full Rotation.path | 54 +++++++++++++++++++ .../pathplanner/paths/Rotation Test.path | 54 +++++++++++++++++++ src/main/deploy/pathplanner/settings.json | 2 +- .../frc/robot/subsystems/drive/Drive.java | 5 +- 10 files changed, 239 insertions(+), 11 deletions(-) create mode 100644 src/main/deploy/pathplanner/autos/Back and Forth.auto create mode 100644 src/main/deploy/pathplanner/autos/Rotate 180.auto create mode 100644 src/main/deploy/pathplanner/autos/Rotate 90.auto rename src/main/deploy/pathplanner/autos/{Test.auto => Test 4ft.auto} (100%) create mode 100644 src/main/deploy/pathplanner/paths/Copy of Consistancy Test.path create mode 100644 src/main/deploy/pathplanner/paths/Full Rotation.path create mode 100644 src/main/deploy/pathplanner/paths/Rotation Test.path diff --git a/src/main/deploy/pathplanner/autos/Back and Forth.auto b/src/main/deploy/pathplanner/autos/Back and Forth.auto new file mode 100644 index 0000000..eac2ca1 --- /dev/null +++ b/src/main/deploy/pathplanner/autos/Back and Forth.auto @@ -0,0 +1,25 @@ +{ + "version": "2025.0", + "command": { + "type": "sequential", + "data": { + "commands": [ + { + "type": "path", + "data": { + "pathName": "Consistancy Test" + } + }, + { + "type": "path", + "data": { + "pathName": "Copy of Consistancy Test" + } + } + ] + } + }, + "resetOdom": true, + "folder": null, + "choreoAuto": false +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/autos/Rotate 180.auto b/src/main/deploy/pathplanner/autos/Rotate 180.auto new file mode 100644 index 0000000..27d5670 --- /dev/null +++ b/src/main/deploy/pathplanner/autos/Rotate 180.auto @@ -0,0 +1,19 @@ +{ + "version": "2025.0", + "command": { + "type": "sequential", + "data": { + "commands": [ + { + "type": "path", + "data": { + "pathName": "Rotation Test" + } + } + ] + } + }, + "resetOdom": true, + "folder": null, + "choreoAuto": false +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/autos/Rotate 90.auto b/src/main/deploy/pathplanner/autos/Rotate 90.auto new file mode 100644 index 0000000..27d5670 --- /dev/null +++ b/src/main/deploy/pathplanner/autos/Rotate 90.auto @@ -0,0 +1,19 @@ +{ + "version": "2025.0", + "command": { + "type": "sequential", + "data": { + "commands": [ + { + "type": "path", + "data": { + "pathName": "Rotation Test" + } + } + ] + } + }, + "resetOdom": true, + "folder": null, + "choreoAuto": false +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/autos/Test.auto b/src/main/deploy/pathplanner/autos/Test 4ft.auto similarity index 100% rename from src/main/deploy/pathplanner/autos/Test.auto rename to src/main/deploy/pathplanner/autos/Test 4ft.auto diff --git a/src/main/deploy/pathplanner/paths/Consistancy Test.path b/src/main/deploy/pathplanner/paths/Consistancy Test.path index 232a7a0..a6f90b5 100644 --- a/src/main/deploy/pathplanner/paths/Consistancy Test.path +++ b/src/main/deploy/pathplanner/paths/Consistancy Test.path @@ -3,25 +3,25 @@ "waypoints": [ { "anchor": { - "x": 9.352443530509635, + "x": 9.335298121098802, "y": 6.184303023726215 }, "prevControl": null, "nextControl": { - "x": 8.288248974318456, - "y": 6.2248937493479595 + "x": 8.416560228412514, + "y": 6.1682387913029775 }, "isLocked": false, "linkedName": null }, { "anchor": { - "x": 8.18080005787037, + "x": 8.218592664930556, "y": 6.184303023726215 }, "prevControl": { - "x": 8.558069551829362, - "y": 6.168244196346038 + "x": 8.883800254470446, + "y": 6.188077684828391 }, "nextControl": null, "isLocked": false, @@ -33,15 +33,15 @@ "pointTowardsZones": [], "eventMarkers": [], "globalConstraints": { - "maxVelocity": 1.0, - "maxAcceleration": 1.0, + "maxVelocity": 4.0, + "maxAcceleration": 6.0, "maxAngularVelocity": 540.0, "maxAngularAcceleration": 1200.0, "nominalVoltage": 12.0, "unlimited": false }, "goalEndState": { - "velocity": 0, + "velocity": 0.0, "rotation": 0.0 }, "reversed": false, diff --git a/src/main/deploy/pathplanner/paths/Copy of Consistancy Test.path b/src/main/deploy/pathplanner/paths/Copy of Consistancy Test.path new file mode 100644 index 0000000..37de65e --- /dev/null +++ b/src/main/deploy/pathplanner/paths/Copy of Consistancy Test.path @@ -0,0 +1,54 @@ +{ + "version": "2025.0", + "waypoints": [ + { + "anchor": { + "x": 8.219, + "y": 6.184303023726215 + }, + "prevControl": null, + "nextControl": { + "x": 7.881377691007526, + "y": 6.175763200957433 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 9.335, + "y": 6.184303023726215 + }, + "prevControl": { + "x": 9.584992488282404, + "y": 6.186241012986395 + }, + "nextControl": null, + "isLocked": false, + "linkedName": null + } + ], + "rotationTargets": [], + "constraintZones": [], + "pointTowardsZones": [], + "eventMarkers": [], + "globalConstraints": { + "maxVelocity": 4.0, + "maxAcceleration": 6.0, + "maxAngularVelocity": 540.0, + "maxAngularAcceleration": 1200.0, + "nominalVoltage": 12.0, + "unlimited": false + }, + "goalEndState": { + "velocity": 0.0, + "rotation": 0.0 + }, + "reversed": false, + "folder": null, + "idealStartingState": { + "velocity": 0, + "rotation": 0.0 + }, + "useDefaultConstraints": false +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/Full Rotation.path b/src/main/deploy/pathplanner/paths/Full Rotation.path new file mode 100644 index 0000000..984eb1f --- /dev/null +++ b/src/main/deploy/pathplanner/paths/Full Rotation.path @@ -0,0 +1,54 @@ +{ + "version": "2025.0", + "waypoints": [ + { + "anchor": { + "x": 9.335298121098802, + "y": 6.184303023726215 + }, + "prevControl": null, + "nextControl": { + "x": 8.416560228412514, + "y": 6.1682387913029775 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 8.218592664930556, + "y": 6.184303023726215 + }, + "prevControl": { + "x": 8.883800254470446, + "y": 6.188077684828391 + }, + "nextControl": null, + "isLocked": false, + "linkedName": null + } + ], + "rotationTargets": [], + "constraintZones": [], + "pointTowardsZones": [], + "eventMarkers": [], + "globalConstraints": { + "maxVelocity": 4.0, + "maxAcceleration": 6.0, + "maxAngularVelocity": 540.0, + "maxAngularAcceleration": 1200.0, + "nominalVoltage": 12.0, + "unlimited": false + }, + "goalEndState": { + "velocity": 0.0, + "rotation": 180.0 + }, + "reversed": false, + "folder": null, + "idealStartingState": { + "velocity": 0, + "rotation": 0.0 + }, + "useDefaultConstraints": false +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/Rotation Test.path b/src/main/deploy/pathplanner/paths/Rotation Test.path new file mode 100644 index 0000000..80e5dc5 --- /dev/null +++ b/src/main/deploy/pathplanner/paths/Rotation Test.path @@ -0,0 +1,54 @@ +{ + "version": "2025.0", + "waypoints": [ + { + "anchor": { + "x": 9.335298121098802, + "y": 6.184303023726215 + }, + "prevControl": null, + "nextControl": { + "x": 8.416560228412514, + "y": 6.1682387913029775 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 8.218592664930556, + "y": 6.184303023726215 + }, + "prevControl": { + "x": 8.883800254470446, + "y": 6.188077684828391 + }, + "nextControl": null, + "isLocked": false, + "linkedName": null + } + ], + "rotationTargets": [], + "constraintZones": [], + "pointTowardsZones": [], + "eventMarkers": [], + "globalConstraints": { + "maxVelocity": 4.0, + "maxAcceleration": 6.0, + "maxAngularVelocity": 540.0, + "maxAngularAcceleration": 1200.0, + "nominalVoltage": 12.0, + "unlimited": false + }, + "goalEndState": { + "velocity": 0.0, + "rotation": 90.0 + }, + "reversed": false, + "folder": null, + "idealStartingState": { + "velocity": 0, + "rotation": 0.0 + }, + "useDefaultConstraints": false +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/settings.json b/src/main/deploy/pathplanner/settings.json index a7eab4d..27f6366 100644 --- a/src/main/deploy/pathplanner/settings.json +++ b/src/main/deploy/pathplanner/settings.json @@ -29,4 +29,4 @@ "bumperOffsetX": 0.0, "bumperOffsetY": 0.0, "robotFeatures": [] -} +} \ No newline at end of file diff --git a/src/main/java/frc/robot/subsystems/drive/Drive.java b/src/main/java/frc/robot/subsystems/drive/Drive.java index 1716362..16d7aa1 100644 --- a/src/main/java/frc/robot/subsystems/drive/Drive.java +++ b/src/main/java/frc/robot/subsystems/drive/Drive.java @@ -169,7 +169,10 @@ public Drive() { this::getChassisSpeeds, this::runVelocity, new PPHolonomicDriveController( - new PIDConstants(2.0, 0.0, 0.0), new PIDConstants(0.6, 0.0, 0.0)), + // Rotate + new PIDConstants(4, 0.0, 0.0), + // Drive + new PIDConstants(5, 0.0, 0)), PP_CONFIG, () -> DriverStation.getAlliance().orElse(Alliance.Blue) == Alliance.Red, this); From d2176c4cf8ae266ebf9e7392595d483c8ec7bc3c Mon Sep 17 00:00:00 2001 From: AquaShock211 <143662909+AquaShock211@users.noreply.github.com> Date: Sat, 18 Jan 2025 14:44:40 -0700 Subject: [PATCH 03/15] Added comments from old pid --- src/main/java/frc/robot/commands/OnTheFly.java | 5 ++++- src/main/java/frc/robot/subsystems/drive/Drive.java | 2 ++ 2 files changed, 6 insertions(+), 1 deletion(-) diff --git a/src/main/java/frc/robot/commands/OnTheFly.java b/src/main/java/frc/robot/commands/OnTheFly.java index 1a9b015..2fdcbb5 100644 --- a/src/main/java/frc/robot/commands/OnTheFly.java +++ b/src/main/java/frc/robot/commands/OnTheFly.java @@ -2,4 +2,7 @@ import edu.wpi.first.wpilibj2.command.Command; -public class OnTheFly extends Command {} +public class OnTheFly extends Command { + + +} diff --git a/src/main/java/frc/robot/subsystems/drive/Drive.java b/src/main/java/frc/robot/subsystems/drive/Drive.java index 16d7aa1..46fbeea 100644 --- a/src/main/java/frc/robot/subsystems/drive/Drive.java +++ b/src/main/java/frc/robot/subsystems/drive/Drive.java @@ -170,8 +170,10 @@ public Drive() { this::runVelocity, new PPHolonomicDriveController( // Rotate + //5, 0, 0 new PIDConstants(4, 0.0, 0.0), // Drive + //15, 0, 0 new PIDConstants(5, 0.0, 0)), PP_CONFIG, () -> DriverStation.getAlliance().orElse(Alliance.Blue) == Alliance.Red, From 5c509507dd62fa62a290568577d2c09353e70bc8 Mon Sep 17 00:00:00 2001 From: AquaShock211 <143662909+AquaShock211@users.noreply.github.com> Date: Sat, 18 Jan 2025 15:31:37 -0700 Subject: [PATCH 04/15] Auto and Wheel stuff --- .../pathplanner/paths/Consistancy Test.path | 4 ++-- src/main/deploy/pathplanner/settings.json | 2 +- src/main/java/frc/robot/Constants.java | 9 ++++++++- src/main/java/frc/robot/commands/OnTheFly.java | 5 +---- .../java/frc/robot/generated/TunerConstants.java | 16 ++++++++-------- .../java/frc/robot/subsystems/drive/Drive.java | 9 ++++----- src/main/java/frc/robot/util/RBSIEnum.java | 5 +++++ 7 files changed, 29 insertions(+), 21 deletions(-) diff --git a/src/main/deploy/pathplanner/paths/Consistancy Test.path b/src/main/deploy/pathplanner/paths/Consistancy Test.path index a6f90b5..5254abc 100644 --- a/src/main/deploy/pathplanner/paths/Consistancy Test.path +++ b/src/main/deploy/pathplanner/paths/Consistancy Test.path @@ -33,8 +33,8 @@ "pointTowardsZones": [], "eventMarkers": [], "globalConstraints": { - "maxVelocity": 4.0, - "maxAcceleration": 6.0, + "maxVelocity": 0.5, + "maxAcceleration": 1.0, "maxAngularVelocity": 540.0, "maxAngularAcceleration": 1200.0, "nominalVoltage": 12.0, diff --git a/src/main/deploy/pathplanner/settings.json b/src/main/deploy/pathplanner/settings.json index 27f6366..a7eab4d 100644 --- a/src/main/deploy/pathplanner/settings.json +++ b/src/main/deploy/pathplanner/settings.json @@ -29,4 +29,4 @@ "bumperOffsetX": 0.0, "bumperOffsetY": 0.0, "robotFeatures": [] -} \ 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 6d6eeac..adfb7e0 100644 --- a/src/main/java/frc/robot/Constants.java +++ b/src/main/java/frc/robot/Constants.java @@ -37,6 +37,7 @@ import frc.robot.util.RBSIEnum.CTREPro; import frc.robot.util.RBSIEnum.Mode; import frc.robot.util.RBSIEnum.MotorIdleMode; +import frc.robot.util.RBSIEnum.PracticeSpace; import frc.robot.util.RBSIEnum.RobotType; import frc.robot.util.RBSIEnum.SwerveType; import frc.robot.util.RBSIEnum.VisionType; @@ -73,6 +74,8 @@ public final class Constants { private static AutoType autoType = AutoType.PATHPLANNER; // PATHPLANNER, CHOREO private static VisionType visionType = VisionType.NONE; // PHOTON, LIMELIGHT, NONE + private static PracticeSpace practiceSpace = PracticeSpace.NUTHOUSE; // NUTHOUSE, FIELD + /** Checks whether the correct robot is selected when deploying. */ public static void main(String... args) { if (robotType == RobotType.SIMBOT) { @@ -125,7 +128,11 @@ public static final class DrivebaseConstants { // Theoretical free speed (m/s) at 12v applied output; // IMPORTANT: Follow the AdvantageKit instructions for measuring the ACTUAL maximum linear speed // of YOUR ROBOT, and replace the estimate here with your measured value! - public static final double kMaxLinearSpeed = Units.feetToMeters(18); + public static final double kMaxLinearSpeed = + switch (practiceSpace) { + case NUTHOUSE -> Units.feetToMeters(1); + case FIELD -> Units.feetToMeters(18); + }; // Set 3/4 of a rotation per second as the max angular velocity (radians/sec) public static final double kMaxAngularSpeed = 1.5 * Math.PI; diff --git a/src/main/java/frc/robot/commands/OnTheFly.java b/src/main/java/frc/robot/commands/OnTheFly.java index 2fdcbb5..1a9b015 100644 --- a/src/main/java/frc/robot/commands/OnTheFly.java +++ b/src/main/java/frc/robot/commands/OnTheFly.java @@ -2,7 +2,4 @@ import edu.wpi.first.wpilibj2.command.Command; -public class OnTheFly extends Command { - - -} +public class OnTheFly extends Command {} diff --git a/src/main/java/frc/robot/generated/TunerConstants.java b/src/main/java/frc/robot/generated/TunerConstants.java index 4c4239a..950de1d 100755 --- a/src/main/java/frc/robot/generated/TunerConstants.java +++ b/src/main/java/frc/robot/generated/TunerConstants.java @@ -138,7 +138,7 @@ public class TunerConstants { private static final int kFrontLeftDriveMotorId = 1; private static final int kFrontLeftSteerMotorId = 2; private static final int kFrontLeftEncoderId = 3; - private static final Angle kFrontLeftEncoderOffset = Rotations.of(0.087158203125); + private static final Angle kFrontLeftEncoderOffset = Rotations.of(0.088134765625); private static final boolean kFrontLeftSteerMotorInverted = true; private static final boolean kFrontLeftEncoderInverted = false; @@ -149,7 +149,7 @@ public class TunerConstants { private static final int kFrontRightDriveMotorId = 4; private static final int kFrontRightSteerMotorId = 5; private static final int kFrontRightEncoderId = 6; - private static final Angle kFrontRightEncoderOffset = Rotations.of(0.410888671875); + private static final Angle kFrontRightEncoderOffset = Rotations.of(0.23779296875); private static final boolean kFrontRightSteerMotorInverted = true; private static final boolean kFrontRightEncoderInverted = false; @@ -160,7 +160,7 @@ public class TunerConstants { private static final int kBackLeftDriveMotorId = 7; private static final int kBackLeftSteerMotorId = 8; private static final int kBackLeftEncoderId = 9; - private static final Angle kBackLeftEncoderOffset = Rotations.of(-0.1142578125); + private static final Angle kBackLeftEncoderOffset = Rotations.of(-0.114990234375); private static final boolean kBackLeftSteerMotorInverted = true; private static final boolean kBackLeftEncoderInverted = false; @@ -171,7 +171,7 @@ public class TunerConstants { private static final int kBackRightDriveMotorId = 10; private static final int kBackRightSteerMotorId = 11; private static final int kBackRightEncoderId = 12; - private static final Angle kBackRightEncoderOffset = Rotations.of(-0.471923828125); + private static final Angle kBackRightEncoderOffset = Rotations.of(-0.47265625); private static final boolean kBackRightSteerMotorInverted = true; private static final boolean kBackRightEncoderInverted = false; @@ -231,10 +231,10 @@ public class TunerConstants { kBackRightSteerMotorInverted, kBackRightEncoderInverted); - // /** - // * Creates a CommandSwerveDrivetrain instance. - // * This should only be called once in your robot program,. - // */ + /** + * Creates a CommandSwerveDrivetrain instance. This should only be called once in your robot + * program,. + */ // public static CommandSwerveDrivetrain createDrivetrain() { // return new CommandSwerveDrivetrain( // DrivetrainConstants, FrontLeft, FrontRight, BackLeft, BackRight diff --git a/src/main/java/frc/robot/subsystems/drive/Drive.java b/src/main/java/frc/robot/subsystems/drive/Drive.java index 46fbeea..c96c031 100644 --- a/src/main/java/frc/robot/subsystems/drive/Drive.java +++ b/src/main/java/frc/robot/subsystems/drive/Drive.java @@ -169,12 +169,11 @@ public Drive() { this::getChassisSpeeds, this::runVelocity, new PPHolonomicDriveController( - // Rotate - //5, 0, 0 - new PIDConstants(4, 0.0, 0.0), // Drive - //15, 0, 0 - new PIDConstants(5, 0.0, 0)), + // 5, 0, 0 + new PIDConstants(3, 0.0, 0), + // Rotate + new PIDConstants(2.5, 0.0, 0.1)), PP_CONFIG, () -> DriverStation.getAlliance().orElse(Alliance.Blue) == Alliance.Red, this); diff --git a/src/main/java/frc/robot/util/RBSIEnum.java b/src/main/java/frc/robot/util/RBSIEnum.java index ddb31d8..adaf066 100644 --- a/src/main/java/frc/robot/util/RBSIEnum.java +++ b/src/main/java/frc/robot/util/RBSIEnum.java @@ -66,4 +66,9 @@ public static enum MotorIdleMode { COAST, // Allow the motor to coast when idle BRAKE // Hold motor position when idle } + + public static enum PracticeSpace { + NUTHOUSE, + FIELD + } } From a8c7140e734c0b54310c4993d7cb5234dda957e9 Mon Sep 17 00:00:00 2001 From: "dependabot[bot]" <49699333+dependabot[bot]@users.noreply.github.com> Date: Mon, 20 Jan 2025 17:08:32 +0000 Subject: [PATCH 05/15] Bump com.diffplug.spotless from 6.25.0 to 7.0.2 Bumps com.diffplug.spotless from 6.25.0 to 7.0.2. --- updated-dependencies: - dependency-name: com.diffplug.spotless dependency-type: direct:production update-type: version-update:semver-major ... Signed-off-by: dependabot[bot] --- build.gradle | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/build.gradle b/build.gradle index f1e53c3..968adf0 100644 --- a/build.gradle +++ b/build.gradle @@ -2,7 +2,7 @@ plugins { id "java" id "edu.wpi.first.GradleRIO" version "2025.2.1" id "com.peterabeles.gversion" version "1.10.3" - id "com.diffplug.spotless" version "6.25.0" + id "com.diffplug.spotless" version "7.0.2" id "io.freefair.lombok" version "8.11" id "com.google.protobuf" version "0.9.4" } From 1834e4607d3103efc92194a72a4fc5b81dd9b8a4 Mon Sep 17 00:00:00 2001 From: AquaShock211 <143662909+AquaShock211@users.noreply.github.com> Date: Mon, 20 Jan 2025 12:07:05 -0700 Subject: [PATCH 06/15] Made setNewPose to set the Pose to a specific point on the field --- src/main/java/frc/robot/RobotContainer.java | 4 ++++ src/main/java/frc/robot/subsystems/drive/Drive.java | 6 +++++- 2 files changed, 9 insertions(+), 1 deletion(-) diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 90005bf..6b9c11f 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -287,6 +287,10 @@ private void configureBindings() { m_drivebase) .ignoringDisable(true)); + driverController + .leftBumper() + .onTrue(Commands.runOnce(() -> new Pose2d(10.0, 10.0, new Rotation2d()))); + // Press RIGHT BUMPER --> Run the example flywheel driverController .rightBumper() diff --git a/src/main/java/frc/robot/subsystems/drive/Drive.java b/src/main/java/frc/robot/subsystems/drive/Drive.java index c96c031..cb02f11 100644 --- a/src/main/java/frc/robot/subsystems/drive/Drive.java +++ b/src/main/java/frc/robot/subsystems/drive/Drive.java @@ -170,7 +170,6 @@ public Drive() { this::runVelocity, new PPHolonomicDriveController( // Drive - // 5, 0, 0 new PIDConstants(3, 0.0, 0), // Rotate new PIDConstants(2.5, 0.0, 0.1)), @@ -403,6 +402,11 @@ public void setPose(Pose2d pose) { m_PoseEstimator.resetPosition(rawGyroRotation, getModulePositions(), pose); } + /** Sets the current odometry pose to a specific spot. */ + public void setNewPose(Pose2d pose) { + new Pose2d(10.0, 10.0, new Rotation2d()); + } + /** Adds a new timestamped vision measurement. */ public void addVisionMeasurement( Pose2d visionRobotPoseMeters, From 2473baee6946835e078c1f2ab7abcc09b9e4fb5b Mon Sep 17 00:00:00 2001 From: tbowers7 Date: Mon, 20 Jan 2025 21:48:03 +0000 Subject: [PATCH 07/15] Update to the latest version of Az-RBSI Signed-off-by: tbowers7 --- build.gradle | 6 +- src/main/deploy/pathplanner/autos/Test.auto | 19 ++ .../pathplanner/paths/Consistancy Test.path | 54 ++++ src/main/java/frc/robot/Constants.java | 36 ++- src/main/java/frc/robot/Robot.java | 20 +- src/main/java/frc/robot/RobotContainer.java | 15 +- .../robot/commands/ChoreoAutoController.java | 6 +- .../frc/robot/commands/DriveCommands.java | 7 +- .../frc/robot/generated/TunerConstants.java | 8 +- .../frc/robot/subsystems/drive/Drive.java | 230 +++++++----------- .../frc/robot/subsystems/drive/GyroIO.java | 2 - .../robot/subsystems/drive/GyroIONavX.java | 38 +-- .../robot/subsystems/drive/GyroIOPigeon2.java | 39 ++- .../subsystems/drive/ModuleIOBlended.java | 8 +- .../robot/subsystems/drive/ModuleIOSpark.java | 14 +- .../drive/ModuleIOSparkCANcoder.java | 14 +- .../subsystems/drive/SwerveConstants.java | 6 +- .../vision/VisionIOPhotonVision.java | 3 + src/main/java/frc/robot/util/RBSIParsing.java | 77 ++++++ vendordeps/AdvantageKit.json | 11 +- vendordeps/Phoenix6-frc2025-latest.json | 82 +++++-- vendordeps/REVLib.json | 13 +- vendordeps/ReduxLib-2025.0.1.json | 72 ++++++ vendordeps/Studica-2025.0.1.json | 71 ++++++ vendordeps/ThriftyLib.json | 4 +- vendordeps/maple-sim.json | 4 +- vendordeps/photonlib.json | 12 +- vendordeps/yagsl-2025.2.2.json | 64 +++++ 28 files changed, 637 insertions(+), 298 deletions(-) create mode 100644 src/main/deploy/pathplanner/autos/Test.auto create mode 100644 src/main/deploy/pathplanner/paths/Consistancy Test.path create mode 100644 src/main/java/frc/robot/util/RBSIParsing.java create mode 100644 vendordeps/ReduxLib-2025.0.1.json create mode 100644 vendordeps/Studica-2025.0.1.json create mode 100644 vendordeps/yagsl-2025.2.2.json diff --git a/build.gradle b/build.gradle index 968adf0..1dfb32a 100644 --- a/build.gradle +++ b/build.gradle @@ -214,7 +214,7 @@ spotless { exclude '**/build/**', '**/build-*/**' } greclipse() - indentWithSpaces(4) + leadingTabsToSpaces(4) trimTrailingWhitespace() endWithNewline() } @@ -232,7 +232,7 @@ spotless { } eclipseWtp('xml') trimTrailingWhitespace() - indentWithSpaces(2) + leadingTabsToSpaces(2) endWithNewline() } format "misc", { @@ -241,7 +241,7 @@ spotless { exclude '**/build/**', '**/build-*/**' } trimTrailingWhitespace() - indentWithSpaces(2) + leadingTabsToSpaces(2) endWithNewline() } } diff --git a/src/main/deploy/pathplanner/autos/Test.auto b/src/main/deploy/pathplanner/autos/Test.auto new file mode 100644 index 0000000..832e9de --- /dev/null +++ b/src/main/deploy/pathplanner/autos/Test.auto @@ -0,0 +1,19 @@ +{ + "version": "2025.0", + "command": { + "type": "sequential", + "data": { + "commands": [ + { + "type": "path", + "data": { + "pathName": "Consistancy Test" + } + } + ] + } + }, + "resetOdom": true, + "folder": null, + "choreoAuto": false +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/Consistancy Test.path b/src/main/deploy/pathplanner/paths/Consistancy Test.path new file mode 100644 index 0000000..cc91670 --- /dev/null +++ b/src/main/deploy/pathplanner/paths/Consistancy Test.path @@ -0,0 +1,54 @@ +{ + "version": "2025.0", + "waypoints": [ + { + "anchor": { + "x": 9.35, + "y": 6.18 + }, + "prevControl": null, + "nextControl": { + "x": 9.141987426415538, + "y": 6.318675049056306 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 8.18, + "y": 6.18 + }, + "prevControl": { + "x": 8.56, + "y": 6.17 + }, + "nextControl": null, + "isLocked": false, + "linkedName": null + } + ], + "rotationTargets": [], + "constraintZones": [], + "pointTowardsZones": [], + "eventMarkers": [], + "globalConstraints": { + "maxVelocity": 0.5, + "maxAcceleration": 1.0, + "maxAngularVelocity": 540.0, + "maxAngularAcceleration": 720.0, + "nominalVoltage": 12.0, + "unlimited": false + }, + "goalEndState": { + "velocity": 0, + "rotation": 0.0 + }, + "reversed": false, + "folder": null, + "idealStartingState": { + "velocity": 0, + "rotation": 0.0 + }, + "useDefaultConstraints": false +} \ 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 6d6eeac..08fb4a5 100644 --- a/src/main/java/frc/robot/Constants.java +++ b/src/main/java/frc/robot/Constants.java @@ -19,17 +19,21 @@ import com.fasterxml.jackson.core.JsonProcessingException; import com.fasterxml.jackson.databind.ObjectMapper; +import com.pathplanner.lib.config.ModuleConfig; import com.pathplanner.lib.config.PIDConstants; +import com.pathplanner.lib.config.RobotConfig; import edu.wpi.first.apriltag.AprilTagFieldLayout; import edu.wpi.first.apriltag.AprilTagFields; import edu.wpi.first.math.geometry.Rotation2d; import edu.wpi.first.math.geometry.Rotation3d; import edu.wpi.first.math.geometry.Transform3d; import edu.wpi.first.math.geometry.Translation3d; +import edu.wpi.first.math.system.plant.DCMotor; import edu.wpi.first.math.util.Units; import edu.wpi.first.wpilibj.Filesystem; import edu.wpi.first.wpilibj.RobotBase; import frc.robot.Constants.AprilTagConstants.AprilTagLayoutType; +import frc.robot.subsystems.drive.Drive; import frc.robot.subsystems.drive.SwerveConstants; import frc.robot.util.Alert; import frc.robot.util.Alert.AlertType; @@ -135,10 +139,6 @@ public static final class DrivebaseConstants { public static final double kMaxLinearAccel = 4.0; // m/s/s public static final double kMaxAngularAccel = Units.degreesToRadians(720); - // Drive and Turn PID constants - public static final PIDConstants drivePID = new PIDConstants(0.05, 0.0, 0.0); - public static final PIDConstants steerPID = new PIDConstants(2.0, 0.0, 0.4); - // Hold time on motor brakes when disabled public static final double kWheelLockTime = 10; // seconds @@ -235,10 +235,30 @@ public static class OperatorConstants { /** Autonomous Action Constants ****************************************** */ public static final class AutoConstants { - // PathPlanner Translation PID constants - public static final PIDConstants kAutoDrivePID = new PIDConstants(0.7, 0, 0); - // PathPlanner Rotation PID constants - public static final PIDConstants kAutoSteerPID = new PIDConstants(0.4, 0, 0.01); + // Drive and Turn PID constants used for PathPlanner + public static final PIDConstants kPPdrivePID = new PIDConstants(5.0, 0.0, 0.0); + public static final PIDConstants kPPsteerPID = new PIDConstants(5.0, 0.0, 0.0); + + // PathPlanner Config constants + public static final RobotConfig kPathPlannerConfig = + new RobotConfig( + PhysicalConstants.kRobotMassKg, + PhysicalConstants.kRobotMOI, + new ModuleConfig( + SwerveConstants.kWheelRadiusMeters, + DrivebaseConstants.kMaxLinearSpeed, + PhysicalConstants.kWheelCOF, + DCMotor.getKrakenX60Foc(1).withReduction(SwerveConstants.kDriveGearRatio), + SwerveConstants.kDriveSlipCurrent, + 1), + Drive.getModuleTranslations()); + + // Alternatively, we can build this from the PathPlanner GUI: + // public static final RobotConfig kPathPlannerConfig = RobotConfig.fromGUISettings(); + + // Drive and Turn PID constants used for Chorep + public static final PIDConstants kChoreoDrivePID = new PIDConstants(10.0, 0.0, 0.0); + public static final PIDConstants kChoreoSteerPID = new PIDConstants(7.5, 0.0, 0.0); } /** Vision Constants (Assuming PhotonVision) ***************************** */ diff --git a/src/main/java/frc/robot/Robot.java b/src/main/java/frc/robot/Robot.java index f5bf2c2..e800a3c 100644 --- a/src/main/java/frc/robot/Robot.java +++ b/src/main/java/frc/robot/Robot.java @@ -40,8 +40,7 @@ public class Robot extends LoggedRobot { * This function is run when the robot is first started up and should be used for any * initialization code. */ - @Override - public void robotInit() { + public Robot() { // Record metadata Logger.recordMetadata("Robot", Constants.getRobot().toString()); Logger.recordMetadata("TuningMode", Boolean.toString(Constants.tuningMode)); @@ -85,18 +84,14 @@ public void robotInit() { break; } - // See http://bit.ly/3YIzFZ6 for more information on timestamps in AdvantageKit. - // Logger.disableDeterministicTimestamps() - // Start AdvantageKit logger Logger.start(); - // Instantiate our RobotContainer. This will perform all our button bindings, - // and put our autonomous chooser on the dashboard. + // Instantiate our RobotContainer. This will perform all our button bindings, and put our + // autonomous chooser on the dashboard. m_robotContainer = new RobotContainer(); - // Create a timer to disable motor brake a few seconds after disable. This will - // let the robot + // Create a timer to disable motor brake a few seconds after disable. This will let the robot // stop immediately when disabled, but then also let it be pushed more m_disabledTimer = new Timer(); } @@ -144,8 +139,11 @@ public void disabledPeriodic() { @Override public void autonomousInit() { - // TODO: Make sure Gyro inits here with whatever is in the path planning thingie + // Just in case, cancel all running commands + CommandScheduler.getInstance().cancelAll(); m_robotContainer.setMotorBrake(true); + + // TODO: Make sure Gyro inits here with whatever is in the path planning thingie switch (Constants.getAutoType()) { case PATHPLANNER: m_autoCommandPathPlanner = m_robotContainer.getAutonomousCommandPathPlanner(); @@ -179,7 +177,6 @@ public void teleopInit() { } else { CommandScheduler.getInstance().cancelAll(); } - m_robotContainer.setDriveMode(); m_robotContainer.setMotorBrake(true); } @@ -192,7 +189,6 @@ public void teleopPeriodic() {} public void testInit() { // Cancels all running commands at the start of test mode. CommandScheduler.getInstance().cancelAll(); - m_robotContainer.setDriveMode(); } /** This function is called periodically during test mode. */ diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 7cd9608..beb2eb1 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -26,7 +26,6 @@ import choreo.auto.AutoRoutine; import choreo.auto.AutoTrajectory; import com.pathplanner.lib.auto.AutoBuilder; -import com.pathplanner.lib.auto.NamedCommands; import edu.wpi.first.math.geometry.Pose2d; import edu.wpi.first.math.geometry.Rotation2d; import edu.wpi.first.wpilibj.GenericHID; @@ -164,6 +163,7 @@ public RobotContainer() { autoChooserChoreo = null; autoFactoryChoreo = null; break; + case CHOREO: autoFactoryChoreo = new AutoFactory( @@ -179,24 +179,25 @@ public RobotContainer() { // Set the others to null autoChooserPathPlanner = null; break; + default: // Then, throw the error throw new RuntimeException( "Incorrect AUTO type selected in Constants: " + Constants.getAutoType()); } - // Configure the trigger bindings - configureBindings(); // Define Auto commands defineAutoCommands(); // Define SysIs Routines definesysIdRoutines(); + // Configure the button and trigger bindings + configureBindings(); } /** Use this method to define your Autonomous commands for use with PathPlanner / Choreo */ private void defineAutoCommands() { - NamedCommands.registerCommand("Zero", Commands.runOnce(() -> m_drivebase.zero())); + // NamedCommands.registerCommand("Zero", Commands.runOnce(() -> m_drivebase.zero())); } /** @@ -257,7 +258,7 @@ private void configureBindings() { .onTrue( Commands.runOnce( () -> - m_drivebase.setPose( + m_drivebase.resetPose( new Pose2d(m_drivebase.getPose().getTranslation(), new Rotation2d())), m_drivebase) .ignoringDisable(true)); @@ -295,10 +296,6 @@ public void getAutonomousCommandChoreo() { RobotModeTriggers.autonomous().whileTrue(autoChooserChoreo.selectedCommandScheduler()); } - public void setDriveMode() { - configureBindings(); - } - /** Set the motor neutral mode to BRAKE / COAST for T/F */ public void setMotorBrake(boolean brake) { m_drivebase.setMotorBrake(brake); diff --git a/src/main/java/frc/robot/commands/ChoreoAutoController.java b/src/main/java/frc/robot/commands/ChoreoAutoController.java index 852cd6b..ebe9c71 100644 --- a/src/main/java/frc/robot/commands/ChoreoAutoController.java +++ b/src/main/java/frc/robot/commands/ChoreoAutoController.java @@ -27,11 +27,11 @@ public class ChoreoAutoController implements Consumer { private final Drive drive; // drive subsystem private final PIDController xController = - new PIDController(kAutoDrivePID.kP, 0.0, kAutoDrivePID.kD); + new PIDController(kChoreoDrivePID.kP, kChoreoDrivePID.kI, kChoreoDrivePID.kD); private final PIDController yController = - new PIDController(kAutoDrivePID.kP, 0.0, kAutoDrivePID.kD); + new PIDController(kChoreoDrivePID.kP, kChoreoDrivePID.kI, kChoreoDrivePID.kD); private final PIDController headingController = - new PIDController(kAutoSteerPID.kP, 0.0, kAutoSteerPID.kD); + new PIDController(kChoreoSteerPID.kP, kChoreoSteerPID.kI, kChoreoSteerPID.kD); public ChoreoAutoController(Drive drive) { this.drive = drive; diff --git a/src/main/java/frc/robot/commands/DriveCommands.java b/src/main/java/frc/robot/commands/DriveCommands.java index 7909b91..5421e24 100644 --- a/src/main/java/frc/robot/commands/DriveCommands.java +++ b/src/main/java/frc/robot/commands/DriveCommands.java @@ -30,6 +30,7 @@ import edu.wpi.first.wpilibj.Timer; import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.Commands; +import frc.robot.Constants.AutoConstants; import frc.robot.Constants.DrivebaseConstants; import frc.robot.Constants.OperatorConstants; import frc.robot.subsystems.drive.Drive; @@ -125,9 +126,9 @@ public static Command fieldRelativeDriveAtAngle( // Create PID controller ProfiledPIDController angleController = new ProfiledPIDController( - DrivebaseConstants.steerPID.kP, - DrivebaseConstants.steerPID.kI, - DrivebaseConstants.steerPID.kD, + AutoConstants.kPPsteerPID.kP, + AutoConstants.kPPsteerPID.kI, + AutoConstants.kPPsteerPID.kD, new TrapezoidProfile.Constraints( DrivebaseConstants.kMaxAngularSpeed, DrivebaseConstants.kMaxAngularAccel)); angleController.enableContinuousInput(-Math.PI, Math.PI); diff --git a/src/main/java/frc/robot/generated/TunerConstants.java b/src/main/java/frc/robot/generated/TunerConstants.java index 4c4239a..929773a 100755 --- a/src/main/java/frc/robot/generated/TunerConstants.java +++ b/src/main/java/frc/robot/generated/TunerConstants.java @@ -138,7 +138,7 @@ public class TunerConstants { private static final int kFrontLeftDriveMotorId = 1; private static final int kFrontLeftSteerMotorId = 2; private static final int kFrontLeftEncoderId = 3; - private static final Angle kFrontLeftEncoderOffset = Rotations.of(0.087158203125); + private static final Angle kFrontLeftEncoderOffset = Rotations.of(0.088134765625); private static final boolean kFrontLeftSteerMotorInverted = true; private static final boolean kFrontLeftEncoderInverted = false; @@ -149,7 +149,7 @@ public class TunerConstants { private static final int kFrontRightDriveMotorId = 4; private static final int kFrontRightSteerMotorId = 5; private static final int kFrontRightEncoderId = 6; - private static final Angle kFrontRightEncoderOffset = Rotations.of(0.410888671875); + private static final Angle kFrontRightEncoderOffset = Rotations.of(0.23779296875); private static final boolean kFrontRightSteerMotorInverted = true; private static final boolean kFrontRightEncoderInverted = false; @@ -160,7 +160,7 @@ public class TunerConstants { private static final int kBackLeftDriveMotorId = 7; private static final int kBackLeftSteerMotorId = 8; private static final int kBackLeftEncoderId = 9; - private static final Angle kBackLeftEncoderOffset = Rotations.of(-0.1142578125); + private static final Angle kBackLeftEncoderOffset = Rotations.of(-0.114990234375); private static final boolean kBackLeftSteerMotorInverted = true; private static final boolean kBackLeftEncoderInverted = false; @@ -171,7 +171,7 @@ public class TunerConstants { private static final int kBackRightDriveMotorId = 10; private static final int kBackRightSteerMotorId = 11; private static final int kBackRightEncoderId = 12; - private static final Angle kBackRightEncoderOffset = Rotations.of(-0.471923828125); + private static final Angle kBackRightEncoderOffset = Rotations.of(-0.47265625); private static final boolean kBackRightSteerMotorInverted = true; private static final boolean kBackRightEncoderInverted = false; diff --git a/src/main/java/frc/robot/subsystems/drive/Drive.java b/src/main/java/frc/robot/subsystems/drive/Drive.java index 7d468d9..63664b6 100644 --- a/src/main/java/frc/robot/subsystems/drive/Drive.java +++ b/src/main/java/frc/robot/subsystems/drive/Drive.java @@ -21,9 +21,6 @@ import choreo.trajectory.SwerveSample; import com.ctre.phoenix6.swerve.SwerveRequest; import com.pathplanner.lib.auto.AutoBuilder; -import com.pathplanner.lib.config.ModuleConfig; -import com.pathplanner.lib.config.PIDConstants; -import com.pathplanner.lib.config.RobotConfig; import com.pathplanner.lib.controllers.PPHolonomicDriveController; import com.pathplanner.lib.pathfinding.Pathfinding; import com.pathplanner.lib.util.PathPlannerLogging; @@ -43,7 +40,6 @@ import edu.wpi.first.math.kinematics.SwerveModuleState; import edu.wpi.first.math.numbers.N1; import edu.wpi.first.math.numbers.N3; -import edu.wpi.first.math.system.plant.DCMotor; import edu.wpi.first.wpilibj.Alert; import edu.wpi.first.wpilibj.Alert.AlertType; import edu.wpi.first.wpilibj.DriverStation; @@ -52,10 +48,11 @@ import edu.wpi.first.wpilibj2.command.SubsystemBase; import edu.wpi.first.wpilibj2.command.sysid.SysIdRoutine; import frc.robot.Constants; +import frc.robot.Constants.AutoConstants; import frc.robot.Constants.DrivebaseConstants; -import frc.robot.Constants.PhysicalConstants; import frc.robot.util.LocalADStarAK; import frc.robot.util.RBSIEnum.Mode; +import frc.robot.util.RBSIParsing; import java.util.concurrent.locks.Lock; import java.util.concurrent.locks.ReentrantLock; import org.littletonrobotics.junction.AutoLogOutput; @@ -63,20 +60,6 @@ public class Drive extends SubsystemBase { - // PathPlanner config constants - private static final RobotConfig PP_CONFIG = - new RobotConfig( - PhysicalConstants.kRobotMassKg, - PhysicalConstants.kRobotMOI, - new ModuleConfig( - kWheelRadiusMeters, - DrivebaseConstants.kMaxLinearSpeed, - PhysicalConstants.kWheelCOF, - DCMotor.getKrakenX60Foc(1).withReduction(kDriveGearRatio), - kDriveSlipCurrent, - 1), - getModuleTranslations()); - static final Lock odometryLock = new ReentrantLock(); private final GyroIO gyroIO; private final GyroIOInputsAutoLogged gyroInputs = new GyroIOInputsAutoLogged(); @@ -97,11 +80,9 @@ public class Drive extends SubsystemBase { private SwerveDrivePoseEstimator m_PoseEstimator = new SwerveDrivePoseEstimator(kinematics, rawGyroRotation, lastModulePositions, new Pose2d()); - // Choreo drive controller - private final PIDController xController = new PIDController(10.0, 0.0, 0.0); - // Constructor public Drive() { + switch (Constants.getSwerveType()) { case PHOENIX6: // This one is easy because it's all CTRE all the time @@ -125,7 +106,7 @@ public Drive() { throw new RuntimeException("Invalid IMU type"); } // Then parse the module(s) - Byte modType = parseModuleType(); + Byte modType = RBSIParsing.parseModuleType(); for (int i = 0; i < 4; i++) { switch (modType) { case 0b00000000: // ALL-CTRE @@ -162,17 +143,21 @@ public Drive() { // Configure Autonomous Path Building for PathPlanner based on `AutoType` switch (Constants.getAutoType()) { case PATHPLANNER: - // Configure AutoBuilder for PathPlanner - AutoBuilder.configure( - this::getPose, - this::setPose, - this::getChassisSpeeds, - this::runVelocity, - new PPHolonomicDriveController( - new PIDConstants(5.0, 0.0, 0.0), new PIDConstants(5.0, 0.0, 0.0)), - PP_CONFIG, - () -> DriverStation.getAlliance().orElse(Alliance.Blue) == Alliance.Red, - this); + try { + // Configure AutoBuilder for PathPlanner + AutoBuilder.configure( + this::getPose, + this::resetPose, + this::getChassisSpeeds, + (speeds, feedforwards) -> runVelocity(speeds), + new PPHolonomicDriveController(AutoConstants.kPPdrivePID, AutoConstants.kPPsteerPID), + AutoConstants.kPathPlannerConfig, + () -> DriverStation.getAlliance().orElse(Alliance.Blue) == Alliance.Red, + this); + } catch (Exception e) { + DriverStation.reportError( + "Failed to load PathPlanner config and configure AutoBuilder", e.getStackTrace()); + } Pathfinding.setPathfinder(new LocalADStarAK()); PathPlannerLogging.setLogActivePathCallback( (activePath) -> { @@ -203,6 +188,7 @@ public Drive() { (voltage) -> runCharacterization(voltage.in(Volts)), null, this)); } + /** Periodic function that is called each robot cycle by the command scheduler */ @Override public void periodic() { odometryLock.lock(); // Prevents odometry updates while reading data @@ -261,6 +247,8 @@ public void periodic() { gyroDisconnectedAlert.set(!gyroInputs.connected && Constants.getMode() != Mode.SIM); } + /** Drive Base Action Functions ****************************************** */ + /** * Sets the swerve drive motors to brake/coast mode. * @@ -274,6 +262,24 @@ public void setMotorBrake(boolean brake) { } } + /** Stops the drive. */ + public void stop() { + runVelocity(new ChassisSpeeds()); + } + + /** + * Stops the drive and turns the modules to an X arrangement to resist movement. The modules will + * return to their normal orientations the next time a nonzero velocity is requested. + */ + public void stopWithX() { + Rotation2d[] headings = new Rotation2d[4]; + for (int i = 0; i < 4; i++) { + headings[i] = getModuleTranslations()[i].getAngle(); + } + kinematics.resetHeadings(headings); + stop(); + } + /** * Runs the drive at the desired velocity. * @@ -305,28 +311,7 @@ public void runCharacterization(double output) { } } - /** Re-zero the gyro at the present heading */ - public void zero() { - gyroIO.zero(); - } - - /** Stops the drive. */ - public void stop() { - runVelocity(new ChassisSpeeds()); - } - - /** - * Stops the drive and turns the modules to an X arrangement to resist movement. The modules will - * return to their normal orientations the next time a nonzero velocity is requested. - */ - public void stopWithX() { - Rotation2d[] headings = new Rotation2d[4]; - for (int i = 0; i < 4; i++) { - headings[i] = getModuleTranslations()[i].getAngle(); - } - kinematics.resetHeadings(headings); - stop(); - } + /** SysId Characterization Routines ************************************** */ /** Returns a command to run a quasistatic test in the specified direction. */ public Command sysIdQuasistatic(SysIdRoutine.Direction direction) { @@ -340,6 +325,8 @@ public Command sysIdDynamic(SysIdRoutine.Direction direction) { return run(() -> runCharacterization(0.0)).withTimeout(1.0).andThen(sysId.dynamic(direction)); } + /** Getter Functions ***************************************************** */ + /** Returns the module states (turn angles and drive velocities) for all of the modules. */ @AutoLogOutput(key = "SwerveStates/Measured") private SwerveModuleState[] getModuleStates() { @@ -365,6 +352,27 @@ private ChassisSpeeds getChassisSpeeds() { return kinematics.toChassisSpeeds(getModuleStates()); } + /** Returns the current odometry pose. */ + @AutoLogOutput(key = "Odometry/Robot") + public Pose2d getPose() { + return m_PoseEstimator.getEstimatedPosition(); + } + + /** Returns the current odometry rotation. */ + public Rotation2d getRotation() { + return getPose().getRotation(); + } + + /** Returns an array of module translations. */ + public static Translation2d[] getModuleTranslations() { + return new Translation2d[] { + new Translation2d(kFLXPosMeters, kFLYPosMeters), + new Translation2d(kFRXPosMeters, kFRYPosMeters), + new Translation2d(kBLXPosMeters, kBLYPosMeters), + new Translation2d(kBRXPosMeters, kBRYPosMeters) + }; + } + /** Returns the position of each module in radians. */ public double[] getWheelRadiusCharacterizationPositions() { double[] values = new double[4]; @@ -383,19 +391,24 @@ public double getFFCharacterizationVelocity() { return output; } - /** Returns the current odometry pose. */ - @AutoLogOutput(key = "Odometry/Robot") - public Pose2d getPose() { - return m_PoseEstimator.getEstimatedPosition(); + /** Returns the maximum linear speed in meters per sec. */ + public double getMaxLinearSpeedMetersPerSec() { + return DrivebaseConstants.kMaxLinearSpeed; } - /** Returns the current odometry rotation. */ - public Rotation2d getRotation() { - return getPose().getRotation(); + /** Returns the maximum angular speed in radians per sec. */ + public double getMaxAngularSpeedRadPerSec() { + return getMaxLinearSpeedMetersPerSec() / kDriveBaseRadiusMeters; } + public Object getGyro() { + return gyroIO.getGyro(); + } + + /* Setter Functions ****************************************************** */ + /** Resets the current odometry pose. */ - public void setPose(Pose2d pose) { + public void resetPose(Pose2d pose) { m_PoseEstimator.resetPosition(rawGyroRotation, getModulePositions(), pose); } @@ -408,30 +421,8 @@ public void addVisionMeasurement( visionRobotPoseMeters, timestampSeconds, visionMeasurementStdDevs); } - /** Returns the maximum linear speed in meters per sec. */ - public double getMaxLinearSpeedMetersPerSec() { - return DrivebaseConstants.kMaxLinearSpeed; - } - - /** Returns the maximum angular speed in radians per sec. */ - public double getMaxAngularSpeedRadPerSec() { - return getMaxLinearSpeedMetersPerSec() / kDriveBaseRadiusMeters; - } - - /** Returns an array of module translations. */ - public static Translation2d[] getModuleTranslations() { - return new Translation2d[] { - new Translation2d(kFLXPosMeters, kFLYPosMeters), - new Translation2d(kFRXPosMeters, kFRYPosMeters), - new Translation2d(kBLXPosMeters, kBLYPosMeters), - new Translation2d(kBRXPosMeters, kBRYPosMeters) - }; - } - - public Object getGyro() { - return gyroIO.getGyro(); - } - + /** CHOREO SECTION (Ignore if AutoType == PATHPLANNER) ******************* */ + /** Choreo: Reset odometry */ public Command resetOdometry(Pose2d orElseGet) { // TODO Auto-generated method stub throw new UnsupportedOperationException("Unimplemented method 'resetOdometry'"); @@ -441,6 +432,7 @@ public Command resetOdometry(Pose2d orElseGet) { private final SwerveRequest.ApplyFieldSpeeds m_pathApplyFieldSpeeds = new SwerveRequest.ApplyFieldSpeeds(); + // Choreo Controller Values private final PIDController m_pathXController = new PIDController(10, 0, 0); private final PIDController m_pathYController = new PIDController(10, 0, 0); private final PIDController m_pathThetaController = new PIDController(7, 0, 0); @@ -474,66 +466,12 @@ public void followTrajectory(SwerveSample sample) { // Generate the next speeds for the robot ChassisSpeeds speeds = new ChassisSpeeds( - sample.vx + xController.calculate(pose.getX(), sample.x), - sample.vy + xController.calculate(pose.getX(), sample.y), - sample.omega + xController.calculate(pose.getRotation().getRadians(), sample.heading)); + sample.vx + m_pathXController.calculate(pose.getX(), sample.x), + sample.vy + m_pathXController.calculate(pose.getX(), sample.y), + sample.omega + + m_pathXController.calculate(pose.getRotation().getRadians(), sample.heading)); // Apply the generated speeds runVelocity(speeds); } - - /** - * Parse the module type given the type information for the FL module - * - * @return Byte The module type as bits in a byte. - */ - private Byte parseModuleType() { - // NOTE: This assumes all 4 modules have the same arrangement! - Byte b_drive; // [x,x,-,-,-,-,-,-] - Byte b_steer; // [-,-,x,x,-,-,-,-] - Byte b_encoder; // [-,-,-,-,x,x,-,-] - switch (kFLDriveType) { - case "falcon": - case "kraken": - case "talonfx": - // CTRE Drive Motor - b_drive = 0b00; - break; - case "sparkmax": - case "sparkflex": - // REV Drive Motor - b_drive = 0b01; - break; - default: - throw new RuntimeException("Invalid drive motor type"); - } - switch (kFLSteerType) { - case "falcon": - case "kraken": - case "talonfx": - // CTRE Drive Motor - b_steer = 0b00; - break; - case "sparkmax": - case "sparkflex": - // REV Drive Motor - b_steer = 0b01; - break; - default: - throw new RuntimeException("Invalid steer motor type"); - } - switch (kFLEncoderType) { - case "cancoder": - // CTRE CANcoder - b_encoder = 0b00; - break; - case "analog": - // Analog Encoder - b_encoder = 0b01; - break; - default: - throw new RuntimeException("Invalid swerve encoder type"); - } - return (byte) (0b00000000 | b_drive << 6 | b_steer << 4 | b_encoder << 2); - } } diff --git a/src/main/java/frc/robot/subsystems/drive/GyroIO.java b/src/main/java/frc/robot/subsystems/drive/GyroIO.java index c8c4d16..93cf0b1 100644 --- a/src/main/java/frc/robot/subsystems/drive/GyroIO.java +++ b/src/main/java/frc/robot/subsystems/drive/GyroIO.java @@ -30,7 +30,5 @@ public static class GyroIOInputs { public default void updateInputs(GyroIOInputs inputs) {} - public default void zero() {} - public abstract T getGyro(); } diff --git a/src/main/java/frc/robot/subsystems/drive/GyroIONavX.java b/src/main/java/frc/robot/subsystems/drive/GyroIONavX.java index 6de5b72..219ee81 100644 --- a/src/main/java/frc/robot/subsystems/drive/GyroIONavX.java +++ b/src/main/java/frc/robot/subsystems/drive/GyroIONavX.java @@ -66,23 +66,23 @@ public void updateInputs(GyroIOInputs inputs) { yawPositionQueue.clear(); } - /** - * Zero the NavX - * - *

This method should always rezero the pigeon in ALWAYS-BLUE-ORIGIN orientation. Testing, - * however, shows that it's not doing what I think it should be doing. There is likely - * interference with something else in the odometry - */ - @Override - public void zero() { - // With the Pigeon facing forward, forward depends on the alliance selected. - // Set Angle Adjustment based on alliance - if (DriverStation.getAlliance().get() == Alliance.Blue) { - navx.setAngleAdjustment(0.0); - } else { - navx.setAngleAdjustment(180.0); - } - System.out.println("Setting YAW to " + navx.getAngleAdjustment()); - navx.zeroYaw(); - } + // /** + // * Zero the NavX + // * + // *

This method should always rezero the pigeon in ALWAYS-BLUE-ORIGIN orientation. Testing, + // * however, shows that it's not doing what I think it should be doing. There is likely + // * interference with something else in the odometry + // */ + // @Override + // public void zero() { + // // With the Pigeon facing forward, forward depends on the alliance selected. + // // Set Angle Adjustment based on alliance + // if (DriverStation.getAlliance().get() == Alliance.Blue) { + // navx.setAngleAdjustment(0.0); + // } else { + // navx.setAngleAdjustment(180.0); + // } + // System.out.println("Setting YAW to " + navx.getAngleAdjustment()); + // navx.zeroYaw(); + // } } diff --git a/src/main/java/frc/robot/subsystems/drive/GyroIOPigeon2.java b/src/main/java/frc/robot/subsystems/drive/GyroIOPigeon2.java index a4ae3d3..fe771eb 100644 --- a/src/main/java/frc/robot/subsystems/drive/GyroIOPigeon2.java +++ b/src/main/java/frc/robot/subsystems/drive/GyroIOPigeon2.java @@ -26,8 +26,6 @@ import edu.wpi.first.math.util.Units; import edu.wpi.first.units.measure.Angle; import edu.wpi.first.units.measure.AngularVelocity; -import edu.wpi.first.wpilibj.DriverStation; -import edu.wpi.first.wpilibj.DriverStation.Alliance; import java.util.Queue; /** IO implementation for Pigeon2 */ @@ -60,6 +58,7 @@ public void updateInputs(GyroIOInputs inputs) { inputs.connected = BaseStatusSignal.refreshAll(yaw, yawVelocity).equals(StatusCode.OK); inputs.yawPosition = Rotation2d.fromDegrees(yaw.getValueAsDouble()); inputs.yawVelocityRadPerSec = Units.degreesToRadians(yawVelocity.getValueAsDouble()); + inputs.odometryYawTimestamps = yawTimestampQueue.stream().mapToDouble((Double value) -> value).toArray(); inputs.odometryYawPositions = @@ -70,22 +69,22 @@ public void updateInputs(GyroIOInputs inputs) { yawPositionQueue.clear(); } - /** - * Zero the Pigeon2 - * - *

This method should always rezero the pigeon in ALWAYS-BLUE-ORIGIN orientation. Testing, - * however, shows that it's not doing what I think it should be doing. There is likely - * interference with something else in the odometry - */ - @Override - public void zero() { - // With the Pigeon facing forward, forward depends on the alliance selected. - if (DriverStation.getAlliance().get() == Alliance.Blue) { - System.out.println("Alliance Blue: Setting YAW to 0"); - pigeon.setYaw(0.0); - } else { - System.out.println("Alliance Red: Setting YAW to 180"); - pigeon.setYaw(180.0); - } - } + // /** + // * Zero the Pigeon2 + // * + // *

This method should always rezero the pigeon in ALWAYS-BLUE-ORIGIN orientation. Testing, + // * however, shows that it's not doing what I think it should be doing. There is likely + // * interference with something else in the odometry + // */ + // @Override + // public void zero() { + // // With the Pigeon facing forward, forward depends on the alliance selected. + // if (DriverStation.getAlliance().get() == Alliance.Blue) { + // System.out.println("Alliance Blue: Setting YAW to 0"); + // pigeon.setYaw(0.0); + // } else { + // System.out.println("Alliance Red: Setting YAW to 180"); + // pigeon.setYaw(180.0); + // } + // } } diff --git a/src/main/java/frc/robot/subsystems/drive/ModuleIOBlended.java b/src/main/java/frc/robot/subsystems/drive/ModuleIOBlended.java index d7f6e9d..2ffe7fa 100644 --- a/src/main/java/frc/robot/subsystems/drive/ModuleIOBlended.java +++ b/src/main/java/frc/robot/subsystems/drive/ModuleIOBlended.java @@ -53,7 +53,7 @@ import edu.wpi.first.units.measure.Current; import edu.wpi.first.units.measure.Voltage; import frc.robot.Constants; -import frc.robot.Constants.DrivebaseConstants; +import frc.robot.Constants.AutoConstants; import frc.robot.util.PhoenixUtil; import frc.robot.util.SparkUtil; import java.util.Queue; @@ -205,9 +205,9 @@ public ModuleIOBlended(int module) { .positionWrappingEnabled(true) .positionWrappingInputRange(turnPIDMinInput, turnPIDMaxInput) .pidf( - DrivebaseConstants.steerPID.kP, - DrivebaseConstants.steerPID.kI, - DrivebaseConstants.steerPID.kD, + AutoConstants.kPPsteerPID.kP, + AutoConstants.kPPsteerPID.kI, + AutoConstants.kPPsteerPID.kD, 0.0); turnConfig .signals diff --git a/src/main/java/frc/robot/subsystems/drive/ModuleIOSpark.java b/src/main/java/frc/robot/subsystems/drive/ModuleIOSpark.java index 841ac55..83d9d4a 100644 --- a/src/main/java/frc/robot/subsystems/drive/ModuleIOSpark.java +++ b/src/main/java/frc/robot/subsystems/drive/ModuleIOSpark.java @@ -37,7 +37,7 @@ import edu.wpi.first.math.MathUtil; import edu.wpi.first.math.filter.Debouncer; import edu.wpi.first.math.geometry.Rotation2d; -import frc.robot.Constants.DrivebaseConstants; +import frc.robot.Constants.AutoConstants; import java.util.Queue; import java.util.function.DoubleSupplier; @@ -135,9 +135,9 @@ public ModuleIOSpark(int module) { .closedLoop .feedbackSensor(FeedbackSensor.kPrimaryEncoder) .pidf( - DrivebaseConstants.drivePID.kP, - DrivebaseConstants.drivePID.kI, - DrivebaseConstants.drivePID.kD, + AutoConstants.kPPdrivePID.kP, + AutoConstants.kPPdrivePID.kI, + AutoConstants.kPPdrivePID.kD, 0.0); driveConfig .signals @@ -175,9 +175,9 @@ public ModuleIOSpark(int module) { .positionWrappingEnabled(true) .positionWrappingInputRange(turnPIDMinInput, turnPIDMaxInput) .pidf( - DrivebaseConstants.steerPID.kP, - DrivebaseConstants.steerPID.kI, - DrivebaseConstants.steerPID.kD, + AutoConstants.kPPsteerPID.kP, + AutoConstants.kPPsteerPID.kI, + AutoConstants.kPPsteerPID.kD, 0.0); turnConfig .signals diff --git a/src/main/java/frc/robot/subsystems/drive/ModuleIOSparkCANcoder.java b/src/main/java/frc/robot/subsystems/drive/ModuleIOSparkCANcoder.java index ce7744c..f248e2b 100644 --- a/src/main/java/frc/robot/subsystems/drive/ModuleIOSparkCANcoder.java +++ b/src/main/java/frc/robot/subsystems/drive/ModuleIOSparkCANcoder.java @@ -42,7 +42,7 @@ import edu.wpi.first.math.util.Units; import edu.wpi.first.units.measure.Angle; import edu.wpi.first.units.measure.AngularVelocity; -import frc.robot.Constants.DrivebaseConstants; +import frc.robot.Constants.AutoConstants; import java.util.Queue; import java.util.function.DoubleSupplier; @@ -155,9 +155,9 @@ public ModuleIOSparkCANcoder(int module) { .closedLoop .feedbackSensor(FeedbackSensor.kPrimaryEncoder) .pidf( - DrivebaseConstants.drivePID.kP, - DrivebaseConstants.drivePID.kI, - DrivebaseConstants.drivePID.kD, + AutoConstants.kPPdrivePID.kP, + AutoConstants.kPPdrivePID.kI, + AutoConstants.kPPdrivePID.kD, 0.0); driveConfig .signals @@ -195,9 +195,9 @@ public ModuleIOSparkCANcoder(int module) { .positionWrappingEnabled(true) .positionWrappingInputRange(turnPIDMinInput, turnPIDMaxInput) .pidf( - DrivebaseConstants.steerPID.kP, - DrivebaseConstants.steerPID.kI, - DrivebaseConstants.steerPID.kD, + AutoConstants.kPPsteerPID.kP, + AutoConstants.kPPsteerPID.kI, + AutoConstants.kPPsteerPID.kD, 0.0); turnConfig .signals diff --git a/src/main/java/frc/robot/subsystems/drive/SwerveConstants.java b/src/main/java/frc/robot/subsystems/drive/SwerveConstants.java index d511927..cffffbf 100644 --- a/src/main/java/frc/robot/subsystems/drive/SwerveConstants.java +++ b/src/main/java/frc/robot/subsystems/drive/SwerveConstants.java @@ -33,6 +33,7 @@ public class SwerveConstants { public static final double kDriveGearRatio; public static final double kSteerGearRatio; public static final double kWheelRadiusInches; + public static final double kWheelRadiusMeters; public static final String kCANbusName; public static final int kPigeonId; public static final double kSteerInertia; @@ -112,7 +113,8 @@ public class SwerveConstants { kCoupleRatio = TunerConstants.FrontLeft.CouplingGearRatio; kDriveGearRatio = TunerConstants.FrontLeft.DriveMotorGearRatio; kSteerGearRatio = TunerConstants.FrontLeft.SteerMotorGearRatio; - kWheelRadiusInches = TunerConstants.FrontLeft.WheelRadius; + kWheelRadiusMeters = TunerConstants.FrontLeft.WheelRadius; + kWheelRadiusInches = Units.metersToInches(kWheelRadiusMeters); kCANbusName = TunerConstants.DrivetrainConstants.CANBusName; kPigeonId = TunerConstants.DrivetrainConstants.Pigeon2Id; kSteerInertia = TunerConstants.FrontLeft.SteerInertia; @@ -197,6 +199,7 @@ public class SwerveConstants { kDriveGearRatio = YagslConstants.kDriveGearRatio; kSteerGearRatio = YagslConstants.kSteerGearRatio; kWheelRadiusInches = YagslConstants.kWheelRadiusInches; + kWheelRadiusMeters = Units.inchesToMeters(kWheelRadiusInches); kCANbusName = YagslConstants.kCANbusName; kPigeonId = YagslConstants.kPigeonId; kSteerInertia = YagslConstants.kSteerInertia; @@ -286,7 +289,6 @@ public class SwerveConstants { Math.max( Math.hypot(kBLXPosMeters, kBLYPosMeters), Math.hypot(kBRXPosMeters, kBRYPosMeters))); public static final double kDriveBaseRadiusInches = Units.metersToInches(kDriveBaseRadiusMeters); - public static final double kWheelRadiusMeters = Units.inchesToMeters(kWheelRadiusInches); // Are we on the CANivore or not? public static final double kOdometryFrequency = diff --git a/src/main/java/frc/robot/subsystems/vision/VisionIOPhotonVision.java b/src/main/java/frc/robot/subsystems/vision/VisionIOPhotonVision.java index e61fc33..df70163 100644 --- a/src/main/java/frc/robot/subsystems/vision/VisionIOPhotonVision.java +++ b/src/main/java/frc/robot/subsystems/vision/VisionIOPhotonVision.java @@ -90,6 +90,7 @@ public void updateInputs(VisionIOInputs inputs) { } else if (!result.targets.isEmpty()) { // Single tag result var target = result.targets.get(0); + // Calculate robot pose var tagPose = aprilTagLayout.getTagPose(target.fiducialId); if (tagPose.isPresent()) { @@ -99,8 +100,10 @@ public void updateInputs(VisionIOInputs inputs) { Transform3d fieldToCamera = fieldToTarget.plus(cameraToTarget.inverse()); Transform3d fieldToRobot = fieldToCamera.plus(robotToCamera.inverse()); Pose3d robotPose = new Pose3d(fieldToRobot.getTranslation(), fieldToRobot.getRotation()); + // Add tag ID tagIds.add((short) target.fiducialId); + // Add observation poseObservations.add( new PoseObservation( diff --git a/src/main/java/frc/robot/util/RBSIParsing.java b/src/main/java/frc/robot/util/RBSIParsing.java new file mode 100644 index 0000000..d61c501 --- /dev/null +++ b/src/main/java/frc/robot/util/RBSIParsing.java @@ -0,0 +1,77 @@ +// Copyright (c) 2024-2025 Az-FIRST +// http://github.com/AZ-First +// +// This program is free software; you can redistribute it and/or +// modify it under the terms of the GNU General Public License +// version 3 as published by the Free Software Foundation or +// available in the root directory of this project. +// +// This program is distributed in the hope that it will be useful, +// but WITHOUT ANY WARRANTY; without even the implied warranty of +// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the +// GNU General Public License for more details. + +package frc.robot.util; + +import static frc.robot.subsystems.drive.SwerveConstants.kFLDriveType; +import static frc.robot.subsystems.drive.SwerveConstants.kFLEncoderType; +import static frc.robot.subsystems.drive.SwerveConstants.kFLSteerType; + +/** Container to hold various RBSI-specific parsing routines */ +public class RBSIParsing { + + /** + * Parse the module type given the type information for the FL module + * + * @return Byte The module type as bits in a byte. + */ + public static Byte parseModuleType() { + // NOTE: This assumes all 4 modules have the same arrangement! + Byte b_drive; // [x,x,-,-,-,-,-,-] + Byte b_steer; // [-,-,x,x,-,-,-,-] + Byte b_encoder; // [-,-,-,-,x,x,-,-] + switch (kFLDriveType) { + case "falcon": + case "kraken": + case "talonfx": + // CTRE Drive Motor + b_drive = 0b00; + break; + case "sparkmax": + case "sparkflex": + // REV Drive Motor + b_drive = 0b01; + break; + default: + throw new RuntimeException("Invalid drive motor type"); + } + switch (kFLSteerType) { + case "falcon": + case "kraken": + case "talonfx": + // CTRE Drive Motor + b_steer = 0b00; + break; + case "sparkmax": + case "sparkflex": + // REV Drive Motor + b_steer = 0b01; + break; + default: + throw new RuntimeException("Invalid steer motor type"); + } + switch (kFLEncoderType) { + case "cancoder": + // CTRE CANcoder + b_encoder = 0b00; + break; + case "analog": + // Analog Encoder + b_encoder = 0b01; + break; + default: + throw new RuntimeException("Invalid swerve encoder type"); + } + return (byte) (0b00000000 | b_drive << 6 | b_steer << 4 | b_encoder << 2); + } +} diff --git a/vendordeps/AdvantageKit.json b/vendordeps/AdvantageKit.json index c7f5262..fa81b2f 100644 --- a/vendordeps/AdvantageKit.json +++ b/vendordeps/AdvantageKit.json @@ -1,7 +1,7 @@ { "fileName": "AdvantageKit.json", "name": "AdvantageKit", - "version": "4.0.0", + "version": "4.1.0", "uuid": "d820cc26-74e3-11ec-90d6-0242ac120003", "frcYear": "2025", "mavenUrls": [ @@ -12,21 +12,22 @@ { "groupId": "org.littletonrobotics.akit", "artifactId": "akit-java", - "version": "4.0.0" + "version": "4.1.0" } ], "jniDependencies": [ { "groupId": "org.littletonrobotics.akit", "artifactId": "akit-wpilibio", - "version": "4.0.0", + "version": "4.1.0", "skipInvalidPlatforms": false, "isJar": false, "validPlatforms": [ "linuxathena", - "windowsx86-64", "linuxx86-64", - "osxuniversal" + "linuxarm64", + "osxuniversal", + "windowsx86-64" ] } ], diff --git a/vendordeps/Phoenix6-frc2025-latest.json b/vendordeps/Phoenix6-frc2025-latest.json index b1f42e3..820c61a 100644 --- a/vendordeps/Phoenix6-frc2025-latest.json +++ b/vendordeps/Phoenix6-frc2025-latest.json @@ -1,7 +1,7 @@ { "fileName": "Phoenix6-frc2025-latest.json", "name": "CTRE-Phoenix (v6)", - "version": "25.1.0", + "version": "25.2.1", "frcYear": "2025", "uuid": "e995de00-2c64-4df5-8831-c1441420ff19", "mavenUrls": [ @@ -19,14 +19,14 @@ { "groupId": "com.ctre.phoenix6", "artifactId": "wpiapi-java", - "version": "25.1.0" + "version": "25.2.1" } ], "jniDependencies": [ { "groupId": "com.ctre.phoenix6", "artifactId": "api-cpp", - "version": "25.1.0", + "version": "25.2.1", "isJar": false, "skipInvalidPlatforms": true, "validPlatforms": [ @@ -40,7 +40,7 @@ { "groupId": "com.ctre.phoenix6", "artifactId": "tools", - "version": "25.1.0", + "version": "25.2.1", "isJar": false, "skipInvalidPlatforms": true, "validPlatforms": [ @@ -54,7 +54,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "api-cpp-sim", - "version": "25.1.0", + "version": "25.2.1", "isJar": false, "skipInvalidPlatforms": true, "validPlatforms": [ @@ -68,7 +68,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "tools-sim", - "version": "25.1.0", + "version": "25.2.1", "isJar": false, "skipInvalidPlatforms": true, "validPlatforms": [ @@ -82,7 +82,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simTalonSRX", - "version": "25.1.0", + "version": "25.2.1", "isJar": false, "skipInvalidPlatforms": true, "validPlatforms": [ @@ -96,7 +96,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simVictorSPX", - "version": "25.1.0", + "version": "25.2.1", "isJar": false, "skipInvalidPlatforms": true, "validPlatforms": [ @@ -110,7 +110,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simPigeonIMU", - "version": "25.1.0", + "version": "25.2.1", "isJar": false, "skipInvalidPlatforms": true, "validPlatforms": [ @@ -124,7 +124,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simCANCoder", - "version": "25.1.0", + "version": "25.2.1", "isJar": false, "skipInvalidPlatforms": true, "validPlatforms": [ @@ -138,7 +138,21 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simProTalonFX", - "version": "25.1.0", + "version": "25.2.1", + "isJar": false, + "skipInvalidPlatforms": true, + "validPlatforms": [ + "windowsx86-64", + "linuxx86-64", + "linuxarm64", + "osxuniversal" + ], + "simMode": "swsim" + }, + { + "groupId": "com.ctre.phoenix6.sim", + "artifactId": "simProTalonFXS", + "version": "25.2.1", "isJar": false, "skipInvalidPlatforms": true, "validPlatforms": [ @@ -152,7 +166,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simProCANcoder", - "version": "25.1.0", + "version": "25.2.1", "isJar": false, "skipInvalidPlatforms": true, "validPlatforms": [ @@ -166,7 +180,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simProPigeon2", - "version": "25.1.0", + "version": "25.2.1", "isJar": false, "skipInvalidPlatforms": true, "validPlatforms": [ @@ -180,7 +194,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simProCANrange", - "version": "25.1.0", + "version": "25.2.1", "isJar": false, "skipInvalidPlatforms": true, "validPlatforms": [ @@ -196,7 +210,7 @@ { "groupId": "com.ctre.phoenix6", "artifactId": "wpiapi-cpp", - "version": "25.1.0", + "version": "25.2.1", "libName": "CTRE_Phoenix6_WPI", "headerClassifier": "headers", "sharedLibrary": true, @@ -212,7 +226,7 @@ { "groupId": "com.ctre.phoenix6", "artifactId": "tools", - "version": "25.1.0", + "version": "25.2.1", "libName": "CTRE_PhoenixTools", "headerClassifier": "headers", "sharedLibrary": true, @@ -228,7 +242,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "wpiapi-cpp-sim", - "version": "25.1.0", + "version": "25.2.1", "libName": "CTRE_Phoenix6_WPISim", "headerClassifier": "headers", "sharedLibrary": true, @@ -244,7 +258,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "tools-sim", - "version": "25.1.0", + "version": "25.2.1", "libName": "CTRE_PhoenixTools_Sim", "headerClassifier": "headers", "sharedLibrary": true, @@ -260,7 +274,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simTalonSRX", - "version": "25.1.0", + "version": "25.2.1", "libName": "CTRE_SimTalonSRX", "headerClassifier": "headers", "sharedLibrary": true, @@ -276,7 +290,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simVictorSPX", - "version": "25.1.0", + "version": "25.2.1", "libName": "CTRE_SimVictorSPX", "headerClassifier": "headers", "sharedLibrary": true, @@ -292,7 +306,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simPigeonIMU", - "version": "25.1.0", + "version": "25.2.1", "libName": "CTRE_SimPigeonIMU", "headerClassifier": "headers", "sharedLibrary": true, @@ -308,7 +322,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simCANCoder", - "version": "25.1.0", + "version": "25.2.1", "libName": "CTRE_SimCANCoder", "headerClassifier": "headers", "sharedLibrary": true, @@ -324,7 +338,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simProTalonFX", - "version": "25.1.0", + "version": "25.2.1", "libName": "CTRE_SimProTalonFX", "headerClassifier": "headers", "sharedLibrary": true, @@ -337,10 +351,26 @@ ], "simMode": "swsim" }, + { + "groupId": "com.ctre.phoenix6.sim", + "artifactId": "simProTalonFXS", + "version": "25.2.1", + "libName": "CTRE_SimProTalonFXS", + "headerClassifier": "headers", + "sharedLibrary": true, + "skipInvalidPlatforms": true, + "binaryPlatforms": [ + "windowsx86-64", + "linuxx86-64", + "linuxarm64", + "osxuniversal" + ], + "simMode": "swsim" + }, { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simProCANcoder", - "version": "25.1.0", + "version": "25.2.1", "libName": "CTRE_SimProCANcoder", "headerClassifier": "headers", "sharedLibrary": true, @@ -356,7 +386,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simProPigeon2", - "version": "25.1.0", + "version": "25.2.1", "libName": "CTRE_SimProPigeon2", "headerClassifier": "headers", "sharedLibrary": true, @@ -372,7 +402,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simProCANrange", - "version": "25.1.0", + "version": "25.2.1", "libName": "CTRE_SimProCANrange", "headerClassifier": "headers", "sharedLibrary": true, diff --git a/vendordeps/REVLib.json b/vendordeps/REVLib.json index 6e71967..552a3b0 100644 --- a/vendordeps/REVLib.json +++ b/vendordeps/REVLib.json @@ -1,7 +1,7 @@ { "fileName": "REVLib.json", "name": "REVLib", - "version": "2025.0.0", + "version": "2025.0.1", "frcYear": "2025", "uuid": "3f48eb8c-50fe-43a6-9cb7-44c86353c4cb", "mavenUrls": [ @@ -12,19 +12,18 @@ { "groupId": "com.revrobotics.frc", "artifactId": "REVLib-java", - "version": "2025.0.0" + "version": "2025.0.1" } ], "jniDependencies": [ { "groupId": "com.revrobotics.frc", "artifactId": "REVLib-driver", - "version": "2025.0.0", + "version": "2025.0.1", "skipInvalidPlatforms": true, "isJar": false, "validPlatforms": [ "windowsx86-64", - "windowsx86", "linuxarm64", "linuxx86-64", "linuxathena", @@ -37,14 +36,13 @@ { "groupId": "com.revrobotics.frc", "artifactId": "REVLib-cpp", - "version": "2025.0.0", + "version": "2025.0.1", "libName": "REVLib", "headerClassifier": "headers", "sharedLibrary": false, "skipInvalidPlatforms": true, "binaryPlatforms": [ "windowsx86-64", - "windowsx86", "linuxarm64", "linuxx86-64", "linuxathena", @@ -55,14 +53,13 @@ { "groupId": "com.revrobotics.frc", "artifactId": "REVLib-driver", - "version": "2025.0.0", + "version": "2025.0.1", "libName": "REVLibDriver", "headerClassifier": "headers", "sharedLibrary": false, "skipInvalidPlatforms": true, "binaryPlatforms": [ "windowsx86-64", - "windowsx86", "linuxarm64", "linuxx86-64", "linuxathena", diff --git a/vendordeps/ReduxLib-2025.0.1.json b/vendordeps/ReduxLib-2025.0.1.json new file mode 100644 index 0000000..3a66d72 --- /dev/null +++ b/vendordeps/ReduxLib-2025.0.1.json @@ -0,0 +1,72 @@ +{ + "fileName": "ReduxLib-2025.0.1.json", + "name": "ReduxLib", + "version": "2025.0.1", + "frcYear": "2025", + "uuid": "151ecca8-670b-4026-8160-cdd2679ef2bd", + "mavenUrls": [ + "https://maven.reduxrobotics.com/" + ], + "jsonUrl": "https://frcsdk.reduxrobotics.com/ReduxLib_2025.json", + "javaDependencies": [ + { + "groupId": "com.reduxrobotics.frc", + "artifactId": "ReduxLib-java", + "version": "2025.0.1" + } + ], + "jniDependencies": [ + { + "groupId": "com.reduxrobotics.frc", + "artifactId": "ReduxLib-driver", + "version": "2025.0.1", + "isJar": false, + "skipInvalidPlatforms": true, + "validPlatforms": [ + "linuxathena", + "linuxx86-64", + "linuxarm32", + "linuxarm64", + "osxuniversal", + "windowsx86-64" + ] + } + ], + "cppDependencies": [ + { + "groupId": "com.reduxrobotics.frc", + "artifactId": "ReduxLib-cpp", + "version": "2025.0.1", + "libName": "ReduxLib", + "headerClassifier": "headers", + "sourcesClassifier": "sources", + "sharedLibrary": true, + "skipInvalidPlatforms": true, + "binaryPlatforms": [ + "linuxathena", + "linuxx86-64", + "linuxarm32", + "linuxarm64", + "osxuniversal", + "windowsx86-64" + ] + }, + { + "groupId": "com.reduxrobotics.frc", + "artifactId": "ReduxLib-driver", + "version": "2025.0.1", + "libName": "ReduxCore", + "headerClassifier": "headers", + "sharedLibrary": true, + "skipInvalidPlatforms": true, + "binaryPlatforms": [ + "linuxathena", + "linuxx86-64", + "linuxarm32", + "linuxarm64", + "osxuniversal", + "windowsx86-64" + ] + } + ] +} diff --git a/vendordeps/Studica-2025.0.1.json b/vendordeps/Studica-2025.0.1.json new file mode 100644 index 0000000..a3ed1fe --- /dev/null +++ b/vendordeps/Studica-2025.0.1.json @@ -0,0 +1,71 @@ +{ + "fileName": "Studica-2025.0.1.json", + "name": "Studica", + "version": "2025.0.1", + "uuid": "cb311d09-36e9-4143-a032-55bb2b94443b", + "frcYear": "2025", + "mavenUrls": [ + "https://dev.studica.com/maven/release/2025/" + ], + "jsonUrl": "https://dev.studica.com/releases/2025/Studica-2025.0.1.json", + "cppDependencies": [ + { + "artifactId": "Studica-cpp", + "binaryPlatforms": [ + "linuxathena", + "linuxarm32", + "linuxarm64", + "linuxx86-64", + "osxuniversal", + "windowsx86-64" + ], + "groupId": "com.studica.frc", + "headerClassifier": "headers", + "libName": "Studica", + "sharedLibrary": false, + "skipInvalidPlatforms": true, + "version": "2025.0.1" + }, + { + "artifactId": "Studica-driver", + "binaryPlatforms": [ + "linuxathena", + "linuxarm32", + "linuxarm64", + "linuxx86-64", + "osxuniversal", + "windowsx86-64" + ], + "groupId": "com.studica.frc", + "headerClassifier": "headers", + "libName": "StudicaDriver", + "sharedLibrary": false, + "skipInvalidPlatforms": true, + "version": "2025.0.1" + } + ], + "javaDependencies": [ + { + "artifactId": "Studica-java", + "groupId": "com.studica.frc", + "version": "2025.0.1" + } + ], + "jniDependencies": [ + { + "artifactId": "Studica-driver", + "groupId": "com.studica.frc", + "isJar": false, + "skipInvalidPlatforms": true, + "validPlatforms": [ + "linuxathena", + "linuxarm32", + "linuxarm64", + "linuxx86-64", + "osxuniversal", + "windowsx86-64" + ], + "version": "2025.0.1" + } + ] +} diff --git a/vendordeps/ThriftyLib.json b/vendordeps/ThriftyLib.json index 2394862..287f30c 100644 --- a/vendordeps/ThriftyLib.json +++ b/vendordeps/ThriftyLib.json @@ -1,7 +1,7 @@ { "fileName": "ThriftyLib.json", "name": "ThriftyLib", - "version": "2025.0.1", + "version": "2025.0.2", "frcYear": "2025", "uuid": "60b2694b-9e6e-4026-81ee-6f167946f4b0", "mavenUrls": [ @@ -12,7 +12,7 @@ { "groupId": "com.thethriftybot.frc", "artifactId": "ThriftyLib-java", - "version": "2025.0.1" + "version": "2025.0.2" } ], "jniDependencies": [], diff --git a/vendordeps/maple-sim.json b/vendordeps/maple-sim.json index 39a6c80..f316b27 100644 --- a/vendordeps/maple-sim.json +++ b/vendordeps/maple-sim.json @@ -1,7 +1,7 @@ { "fileName": "maple-sim.json", "name": "maplesim", - "version": "0.3.1", + "version": "0.3.2", "frcYear": "2025", "uuid": "c39481e8-4a63-4a4c-9df6-48d91e4da37b", "mavenUrls": [ @@ -13,7 +13,7 @@ { "groupId": "org.ironmaple", "artifactId": "maplesim-java", - "version": "0.3.1" + "version": "0.3.2" }, { "groupId": "org.dyn4j", diff --git a/vendordeps/photonlib.json b/vendordeps/photonlib.json index 1163429..a908923 100644 --- a/vendordeps/photonlib.json +++ b/vendordeps/photonlib.json @@ -1,7 +1,7 @@ { "fileName": "photonlib.json", "name": "photonlib", - "version": "v2025.0.0-beta-8", + "version": "v2025.1.1", "uuid": "515fe07e-bfc6-11fa-b3de-0242ac130004", "frcYear": "2025", "mavenUrls": [ @@ -13,7 +13,7 @@ { "groupId": "org.photonvision", "artifactId": "photontargeting-cpp", - "version": "v2025.0.0-beta-8", + "version": "v2025.1.1", "skipInvalidPlatforms": true, "isJar": false, "validPlatforms": [ @@ -28,7 +28,7 @@ { "groupId": "org.photonvision", "artifactId": "photonlib-cpp", - "version": "v2025.0.0-beta-8", + "version": "v2025.1.1", "libName": "photonlib", "headerClassifier": "headers", "sharedLibrary": true, @@ -43,7 +43,7 @@ { "groupId": "org.photonvision", "artifactId": "photontargeting-cpp", - "version": "v2025.0.0-beta-8", + "version": "v2025.1.1", "libName": "photontargeting", "headerClassifier": "headers", "sharedLibrary": true, @@ -60,12 +60,12 @@ { "groupId": "org.photonvision", "artifactId": "photonlib-java", - "version": "v2025.0.0-beta-8" + "version": "v2025.1.1" }, { "groupId": "org.photonvision", "artifactId": "photontargeting-java", - "version": "v2025.0.0-beta-8" + "version": "v2025.1.1" } ] } diff --git a/vendordeps/yagsl-2025.2.2.json b/vendordeps/yagsl-2025.2.2.json new file mode 100644 index 0000000..55fa43b --- /dev/null +++ b/vendordeps/yagsl-2025.2.2.json @@ -0,0 +1,64 @@ +{ + "fileName": "yagsl-2025.2.2.json", + "name": "YAGSL", + "version": "2025.2.2", + "frcYear": "2025", + "uuid": "1ccce5a4-acd2-4d18-bca3-4b8047188400", + "mavenUrls": [ + "https://broncbotz3481.github.io/YAGSL-Lib/yagsl/repos" + ], + "jsonUrl": "https://broncbotz3481.github.io/YAGSL-Lib/yagsl/yagsl.json", + "javaDependencies": [ + { + "groupId": "swervelib", + "artifactId": "YAGSL-java", + "version": "2025.2.2" + } + ], + "requires": [ + { + "uuid": "3f48eb8c-50fe-43a6-9cb7-44c86353c4cb", + "errorMessage": "REVLib is required!", + "offlineFileName": "REVLib-2025.0.0.json", + "onlineUrl": "https://software-metadata.revrobotics.com/REVLib-2025.json" + }, + { + "uuid": "151ecca8-670b-4026-8160-cdd2679ef2bd", + "errorMessage": "ReduxLib is required!", + "offlineFileName": "ReduxLib-2025.0.0.json", + "onlineUrl": "https://frcsdk.reduxrobotics.com/ReduxLib_2025.json" + }, + { + "uuid": "e995de00-2c64-4df5-8831-c1441420ff19", + "errorMessage": "Phoenix6 is required!", + "offlineFileName": "Phoenix6-25.1.0.json", + "onlineUrl": "https://maven.ctr-electronics.com/release/com/ctre/phoenix6/latest/Phoenix6-frc2025-latest.json" + }, + { + "uuid": "ab676553-b602-441f-a38d-f1296eff6537", + "errorMessage": "Phoenix5 is required!", + "offlineFileName": "Phoenix5-5.35.0.json", + "onlineUrl": "https://maven.ctr-electronics.com/release/com/ctre/phoenix/Phoenix5-frc2025-latest.json" + }, + { + "uuid": "cb311d09-36e9-4143-a032-55bb2b94443b", + "errorMessage": "Studica is required!", + "offlineFileName": "Studica-2025.0.0.json", + "onlineUrl": "https://dev.studica.com/releases/2025/Studica-2025.0.0.json" + }, + { + "uuid": "60b2694b-9e6e-4026-81ee-6f167946f4b0", + "errorMessage": "ThriftyLib is required!", + "offlineFileName": "ThriftyLib.json", + "onlineUrl": "https://docs.home.thethriftybot.com/ThriftyLib.json" + }, + { + "uuid": "c39481e8-4a63-4a4c-9df6-48d91e4da37b", + "errorMessage": "maple-sim is required for simulation", + "offlineFileName": "maple-sim.json", + "onlineUrl": "https://shenzhen-robotics-alliance.github.io/maple-sim/vendordep/maple-sim.json" + } + ], + "jniDependencies": [], + "cppDependencies": [] +} From d21abf6015197466de7624bde014b77ff3aca3b5 Mon Sep 17 00:00:00 2001 From: AquaShock211 <143662909+AquaShock211@users.noreply.github.com> Date: Tue, 21 Jan 2025 14:54:22 -0700 Subject: [PATCH 08/15] Fixed the not driving problem by changing the max linear speed --- src/main/java/frc/robot/Constants.java | 14 +++++++------- src/main/java/frc/robot/util/RBSIEnum.java | 8 ++++---- 2 files changed, 11 insertions(+), 11 deletions(-) diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java index bd1f1dc..61ead24 100644 --- a/src/main/java/frc/robot/Constants.java +++ b/src/main/java/frc/robot/Constants.java @@ -41,7 +41,6 @@ import frc.robot.util.RBSIEnum.CTREPro; import frc.robot.util.RBSIEnum.Mode; import frc.robot.util.RBSIEnum.MotorIdleMode; -import frc.robot.util.RBSIEnum.PracticeSpace; import frc.robot.util.RBSIEnum.RobotType; import frc.robot.util.RBSIEnum.SwerveType; import frc.robot.util.RBSIEnum.VisionType; @@ -78,7 +77,7 @@ public final class Constants { private static AutoType autoType = AutoType.PATHPLANNER; // PATHPLANNER, CHOREO private static VisionType visionType = VisionType.NONE; // PHOTON, LIMELIGHT, NONE - private static PracticeSpace practiceSpace = PracticeSpace.NUTHOUSE; // NUTHOUSE, FIELD + // private static PracticeSpace practiceSpace = PracticeSpace.NUTHOUSE; // NUTHOUSE, FIELD /** Checks whether the correct robot is selected when deploying. */ public static void main(String... args) { @@ -132,11 +131,12 @@ public static final class DrivebaseConstants { // Theoretical free speed (m/s) at 12v applied output; // IMPORTANT: Follow the AdvantageKit instructions for measuring the ACTUAL maximum linear speed // of YOUR ROBOT, and replace the estimate here with your measured value! - public static final double kMaxLinearSpeed = - switch (practiceSpace) { - case NUTHOUSE -> Units.feetToMeters(1); - case FIELD -> Units.feetToMeters(18); - }; + // public static final double kMaxLinearSpeed = + // switch (practiceSpace) { + // case NUTHOUSE -> Units.feetToMeters(1); + // case FIELD -> Units.feetToMeters(18); + // }; + public static final double kMaxLinearSpeed = Units.feetToMeters(18); // Set 3/4 of a rotation per second as the max angular velocity (radians/sec) public static final double kMaxAngularSpeed = 1.5 * Math.PI; diff --git a/src/main/java/frc/robot/util/RBSIEnum.java b/src/main/java/frc/robot/util/RBSIEnum.java index adaf066..49c3181 100644 --- a/src/main/java/frc/robot/util/RBSIEnum.java +++ b/src/main/java/frc/robot/util/RBSIEnum.java @@ -67,8 +67,8 @@ public static enum MotorIdleMode { BRAKE // Hold motor position when idle } - public static enum PracticeSpace { - NUTHOUSE, - FIELD - } + // public static enum PracticeSpace { + // NUTHOUSE, + // FIELD + // } } From e1a01033a2109da3e199fa111e43dc5a6a96f188 Mon Sep 17 00:00:00 2001 From: AquaShock211 <143662909+AquaShock211@users.noreply.github.com> Date: Sat, 25 Jan 2025 19:40:01 -0700 Subject: [PATCH 09/15] So the robot moves less than 6 in even at 7 m/s --- src/main/deploy/pathplanner/paths/Consistancy Test.path | 6 +++--- src/main/java/frc/robot/Constants.java | 4 ++-- 2 files changed, 5 insertions(+), 5 deletions(-) diff --git a/src/main/deploy/pathplanner/paths/Consistancy Test.path b/src/main/deploy/pathplanner/paths/Consistancy Test.path index 5254abc..f5b104c 100644 --- a/src/main/deploy/pathplanner/paths/Consistancy Test.path +++ b/src/main/deploy/pathplanner/paths/Consistancy Test.path @@ -33,8 +33,8 @@ "pointTowardsZones": [], "eventMarkers": [], "globalConstraints": { - "maxVelocity": 0.5, - "maxAcceleration": 1.0, + "maxVelocity": 4.0, + "maxAcceleration": 7.0, "maxAngularVelocity": 540.0, "maxAngularAcceleration": 1200.0, "nominalVoltage": 12.0, @@ -50,5 +50,5 @@ "velocity": 0, "rotation": 0.0 }, - "useDefaultConstraints": false + "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 61ead24..7137712 100644 --- a/src/main/java/frc/robot/Constants.java +++ b/src/main/java/frc/robot/Constants.java @@ -243,8 +243,8 @@ public static class OperatorConstants { public static final class AutoConstants { // Drive and Turn PID constants used for PathPlanner - public static final PIDConstants kPPdrivePID = new PIDConstants(5.0, 0.0, 0.0); - public static final PIDConstants kPPsteerPID = new PIDConstants(5.0, 0.0, 0.0); + public static final PIDConstants kPPdrivePID = new PIDConstants(0.0, 0.0, 0.0); + public static final PIDConstants kPPsteerPID = new PIDConstants(0.0, 0.0, 0.0); // PathPlanner Config constants public static final RobotConfig kPathPlannerConfig = From 56e7ae4bcd05a8826dc870f131c84fa8662af3ec Mon Sep 17 00:00:00 2001 From: tbowers7 Date: Thu, 30 Jan 2025 23:17:35 +0000 Subject: [PATCH 10/15] Update to the latest version of Az-RBSI Signed-off-by: tbowers7 --- build.gradle | 2 +- src/main/deploy/apriltags/2025-official.json | 404 +++++++++++++++++++ src/main/java/frc/robot/Constants.java | 20 +- src/main/java/frc/robot/util/RBSIEnum.java | 7 - vendordeps/ChoreoLib2025.json | 6 +- vendordeps/PathplannerLib.json | 6 +- vendordeps/REVLib.json | 10 +- vendordeps/URCL.json | 10 +- vendordeps/maple-sim.json | 4 +- 9 files changed, 436 insertions(+), 33 deletions(-) create mode 100644 src/main/deploy/apriltags/2025-official.json diff --git a/build.gradle b/build.gradle index 1dfb32a..bc67bd7 100644 --- a/build.gradle +++ b/build.gradle @@ -3,7 +3,7 @@ plugins { id "edu.wpi.first.GradleRIO" version "2025.2.1" id "com.peterabeles.gversion" version "1.10.3" id "com.diffplug.spotless" version "7.0.2" - id "io.freefair.lombok" version "8.11" + id "io.freefair.lombok" version "8.12" id "com.google.protobuf" version "0.9.4" } diff --git a/src/main/deploy/apriltags/2025-official.json b/src/main/deploy/apriltags/2025-official.json new file mode 100644 index 0000000..eb395c0 --- /dev/null +++ b/src/main/deploy/apriltags/2025-official.json @@ -0,0 +1,404 @@ +{ + "tags": [ + { + "ID": 1, + "pose": { + "translation": { + "x": 16.697198, + "y": 0.65532, + "z": 1.4859 + }, + "rotation": { + "quaternion": { + "W": 0.4539904997395468, + "X": 0.0, + "Y": 0.0, + "Z": 0.8910065241883678 + } + } + } + }, + { + "ID": 2, + "pose": { + "translation": { + "x": 16.697198, + "y": 7.3964799999999995, + "z": 1.4859 + }, + "rotation": { + "quaternion": { + "W": -0.45399049973954675, + "X": -0.0, + "Y": 0.0, + "Z": 0.8910065241883679 + } + } + } + }, + { + "ID": 3, + "pose": { + "translation": { + "x": 11.560809999999998, + "y": 8.05561, + "z": 1.30175 + }, + "rotation": { + "quaternion": { + "W": -0.7071067811865475, + "X": -0.0, + "Y": 0.0, + "Z": 0.7071067811865476 + } + } + } + }, + { + "ID": 4, + "pose": { + "translation": { + "x": 9.276079999999999, + "y": 6.137656, + "z": 1.8679160000000001 + }, + "rotation": { + "quaternion": { + "W": 0.9659258262890683, + "X": 0.0, + "Y": 0.25881904510252074, + "Z": 0.0 + } + } + } + }, + { + "ID": 5, + "pose": { + "translation": { + "x": 9.276079999999999, + "y": 1.914906, + "z": 1.8679160000000001 + }, + "rotation": { + "quaternion": { + "W": 0.9659258262890683, + "X": 0.0, + "Y": 0.25881904510252074, + "Z": 0.0 + } + } + } + }, + { + "ID": 6, + "pose": { + "translation": { + "x": 13.474446, + "y": 3.3063179999999996, + "z": 0.308102 + }, + "rotation": { + "quaternion": { + "W": -0.8660254037844387, + "X": -0.0, + "Y": 0.0, + "Z": 0.49999999999999994 + } + } + } + }, + { + "ID": 7, + "pose": { + "translation": { + "x": 13.890498, + "y": 4.0259, + "z": 0.308102 + }, + "rotation": { + "quaternion": { + "W": 1.0, + "X": 0.0, + "Y": 0.0, + "Z": 0.0 + } + } + } + }, + { + "ID": 8, + "pose": { + "translation": { + "x": 13.474446, + "y": 4.745482, + "z": 0.308102 + }, + "rotation": { + "quaternion": { + "W": 0.8660254037844387, + "X": 0.0, + "Y": 0.0, + "Z": 0.49999999999999994 + } + } + } + }, + { + "ID": 9, + "pose": { + "translation": { + "x": 12.643358, + "y": 4.745482, + "z": 0.308102 + }, + "rotation": { + "quaternion": { + "W": 0.5000000000000001, + "X": 0.0, + "Y": 0.0, + "Z": 0.8660254037844386 + } + } + } + }, + { + "ID": 10, + "pose": { + "translation": { + "x": 12.227305999999999, + "y": 4.0259, + "z": 0.308102 + }, + "rotation": { + "quaternion": { + "W": 6.123233995736766e-17, + "X": 0.0, + "Y": 0.0, + "Z": 1.0 + } + } + } + }, + { + "ID": 11, + "pose": { + "translation": { + "x": 12.643358, + "y": 3.3063179999999996, + "z": 0.308102 + }, + "rotation": { + "quaternion": { + "W": -0.4999999999999998, + "X": -0.0, + "Y": 0.0, + "Z": 0.8660254037844387 + } + } + } + }, + { + "ID": 12, + "pose": { + "translation": { + "x": 0.851154, + "y": 0.65532, + "z": 1.4859 + }, + "rotation": { + "quaternion": { + "W": 0.8910065241883679, + "X": 0.0, + "Y": 0.0, + "Z": 0.45399049973954675 + } + } + } + }, + { + "ID": 13, + "pose": { + "translation": { + "x": 0.851154, + "y": 7.3964799999999995, + "z": 1.4859 + }, + "rotation": { + "quaternion": { + "W": -0.8910065241883678, + "X": -0.0, + "Y": 0.0, + "Z": 0.45399049973954686 + } + } + } + }, + { + "ID": 14, + "pose": { + "translation": { + "x": 8.272272, + "y": 6.137656, + "z": 1.8679160000000001 + }, + "rotation": { + "quaternion": { + "W": 5.914589856893349e-17, + "X": -0.25881904510252074, + "Y": 1.5848095757158825e-17, + "Z": 0.9659258262890683 + } + } + } + }, + { + "ID": 15, + "pose": { + "translation": { + "x": 8.272272, + "y": 1.914906, + "z": 1.8679160000000001 + }, + "rotation": { + "quaternion": { + "W": 5.914589856893349e-17, + "X": -0.25881904510252074, + "Y": 1.5848095757158825e-17, + "Z": 0.9659258262890683 + } + } + } + }, + { + "ID": 16, + "pose": { + "translation": { + "x": 5.9875419999999995, + "y": -0.0038099999999999996, + "z": 1.30175 + }, + "rotation": { + "quaternion": { + "W": 0.7071067811865476, + "X": 0.0, + "Y": 0.0, + "Z": 0.7071067811865476 + } + } + } + }, + { + "ID": 17, + "pose": { + "translation": { + "x": 4.073905999999999, + "y": 3.3063179999999996, + "z": 0.308102 + }, + "rotation": { + "quaternion": { + "W": -0.4999999999999998, + "X": -0.0, + "Y": 0.0, + "Z": 0.8660254037844387 + } + } + } + }, + { + "ID": 18, + "pose": { + "translation": { + "x": 3.6576, + "y": 4.0259, + "z": 0.308102 + }, + "rotation": { + "quaternion": { + "W": 6.123233995736766e-17, + "X": 0.0, + "Y": 0.0, + "Z": 1.0 + } + } + } + }, + { + "ID": 19, + "pose": { + "translation": { + "x": 4.073905999999999, + "y": 4.745482, + "z": 0.308102 + }, + "rotation": { + "quaternion": { + "W": 0.5000000000000001, + "X": 0.0, + "Y": 0.0, + "Z": 0.8660254037844386 + } + } + } + }, + { + "ID": 20, + "pose": { + "translation": { + "x": 4.904739999999999, + "y": 4.745482, + "z": 0.308102 + }, + "rotation": { + "quaternion": { + "W": 0.8660254037844387, + "X": 0.0, + "Y": 0.0, + "Z": 0.49999999999999994 + } + } + } + }, + { + "ID": 21, + "pose": { + "translation": { + "x": 5.321046, + "y": 4.0259, + "z": 0.308102 + }, + "rotation": { + "quaternion": { + "W": 1.0, + "X": 0.0, + "Y": 0.0, + "Z": 0.0 + } + } + } + }, + { + "ID": 22, + "pose": { + "translation": { + "x": 4.904739999999999, + "y": 3.3063179999999996, + "z": 0.308102 + }, + "rotation": { + "quaternion": { + "W": -0.8660254037844387, + "X": -0.0, + "Y": 0.0, + "Z": 0.49999999999999994 + } + } + } + } + ], + "field": { + "length": 17.548, + "width": 8.052 + } +} diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java index 08fb4a5..89b556f 100644 --- a/src/main/java/frc/robot/Constants.java +++ b/src/main/java/frc/robot/Constants.java @@ -41,7 +41,6 @@ import frc.robot.util.RBSIEnum.CTREPro; import frc.robot.util.RBSIEnum.Mode; import frc.robot.util.RBSIEnum.MotorIdleMode; -import frc.robot.util.RBSIEnum.RobotType; import frc.robot.util.RBSIEnum.SwerveType; import frc.robot.util.RBSIEnum.VisionType; import frc.robot.util.RobotDeviceId; @@ -77,6 +76,13 @@ public final class Constants { private static AutoType autoType = AutoType.PATHPLANNER; // PATHPLANNER, CHOREO private static VisionType visionType = VisionType.NONE; // PHOTON, LIMELIGHT, NONE + /** Enumerate the robot types (name your robots here) */ + public static enum RobotType { + DEVBOT, // Development / Alpha / Practice Bot + COMPBOT, // Competition robot + SIMBOT // Simulated robot + } + /** Checks whether the correct robot is selected when deploying. */ public static void main(String... args) { if (robotType == RobotType.SIMBOT) { @@ -368,8 +374,7 @@ public static class CANandPowerPorts { } /** AprilTag Field Layout ************************************************ */ - /* SEASON SPECIFIC! -- This section is for 2024 (Crescendo) */ - // NOTE: This section will be updated to 2025 "Reefscape" following kickoff + /* SEASON SPECIFIC! -- This section is for 2025 (Reefscape) */ public static class AprilTagConstants { public static final double aprilTagWidth = Units.inchesToMeters(6.50); @@ -381,10 +386,11 @@ public static class AprilTagConstants { @Getter public enum AprilTagLayoutType { - OFFICIAL("2024-official"), - SPEAKERS_ONLY("2024-speakers"), - AMPS_ONLY("2024-amps"), - WPI("2024-wpi"); + OFFICIAL("2025-official"); + + // SPEAKERS_ONLY("2024-speakers"), + // AMPS_ONLY("2024-amps"), + // WPI("2024-wpi"); private AprilTagLayoutType(String name) { if (Constants.disableHAL) { diff --git a/src/main/java/frc/robot/util/RBSIEnum.java b/src/main/java/frc/robot/util/RBSIEnum.java index ddb31d8..00495da 100644 --- a/src/main/java/frc/robot/util/RBSIEnum.java +++ b/src/main/java/frc/robot/util/RBSIEnum.java @@ -19,13 +19,6 @@ */ public class RBSIEnum { - /** Enumerate the robot types (add additional bots here) */ - public static enum RobotType { - DEVBOT, // Development / Alpha / Practice Bot - COMPBOT, // Competition robot - SIMBOT // Simulated robot - } - /** Enumerate the robot operation modes */ public static enum Mode { REAL, // REAL == Running on a real robot diff --git a/vendordeps/ChoreoLib2025.json b/vendordeps/ChoreoLib2025.json index faef011..7bb0933 100644 --- a/vendordeps/ChoreoLib2025.json +++ b/vendordeps/ChoreoLib2025.json @@ -1,7 +1,7 @@ { "fileName": "ChoreoLib2025.json", "name": "ChoreoLib", - "version": "2025.0.1", + "version": "2025.0.2", "uuid": "b5e23f0a-dac9-4ad2-8dd6-02767c520aca", "frcYear": "2025", "mavenUrls": [ @@ -13,7 +13,7 @@ { "groupId": "choreo", "artifactId": "ChoreoLib-java", - "version": "2025.0.1" + "version": "2025.0.2" }, { "groupId": "com.google.code.gson", @@ -26,7 +26,7 @@ { "groupId": "choreo", "artifactId": "ChoreoLib-cpp", - "version": "2025.0.1", + "version": "2025.0.2", "libName": "ChoreoLib", "headerClassifier": "headers", "sharedLibrary": false, diff --git a/vendordeps/PathplannerLib.json b/vendordeps/PathplannerLib.json index 1371a40..c0f7c11 100644 --- a/vendordeps/PathplannerLib.json +++ b/vendordeps/PathplannerLib.json @@ -1,7 +1,7 @@ { "fileName": "PathplannerLib.json", "name": "PathplannerLib", - "version": "2025.2.1", + "version": "2025.2.2", "uuid": "1b42324f-17c6-4875-8e77-1c312bc8c786", "frcYear": "2025", "mavenUrls": [ @@ -12,7 +12,7 @@ { "groupId": "com.pathplanner.lib", "artifactId": "PathplannerLib-java", - "version": "2025.2.1" + "version": "2025.2.2" } ], "jniDependencies": [], @@ -20,7 +20,7 @@ { "groupId": "com.pathplanner.lib", "artifactId": "PathplannerLib-cpp", - "version": "2025.2.1", + "version": "2025.2.2", "libName": "PathplannerLib", "headerClassifier": "headers", "sharedLibrary": false, diff --git a/vendordeps/REVLib.json b/vendordeps/REVLib.json index 552a3b0..9658c4a 100644 --- a/vendordeps/REVLib.json +++ b/vendordeps/REVLib.json @@ -1,7 +1,7 @@ { "fileName": "REVLib.json", "name": "REVLib", - "version": "2025.0.1", + "version": "2025.0.2", "frcYear": "2025", "uuid": "3f48eb8c-50fe-43a6-9cb7-44c86353c4cb", "mavenUrls": [ @@ -12,14 +12,14 @@ { "groupId": "com.revrobotics.frc", "artifactId": "REVLib-java", - "version": "2025.0.1" + "version": "2025.0.2" } ], "jniDependencies": [ { "groupId": "com.revrobotics.frc", "artifactId": "REVLib-driver", - "version": "2025.0.1", + "version": "2025.0.2", "skipInvalidPlatforms": true, "isJar": false, "validPlatforms": [ @@ -36,7 +36,7 @@ { "groupId": "com.revrobotics.frc", "artifactId": "REVLib-cpp", - "version": "2025.0.1", + "version": "2025.0.2", "libName": "REVLib", "headerClassifier": "headers", "sharedLibrary": false, @@ -53,7 +53,7 @@ { "groupId": "com.revrobotics.frc", "artifactId": "REVLib-driver", - "version": "2025.0.1", + "version": "2025.0.2", "libName": "REVLibDriver", "headerClassifier": "headers", "sharedLibrary": false, diff --git a/vendordeps/URCL.json b/vendordeps/URCL.json index c991b6a..6beb912 100644 --- a/vendordeps/URCL.json +++ b/vendordeps/URCL.json @@ -1,7 +1,7 @@ { "fileName": "URCL.json", "name": "URCL", - "version": "2025.0.0", + "version": "2025.0.1", "frcYear": "2025", "uuid": "84246d17-a797-4d1e-bd9f-c59cd8d2477c", "mavenUrls": [ @@ -12,14 +12,14 @@ { "groupId": "org.littletonrobotics.urcl", "artifactId": "URCL-java", - "version": "2025.0.0" + "version": "2025.0.1" } ], "jniDependencies": [ { "groupId": "org.littletonrobotics.urcl", "artifactId": "URCL-driver", - "version": "2025.0.0", + "version": "2025.0.1", "skipInvalidPlatforms": true, "isJar": false, "validPlatforms": [ @@ -34,7 +34,7 @@ { "groupId": "org.littletonrobotics.urcl", "artifactId": "URCL-cpp", - "version": "2025.0.0", + "version": "2025.0.1", "libName": "URCL", "headerClassifier": "headers", "sharedLibrary": false, @@ -49,7 +49,7 @@ { "groupId": "org.littletonrobotics.urcl", "artifactId": "URCL-driver", - "version": "2025.0.0", + "version": "2025.0.1", "libName": "URCLDriver", "headerClassifier": "headers", "sharedLibrary": false, diff --git a/vendordeps/maple-sim.json b/vendordeps/maple-sim.json index f316b27..59b3a20 100644 --- a/vendordeps/maple-sim.json +++ b/vendordeps/maple-sim.json @@ -1,7 +1,7 @@ { "fileName": "maple-sim.json", "name": "maplesim", - "version": "0.3.2", + "version": "0.3.5", "frcYear": "2025", "uuid": "c39481e8-4a63-4a4c-9df6-48d91e4da37b", "mavenUrls": [ @@ -13,7 +13,7 @@ { "groupId": "org.ironmaple", "artifactId": "maplesim-java", - "version": "0.3.2" + "version": "0.3.5" }, { "groupId": "org.dyn4j", From c9ad83130b3857885061d6068e824125e03441d4 Mon Sep 17 00:00:00 2001 From: "Timothy P. Ellsworth Bowers" Date: Thu, 30 Jan 2025 16:21:29 -0700 Subject: [PATCH 11/15] Remove unused files from Az-RBSI update deleted: src/main/deploy/apriltags/2024-amps.json deleted: src/main/deploy/apriltags/2024-official.json deleted: src/main/deploy/apriltags/2024-speakers.json deleted: src/main/deploy/apriltags/2024-wpi.json deleted: vendordeps/ReduxLib-2025.0.0.json deleted: vendordeps/Studica-2025.0.0.json deleted: vendordeps/yagsl-2025.1.3.jwt151.json --- src/main/deploy/apriltags/2024-amps.json | 44 --- src/main/deploy/apriltags/2024-official.json | 296 ------------------- src/main/deploy/apriltags/2024-speakers.json | 80 ----- src/main/deploy/apriltags/2024-wpi.json | 116 -------- vendordeps/ReduxLib-2025.0.0.json | 72 ----- vendordeps/Studica-2025.0.0.json | 71 ----- vendordeps/yagsl-2025.1.3.jwt151.json | 64 ---- 7 files changed, 743 deletions(-) delete mode 100644 src/main/deploy/apriltags/2024-amps.json delete mode 100644 src/main/deploy/apriltags/2024-official.json delete mode 100644 src/main/deploy/apriltags/2024-speakers.json delete mode 100644 src/main/deploy/apriltags/2024-wpi.json delete mode 100644 vendordeps/ReduxLib-2025.0.0.json delete mode 100644 vendordeps/Studica-2025.0.0.json delete mode 100644 vendordeps/yagsl-2025.1.3.jwt151.json diff --git a/src/main/deploy/apriltags/2024-amps.json b/src/main/deploy/apriltags/2024-amps.json deleted file mode 100644 index ddaf4be..0000000 --- a/src/main/deploy/apriltags/2024-amps.json +++ /dev/null @@ -1,44 +0,0 @@ -{ - "tags": [ - { - "ID": 5, - "pose": { - "translation": { - "x": 14.700757999999999, - "y": 8.2042, - "z": 1.355852 - }, - "rotation": { - "quaternion": { - "W": -0.7071067811865475, - "X": -0.0, - "Y": 0.0, - "Z": 0.7071067811865476 - } - } - } - }, - { - "ID": 6, - "pose": { - "translation": { - "x": 1.8415, - "y": 8.2042, - "z": 1.355852 - }, - "rotation": { - "quaternion": { - "W": -0.7071067811865475, - "X": -0.0, - "Y": 0.0, - "Z": 0.7071067811865476 - } - } - } - } - ], - "field": { - "length": 16.541, - "width": 8.211 - } -} diff --git a/src/main/deploy/apriltags/2024-official.json b/src/main/deploy/apriltags/2024-official.json deleted file mode 100644 index 8cb837a..0000000 --- a/src/main/deploy/apriltags/2024-official.json +++ /dev/null @@ -1,296 +0,0 @@ -{ - "tags": [ - { - "ID": 1, - "pose": { - "translation": { - "x": 15.079471999999997, - "y": 0.24587199999999998, - "z": 1.355852 - }, - "rotation": { - "quaternion": { - "W": 0.5000000000000001, - "X": 0.0, - "Y": 0.0, - "Z": 0.8660254037844386 - } - } - } - }, - { - "ID": 2, - "pose": { - "translation": { - "x": 16.185134, - "y": 0.883666, - "z": 1.355852 - }, - "rotation": { - "quaternion": { - "W": 0.5000000000000001, - "X": 0.0, - "Y": 0.0, - "Z": 0.8660254037844386 - } - } - } - }, - { - "ID": 3, - "pose": { - "translation": { - "x": 16.579342, - "y": 4.982717999999999, - "z": 1.4511020000000001 - }, - "rotation": { - "quaternion": { - "W": 6.123233995736766e-17, - "X": 0.0, - "Y": 0.0, - "Z": 1.0 - } - } - } - }, - { - "ID": 4, - "pose": { - "translation": { - "x": 16.579342, - "y": 5.547867999999999, - "z": 1.4511020000000001 - }, - "rotation": { - "quaternion": { - "W": 6.123233995736766e-17, - "X": 0.0, - "Y": 0.0, - "Z": 1.0 - } - } - } - }, - { - "ID": 5, - "pose": { - "translation": { - "x": 14.700757999999999, - "y": 8.2042, - "z": 1.355852 - }, - "rotation": { - "quaternion": { - "W": -0.7071067811865475, - "X": -0.0, - "Y": 0.0, - "Z": 0.7071067811865476 - } - } - } - }, - { - "ID": 6, - "pose": { - "translation": { - "x": 1.8415, - "y": 8.2042, - "z": 1.355852 - }, - "rotation": { - "quaternion": { - "W": -0.7071067811865475, - "X": -0.0, - "Y": 0.0, - "Z": 0.7071067811865476 - } - } - } - }, - { - "ID": 7, - "pose": { - "translation": { - "x": -0.038099999999999995, - "y": 5.547867999999999, - "z": 1.4511020000000001 - }, - "rotation": { - "quaternion": { - "W": 1.0, - "X": 0.0, - "Y": 0.0, - "Z": 0.0 - } - } - } - }, - { - "ID": 8, - "pose": { - "translation": { - "x": -0.038099999999999995, - "y": 4.982717999999999, - "z": 1.4511020000000001 - }, - "rotation": { - "quaternion": { - "W": 1.0, - "X": 0.0, - "Y": 0.0, - "Z": 0.0 - } - } - } - }, - { - "ID": 9, - "pose": { - "translation": { - "x": 0.356108, - "y": 0.883666, - "z": 1.355852 - }, - "rotation": { - "quaternion": { - "W": 0.8660254037844387, - "X": 0.0, - "Y": 0.0, - "Z": 0.49999999999999994 - } - } - } - }, - { - "ID": 10, - "pose": { - "translation": { - "x": 1.4615159999999998, - "y": 0.24587199999999998, - "z": 1.355852 - }, - "rotation": { - "quaternion": { - "W": 0.8660254037844387, - "X": 0.0, - "Y": 0.0, - "Z": 0.49999999999999994 - } - } - } - }, - { - "ID": 11, - "pose": { - "translation": { - "x": 11.904726, - "y": 3.7132259999999997, - "z": 1.3208 - }, - "rotation": { - "quaternion": { - "W": -0.8660254037844387, - "X": -0.0, - "Y": 0.0, - "Z": 0.49999999999999994 - } - } - } - }, - { - "ID": 12, - "pose": { - "translation": { - "x": 11.904726, - "y": 4.49834, - "z": 1.3208 - }, - "rotation": { - "quaternion": { - "W": 0.8660254037844387, - "X": 0.0, - "Y": 0.0, - "Z": 0.49999999999999994 - } - } - } - }, - { - "ID": 13, - "pose": { - "translation": { - "x": 11.220196, - "y": 4.105148, - "z": 1.3208 - }, - "rotation": { - "quaternion": { - "W": 6.123233995736766e-17, - "X": 0.0, - "Y": 0.0, - "Z": 1.0 - } - } - } - }, - { - "ID": 14, - "pose": { - "translation": { - "x": 5.320792, - "y": 4.105148, - "z": 1.3208 - }, - "rotation": { - "quaternion": { - "W": 1.0, - "X": 0.0, - "Y": 0.0, - "Z": 0.0 - } - } - } - }, - { - "ID": 15, - "pose": { - "translation": { - "x": 4.641342, - "y": 4.49834, - "z": 1.3208 - }, - "rotation": { - "quaternion": { - "W": 0.5000000000000001, - "X": 0.0, - "Y": 0.0, - "Z": 0.8660254037844386 - } - } - } - }, - { - "ID": 16, - "pose": { - "translation": { - "x": 4.641342, - "y": 3.7132259999999997, - "z": 1.3208 - }, - "rotation": { - "quaternion": { - "W": -0.4999999999999998, - "X": -0.0, - "Y": 0.0, - "Z": 0.8660254037844387 - } - } - } - } - ], - "field": { - "length": 16.541, - "width": 8.211 - } -} diff --git a/src/main/deploy/apriltags/2024-speakers.json b/src/main/deploy/apriltags/2024-speakers.json deleted file mode 100644 index 36ecc9d..0000000 --- a/src/main/deploy/apriltags/2024-speakers.json +++ /dev/null @@ -1,80 +0,0 @@ -{ - "tags": [ - { - "ID": 3, - "pose": { - "translation": { - "x": 16.579342, - "y": 4.982717999999999, - "z": 1.4511020000000001 - }, - "rotation": { - "quaternion": { - "W": 6.123233995736766e-17, - "X": 0.0, - "Y": 0.0, - "Z": 1.0 - } - } - } - }, - { - "ID": 4, - "pose": { - "translation": { - "x": 16.579342, - "y": 5.547867999999999, - "z": 1.4511020000000001 - }, - "rotation": { - "quaternion": { - "W": 6.123233995736766e-17, - "X": 0.0, - "Y": 0.0, - "Z": 1.0 - } - } - } - }, - { - "ID": 7, - "pose": { - "translation": { - "x": -0.038099999999999995, - "y": 5.547867999999999, - "z": 1.4511020000000001 - }, - "rotation": { - "quaternion": { - "W": 1.0, - "X": 0.0, - "Y": 0.0, - "Z": 0.0 - } - } - } - }, - { - "ID": 8, - "pose": { - "translation": { - "x": -0.038099999999999995, - "y": 4.982717999999999, - "z": 1.4511020000000001 - }, - "rotation": { - "quaternion": { - "W": 1.0, - "X": 0.0, - "Y": 0.0, - "Z": 0.0 - } - } - } - } - ], - "field": { - "length": 16.541, - "width": 8.211 - } -} diff --git a/src/main/deploy/apriltags/2024-wpi.json b/src/main/deploy/apriltags/2024-wpi.json deleted file mode 100644 index bbf6862..0000000 --- a/src/main/deploy/apriltags/2024-wpi.json +++ /dev/null @@ -1,116 +0,0 @@ -{ - "tags": [ - { - "ID": 3, - "pose": { - "translation": { - "x": 16.579342, - "y": 4.982717999999999, - "z": 1.4511020000000001 - }, - "rotation": { - "quaternion": { - "W": 6.123233995736766e-17, - "X": 0.0, - "Y": 0.0, - "Z": 1.0 - } - } - } - }, - { - "ID": 4, - "pose": { - "translation": { - "x": 16.579342, - "y": 5.547867999999999, - "z": 1.4511020000000001 - }, - "rotation": { - "quaternion": { - "W": 6.123233995736766e-17, - "X": 0.0, - "Y": 0.0, - "Z": 1.0 - } - } - } - }, - { - "ID": 5, - "pose": { - "translation": { - "x": 14.700757999999999, - "y": 8.2042, - "z": 1.355852 - }, - "rotation": { - "quaternion": { - "W": -0.7071067811865475, - "X": -0.0, - "Y": 0.0, - "Z": 0.7071067811865476 - } - } - } - }, - { - "ID": 6, - "pose": { - "translation": { - "x": 1.8415, - "y": 8.2042, - "z": 1.355852 - }, - "rotation": { - "quaternion": { - "W": -0.7071067811865475, - "X": -0.0, - "Y": 0.0, - "Z": 0.7071067811865476 - } - } - } - }, - { - "ID": 7, - "pose": { - "translation": { - "x": -0.038099999999999995, - "y": 5.547867999999999, - "z": 1.4511020000000001 - }, - "rotation": { - "quaternion": { - "W": 1.0, - "X": 0.0, - "Y": 0.0, - "Z": 0.0 - } - } - } - }, - { - "ID": 8, - "pose": { - "translation": { - "x": -0.038099999999999995, - "y": 4.982717999999999, - "z": 1.4511020000000001 - }, - "rotation": { - "quaternion": { - "W": 1.0, - "X": 0.0, - "Y": 0.0, - "Z": 0.0 - } - } - } - } - ], - "field": { - "length": 16.541, - "width": 8.211 - } -} diff --git a/vendordeps/ReduxLib-2025.0.0.json b/vendordeps/ReduxLib-2025.0.0.json deleted file mode 100644 index 18ff5a1..0000000 --- a/vendordeps/ReduxLib-2025.0.0.json +++ /dev/null @@ -1,72 +0,0 @@ -{ - "fileName": "ReduxLib-2025.0.0.json", - "name": "ReduxLib", - "version": "2025.0.0", - "frcYear": "2025", - "uuid": "151ecca8-670b-4026-8160-cdd2679ef2bd", - "mavenUrls": [ - "https://maven.reduxrobotics.com/" - ], - "jsonUrl": "https://frcsdk.reduxrobotics.com/ReduxLib_2025.json", - "javaDependencies": [ - { - "groupId": "com.reduxrobotics.frc", - "artifactId": "ReduxLib-java", - "version": "2025.0.0" - } - ], - "jniDependencies": [ - { - "groupId": "com.reduxrobotics.frc", - "artifactId": "ReduxLib-driver", - "version": "2025.0.0", - "isJar": false, - "skipInvalidPlatforms": true, - "validPlatforms": [ - "linuxathena", - "linuxx86-64", - "linuxarm32", - "linuxarm64", - "osxuniversal", - "windowsx86-64" - ] - } - ], - "cppDependencies": [ - { - "groupId": "com.reduxrobotics.frc", - "artifactId": "ReduxLib-cpp", - "version": "2025.0.0", - "libName": "ReduxLib", - "headerClassifier": "headers", - "sourcesClassifier": "sources", - "sharedLibrary": true, - "skipInvalidPlatforms": true, - "binaryPlatforms": [ - "linuxathena", - "linuxx86-64", - "linuxarm32", - "linuxarm64", - "osxuniversal", - "windowsx86-64" - ] - }, - { - "groupId": "com.reduxrobotics.frc", - "artifactId": "ReduxLib-driver", - "version": "2025.0.0", - "libName": "ReduxCore", - "headerClassifier": "headers", - "sharedLibrary": true, - "skipInvalidPlatforms": true, - "binaryPlatforms": [ - "linuxathena", - "linuxx86-64", - "linuxarm32", - "linuxarm64", - "osxuniversal", - "windowsx86-64" - ] - } - ] -} diff --git a/vendordeps/Studica-2025.0.0.json b/vendordeps/Studica-2025.0.0.json deleted file mode 100644 index 8cba530..0000000 --- a/vendordeps/Studica-2025.0.0.json +++ /dev/null @@ -1,71 +0,0 @@ -{ - "fileName": "Studica-2025.0.0.json", - "name": "Studica", - "version": "2025.0.0", - "uuid": "cb311d09-36e9-4143-a032-55bb2b94443b", - "frcYear": "2025", - "mavenUrls": [ - "https://dev.studica.com/maven/release/2025/" - ], - "jsonUrl": "https://dev.studica.com/releases/2025/Studica-2025.0.0.json", - "cppDependencies": [ - { - "artifactId": "Studica-cpp", - "binaryPlatforms": [ - "linuxathena", - "linuxarm32", - "linuxarm64", - "linuxx86-64", - "osxuniversal", - "windowsx86-64" - ], - "groupId": "com.studica.frc", - "headerClassifier": "headers", - "libName": "Studica", - "sharedLibrary": false, - "skipInvalidPlatforms": true, - "version": "2025.0.0" - }, - { - "artifactId": "Studica-driver", - "binaryPlatforms": [ - "linuxathena", - "linuxarm32", - "linuxarm64", - "linuxx86-64", - "osxuniversal", - "windowsx86-64" - ], - "groupId": "com.studica.frc", - "headerClassifier": "headers", - "libName": "StudicaDriver", - "sharedLibrary": false, - "skipInvalidPlatforms": true, - "version": "2025.0.0" - } - ], - "javaDependencies": [ - { - "artifactId": "Studica-java", - "groupId": "com.studica.frc", - "version": "2025.0.0" - } - ], - "jniDependencies": [ - { - "artifactId": "Studica-driver", - "groupId": "com.studica.frc", - "isJar": false, - "skipInvalidPlatforms": true, - "validPlatforms": [ - "linuxathena", - "linuxarm32", - "linuxarm64", - "linuxx86-64", - "osxuniversal", - "windowsx86-64" - ], - "version": "2025.0.0" - } - ] -} diff --git a/vendordeps/yagsl-2025.1.3.jwt151.json b/vendordeps/yagsl-2025.1.3.jwt151.json deleted file mode 100644 index 97dc57e..0000000 --- a/vendordeps/yagsl-2025.1.3.jwt151.json +++ /dev/null @@ -1,64 +0,0 @@ -{ - "fileName": "yagsl-2025.1.3.jwt151.json", - "name": "YAGSL", - "version": "2025.1.3.jwt151", - "frcYear": "2025", - "uuid": "1ccce5a4-acd2-4d18-bca3-4b8047188400", - "mavenUrls": [ - "https://broncbotz3481.github.io/YAGSL-Lib/yagsl/repos" - ], - "jsonUrl": "https://broncbotz3481.github.io/YAGSL-Lib/yagsl/yagsl.json", - "javaDependencies": [ - { - "groupId": "swervelib", - "artifactId": "YAGSL-java", - "version": "2025.1.3.jwt151" - } - ], - "requires": [ - { - "uuid": "3f48eb8c-50fe-43a6-9cb7-44c86353c4cb", - "errorMessage": "REVLib is required!", - "offlineFileName": "REVLib-2025.0.0.json", - "onlineUrl": "https://software-metadata.revrobotics.com/REVLib-2025.json" - }, - { - "uuid": "151ecca8-670b-4026-8160-cdd2679ef2bd", - "errorMessage": "ReduxLib is required!", - "offlineFileName": "ReduxLib-2025.0.0.json", - "onlineUrl": "https://frcsdk.reduxrobotics.com/ReduxLib_2025.json" - }, - { - "uuid": "e995de00-2c64-4df5-8831-c1441420ff19", - "errorMessage": "Phoenix6 is required!", - "offlineFileName": "Phoenix6-25.1.0.json", - "onlineUrl": "https://maven.ctr-electronics.com/release/com/ctre/phoenix6/latest/Phoenix6-frc2025-latest.json" - }, - { - "uuid": "ab676553-b602-441f-a38d-f1296eff6537", - "errorMessage": "Phoenix5 is required!", - "offlineFileName": "Phoenix5-5.35.0.json", - "onlineUrl": "https://maven.ctr-electronics.com/release/com/ctre/phoenix/Phoenix5-frc2025-latest.json" - }, - { - "uuid": "cb311d09-36e9-4143-a032-55bb2b94443b", - "errorMessage": "NavX is required!", - "offlineFileName": "Studica-2025.0.0.json", - "onlineUrl": "https://dev.studica.com/releases/2025/Studica-2025.0.0.json" - }, - { - "uuid": "60b2694b-9e6e-4026-81ee-6f167946f4b0", - "errorMessage": "ThriftyLib is required!", - "offlineFileName": "ThriftyLib.json", - "onlineUrl": "https://docs.home.thethriftybot.com/ThriftyLib.json" - }, - { - "uuid": "c39481e8-4a63-4a4c-9df6-48d91e4da37b", - "errorMessage": "MapleSim is requires for simulation", - "offlineFileName": "maple-sim.json", - "onlineUrl": "https://shenzhen-robotics-alliance.github.io/maple-sim/vendordep/maple-sim.json" - } - ], - "jniDependencies": [], - "cppDependencies": [] -} From 907648e73ea43c5a8ac1d1f1de8d6ee395cbc448 Mon Sep 17 00:00:00 2001 From: AquaShock211 <143662909+AquaShock211@users.noreply.github.com> Date: Thu, 30 Jan 2025 16:24:34 -0700 Subject: [PATCH 12/15] Why Does Pathplanner not pathplan --- src/main/deploy/Choreo/Cherio Matey.chor | 78 +++++++++++++++++++ src/main/deploy/Choreo/Cherio Matey.traj | 49 ++++++++++++ .../pathplanner/autos/Cherio Matey.auto | 19 +++++ .../deploy/pathplanner/autos/Test 4ft.auto | 2 +- .../pathplanner/paths/Consistancy Test.path | 4 +- src/main/java/frc/robot/Constants.java | 2 +- src/main/java/frc/robot/util/RBSIEnum.java | 8 +- 7 files changed, 154 insertions(+), 8 deletions(-) create mode 100644 src/main/deploy/Choreo/Cherio Matey.chor create mode 100644 src/main/deploy/Choreo/Cherio Matey.traj create mode 100644 src/main/deploy/pathplanner/autos/Cherio Matey.auto diff --git a/src/main/deploy/Choreo/Cherio Matey.chor b/src/main/deploy/Choreo/Cherio Matey.chor new file mode 100644 index 0000000..15488be --- /dev/null +++ b/src/main/deploy/Choreo/Cherio Matey.chor @@ -0,0 +1,78 @@ +{ + "name":"Cherio Matey", + "version":1, + "type":"Swerve", + "variables":{ + "expressions":{}, + "poses":{} + }, + "config":{ + "frontLeft":{ + "x":{ + "exp":"11 in", + "val":0.2794 + }, + "y":{ + "exp":"11 in", + "val":0.2794 + } + }, + "backLeft":{ + "x":{ + "exp":"-11 in", + "val":-0.2794 + }, + "y":{ + "exp":"11 in", + "val":0.2794 + } + }, + "mass":{ + "exp":"150 lbs", + "val":68.0388555 + }, + "inertia":{ + "exp":"6 kg m ^ 2", + "val":6.0 + }, + "gearing":{ + "exp":"6.5", + "val":6.5 + }, + "radius":{ + "exp":"2 in", + "val":0.0508 + }, + "vmax":{ + "exp":"6000 RPM", + "val":628.3185307179587 + }, + "tmax":{ + "exp":"1.2 N * m", + "val":1.2 + }, + "cof":{ + "exp":"1.5", + "val":1.5 + }, + "bumper":{ + "front":{ + "exp":"16 in", + "val":0.4064 + }, + "side":{ + "exp":"16 in", + "val":0.4064 + }, + "back":{ + "exp":"16 in", + "val":0.4064 + } + }, + "differentialTrackWidth":{ + "exp":"22 in", + "val":0.5588 + } + }, + "generationFeatures":[] +} diff --git a/src/main/deploy/Choreo/Cherio Matey.traj b/src/main/deploy/Choreo/Cherio Matey.traj new file mode 100644 index 0000000..5cf1860 --- /dev/null +++ b/src/main/deploy/Choreo/Cherio Matey.traj @@ -0,0 +1,49 @@ +{ + "name":"Cherio Matey", + "version":1, + "snapshot":{ + "waypoints":[ + {"x":8.258777618408203, "y":6.292977809906006, "heading":0.0, "intervals":14, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, + {"x":9.338869094848633, "y":6.289803981781006, "heading":0.0, "intervals":40, "split":false, "fixTranslation":true, "fixHeading":false, "overrideIntervals":false}], + "constraints":[ + {"from":"first", "to":null, "data":{"type":"StopPoint", "props":{}}, "enabled":true}, + {"from":"last", "to":null, "data":{"type":"StopPoint", "props":{}}, "enabled":true}, + {"from":"first", "to":"last", "data":{"type":"KeepInRectangle", "props":{"x":0.0, "y":0.0, "w":17.548, "h":8.052}}, "enabled":false}], + "targetDt":0.05 + }, + "params":{ + "waypoints":[ + {"x":{"exp":"8.258777618408203 m", "val":8.258777618408203}, "y":{"exp":"6.292977809906006 m", "val":6.292977809906006}, "heading":{"exp":"0 deg", "val":0.0}, "intervals":14, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, + {"x":{"exp":"9.338869094848633 m", "val":9.338869094848633}, "y":{"exp":"6.289803981781006 m", "val":6.289803981781006}, "heading":{"exp":"0 deg", "val":0.0}, "intervals":40, "split":false, "fixTranslation":true, "fixHeading":false, "overrideIntervals":false}], + "constraints":[ + {"from":"first", "to":null, "data":{"type":"StopPoint", "props":{}}, "enabled":true}, + {"from":"last", "to":null, "data":{"type":"StopPoint", "props":{}}, "enabled":true}, + {"from":"first", "to":"last", "data":{"type":"KeepInRectangle", "props":{"x":{"exp":"0 m", "val":0.0}, "y":{"exp":"0 m", "val":0.0}, "w":{"exp":"17.548 m", "val":17.548}, "h":{"exp":"8.052 m", "val":8.052}}}, "enabled":false}], + "targetDt":{ + "exp":"0.05 s", + "val":0.05 + } + }, + "trajectory":{ + "sampleType":"Swerve", + "waypoints":[0.0,0.69238], + "samples":[ + {"t":0.0, "x":8.25878, "y":6.29298, "heading":0.0, "vx":0.0, "vy":0.0, "omega":0.0, "ax":9.01891, "ay":-0.0265, "alpha":0.0, "fx":[153.40916,153.40916,153.40916,153.40916], "fy":[-0.45079,-0.45079,-0.45079,-0.45079]}, + {"t":0.04946, "x":8.26981, "y":6.29295, "heading":0.0, "vx":0.44604, "vy":-0.00131, "omega":0.0, "ax":9.01749, "ay":-0.0265, "alpha":0.0, "fx":[153.38489,153.38489,153.38489,153.38489], "fy":[-0.45072,-0.45072,-0.45072,-0.45072]}, + {"t":0.09891, "x":8.30289, "y":6.29285, "heading":0.0, "vx":0.892, "vy":-0.00262, "omega":0.0, "ax":9.01543, "ay":-0.02649, "alpha":0.0, "fx":[153.34984,153.34984,153.34984,153.34984], "fy":[-0.45062,-0.45062,-0.45062,-0.45062]}, + {"t":0.14837, "x":8.35803, "y":6.29269, "heading":0.0, "vx":1.33787, "vy":-0.00393, "omega":0.0, "ax":9.01219, "ay":-0.02648, "alpha":0.0, "fx":[153.29478,153.29478,153.29478,153.29478], "fy":[-0.45045,-0.45045,-0.45045,-0.45045]}, + {"t":0.19782, "x":8.43522, "y":6.29246, "heading":0.0, "vx":1.78357, "vy":-0.00524, "omega":0.0, "ax":9.00637, "ay":-0.02647, "alpha":0.0, "fx":[153.19571,153.19571,153.19571,153.19571], "fy":[-0.45016,-0.45016,-0.45016,-0.45016]}, + {"t":0.24728, "x":8.53444, "y":6.29217, "heading":0.0, "vx":2.22899, "vy":-0.00655, "omega":0.0, "ax":8.99279, "ay":-0.02643, "alpha":0.0, "fx":[152.96479,152.96479,152.96479,152.96479], "fy":[-0.44948,-0.44948,-0.44948,-0.44948]}, + {"t":0.29673, "x":8.65568, "y":6.29181, "heading":0.0, "vx":2.67373, "vy":-0.00786, "omega":0.0, "ax":8.92521, "ay":-0.02623, "alpha":0.0, "fx":[151.81529,151.81529,151.81529,151.81529], "fy":[-0.44611,-0.44611,-0.44611,-0.44611]}, + {"t":0.34619, "x":8.79882, "y":6.29139, "heading":0.0, "vx":3.11514, "vy":-0.00915, "omega":0.0, "ax":-8.92521, "ay":0.02623, "alpha":0.0, "fx":[-151.81529,-151.81529,-151.81529,-151.81529], "fy":[0.44611,0.44611,0.44611,0.44611]}, + {"t":0.39565, "x":8.94197, "y":6.29097, "heading":0.0, "vx":2.67373, "vy":-0.00786, "omega":0.0, "ax":-8.99279, "ay":0.02643, "alpha":0.0, "fx":[-152.96479,-152.96479,-152.96479,-152.96479], "fy":[0.44948,0.44948,0.44948,0.44948]}, + {"t":0.4451, "x":9.0632, "y":6.29061, "heading":0.0, "vx":2.22899, "vy":-0.00655, "omega":0.0, "ax":-9.00637, "ay":0.02647, "alpha":0.0, "fx":[-153.19571,-153.19571,-153.19571,-153.19571], "fy":[0.45016,0.45016,0.45016,0.45016]}, + {"t":0.49456, "x":9.16243, "y":6.29032, "heading":0.0, "vx":1.78357, "vy":-0.00524, "omega":0.0, "ax":-9.01219, "ay":0.02648, "alpha":0.0, "fx":[-153.29478,-153.29478,-153.29478,-153.29478], "fy":[0.45045,0.45045,0.45045,0.45045]}, + {"t":0.54401, "x":9.23961, "y":6.2901, "heading":0.0, "vx":1.33787, "vy":-0.00393, "omega":0.0, "ax":-9.01543, "ay":0.02649, "alpha":0.0, "fx":[-153.34984,-153.34984,-153.34984,-153.34984], "fy":[0.45062,0.45062,0.45062,0.45062]}, + {"t":0.59347, "x":9.29475, "y":6.28993, "heading":0.0, "vx":0.892, "vy":-0.00262, "omega":0.0, "ax":-9.01749, "ay":0.0265, "alpha":0.0, "fx":[-153.38489,-153.38489,-153.38489,-153.38489], "fy":[0.45072,0.45072,0.45072,0.45072]}, + {"t":0.64292, "x":9.32784, "y":6.28984, "heading":0.0, "vx":0.44604, "vy":-0.00131, "omega":0.0, "ax":-9.01891, "ay":0.0265, "alpha":0.0, "fx":[-153.40916,-153.40916,-153.40916,-153.40916], "fy":[0.45079,0.45079,0.45079,0.45079]}, + {"t":0.69238, "x":9.33887, "y":6.2898, "heading":0.0, "vx":0.0, "vy":0.0, "omega":0.0, "ax":0.0, "ay":0.0, "alpha":0.0, "fx":[0.0,0.0,0.0,0.0], "fy":[0.0,0.0,0.0,0.0]}], + "splits":[0] + }, + "events":[] +} diff --git a/src/main/deploy/pathplanner/autos/Cherio Matey.auto b/src/main/deploy/pathplanner/autos/Cherio Matey.auto new file mode 100644 index 0000000..ba209c6 --- /dev/null +++ b/src/main/deploy/pathplanner/autos/Cherio Matey.auto @@ -0,0 +1,19 @@ +{ + "version": "2025.0", + "command": { + "type": "sequential", + "data": { + "commands": [ + { + "type": "path", + "data": { + "pathName": "Cherio Matey.0" + } + } + ] + } + }, + "resetOdom": true, + "folder": null, + "choreoAuto": true +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/autos/Test 4ft.auto b/src/main/deploy/pathplanner/autos/Test 4ft.auto index 832e9de..1cf0d7a 100644 --- a/src/main/deploy/pathplanner/autos/Test 4ft.auto +++ b/src/main/deploy/pathplanner/autos/Test 4ft.auto @@ -7,7 +7,7 @@ { "type": "path", "data": { - "pathName": "Consistancy Test" + "pathName": "Copy of Consistancy Test" } } ] diff --git a/src/main/deploy/pathplanner/paths/Consistancy Test.path b/src/main/deploy/pathplanner/paths/Consistancy Test.path index f5b104c..5def06a 100644 --- a/src/main/deploy/pathplanner/paths/Consistancy Test.path +++ b/src/main/deploy/pathplanner/paths/Consistancy Test.path @@ -3,12 +3,12 @@ "waypoints": [ { "anchor": { - "x": 9.335298121098802, + "x": 9.325118272569444, "y": 6.184303023726215 }, "prevControl": null, "nextControl": { - "x": 8.416560228412514, + "x": 8.406380379883156, "y": 6.1682387913029775 }, "isLocked": false, diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java index 7137712..9b55cfb 100644 --- a/src/main/java/frc/robot/Constants.java +++ b/src/main/java/frc/robot/Constants.java @@ -136,7 +136,7 @@ public static final class DrivebaseConstants { // case NUTHOUSE -> Units.feetToMeters(1); // case FIELD -> Units.feetToMeters(18); // }; - public static final double kMaxLinearSpeed = Units.feetToMeters(18); + public static final double kMaxLinearSpeed = Units.feetToMeters(30); // Set 3/4 of a rotation per second as the max angular velocity (radians/sec) public static final double kMaxAngularSpeed = 1.5 * Math.PI; diff --git a/src/main/java/frc/robot/util/RBSIEnum.java b/src/main/java/frc/robot/util/RBSIEnum.java index 49c3181..7109e21 100644 --- a/src/main/java/frc/robot/util/RBSIEnum.java +++ b/src/main/java/frc/robot/util/RBSIEnum.java @@ -67,8 +67,8 @@ public static enum MotorIdleMode { BRAKE // Hold motor position when idle } - // public static enum PracticeSpace { - // NUTHOUSE, - // FIELD - // } + // public static enum PracticeSpace { + // NUTHOUSE, + // FIELD + // } } From f04137ff225db8f317aae4222ee47e2667e91ff1 Mon Sep 17 00:00:00 2001 From: NutHouse coco-prog-3 Date: Fri, 31 Jan 2025 13:05:18 -0700 Subject: [PATCH 13/15] Auto work? --- src/main/java/frc/robot/Constants.java | 4 ++-- .../java/frc/robot/subsystems/drive/SwerveConstants.java | 8 ++++++-- 2 files changed, 8 insertions(+), 4 deletions(-) diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java index 9b55cfb..c51a1ec 100644 --- a/src/main/java/frc/robot/Constants.java +++ b/src/main/java/frc/robot/Constants.java @@ -243,8 +243,8 @@ public static class OperatorConstants { public static final class AutoConstants { // Drive and Turn PID constants used for PathPlanner - public static final PIDConstants kPPdrivePID = new PIDConstants(0.0, 0.0, 0.0); - public static final PIDConstants kPPsteerPID = new PIDConstants(0.0, 0.0, 0.0); + public static final PIDConstants kPPdrivePID = new PIDConstants(3.5, 0.0, 0.0); + public static final PIDConstants kPPsteerPID = new PIDConstants(2, 0.0, 0.0); // PathPlanner Config constants public static final RobotConfig kPathPlannerConfig = diff --git a/src/main/java/frc/robot/subsystems/drive/SwerveConstants.java b/src/main/java/frc/robot/subsystems/drive/SwerveConstants.java index cffffbf..f7b866c 100644 --- a/src/main/java/frc/robot/subsystems/drive/SwerveConstants.java +++ b/src/main/java/frc/robot/subsystems/drive/SwerveConstants.java @@ -113,8 +113,12 @@ public class SwerveConstants { kCoupleRatio = TunerConstants.FrontLeft.CouplingGearRatio; kDriveGearRatio = TunerConstants.FrontLeft.DriveMotorGearRatio; kSteerGearRatio = TunerConstants.FrontLeft.SteerMotorGearRatio; - kWheelRadiusMeters = TunerConstants.FrontLeft.WheelRadius; - kWheelRadiusInches = Units.metersToInches(kWheelRadiusMeters); + // kWheelRadiusMeters = TunerConstants.FrontLeft.WheelRadius; + // kWheelRadiusInches = Units.metersToInches(kWheelRadiusMeters); + + kWheelRadiusInches = TunerConstants.FrontLeft.WheelRadius; + kWheelRadiusMeters = Units.inchesToMeters(kWheelRadiusInches); + kCANbusName = TunerConstants.DrivetrainConstants.CANBusName; kPigeonId = TunerConstants.DrivetrainConstants.Pigeon2Id; kSteerInertia = TunerConstants.FrontLeft.SteerInertia; From 72df0d0ca6eafa1eacb095be85e2463c1dd338e0 Mon Sep 17 00:00:00 2001 From: AquaShock211 <143662909+AquaShock211@users.noreply.github.com> Date: Fri, 31 Jan 2025 14:46:09 -0700 Subject: [PATCH 14/15] Tuning --- .../deploy/pathplanner/autos/Ft4 test.auto | 19 +++++++++++++++++++ .../pathplanner/paths/Consistancy Test.path | 18 +++++++++--------- .../paths/Copy of Consistancy Test.path | 4 ++-- src/main/java/frc/robot/Constants.java | 7 ++++--- .../frc/robot/generated/TunerConstants.java | 2 +- 5 files changed, 35 insertions(+), 15 deletions(-) create mode 100644 src/main/deploy/pathplanner/autos/Ft4 test.auto diff --git a/src/main/deploy/pathplanner/autos/Ft4 test.auto b/src/main/deploy/pathplanner/autos/Ft4 test.auto new file mode 100644 index 0000000..832e9de --- /dev/null +++ b/src/main/deploy/pathplanner/autos/Ft4 test.auto @@ -0,0 +1,19 @@ +{ + "version": "2025.0", + "command": { + "type": "sequential", + "data": { + "commands": [ + { + "type": "path", + "data": { + "pathName": "Consistancy Test" + } + } + ] + } + }, + "resetOdom": true, + "folder": null, + "choreoAuto": false +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/Consistancy Test.path b/src/main/deploy/pathplanner/paths/Consistancy Test.path index 5def06a..660bd28 100644 --- a/src/main/deploy/pathplanner/paths/Consistancy Test.path +++ b/src/main/deploy/pathplanner/paths/Consistancy Test.path @@ -3,25 +3,25 @@ "waypoints": [ { "anchor": { - "x": 9.325118272569444, + "x": 9.400179398148147, "y": 6.184303023726215 }, "prevControl": null, "nextControl": { - "x": 8.406380379883156, - "y": 6.1682387913029775 + "x": 8.752472205652685, + "y": 6.17933470428246 }, "isLocked": false, "linkedName": null }, { "anchor": { - "x": 8.218592664930556, + "x": 8.18, "y": 6.184303023726215 }, "prevControl": { - "x": 8.883800254470446, - "y": 6.188077684828391 + "x": 8.642916609125686, + "y": 6.1899481811172015 }, "nextControl": null, "isLocked": false, @@ -33,8 +33,8 @@ "pointTowardsZones": [], "eventMarkers": [], "globalConstraints": { - "maxVelocity": 4.0, - "maxAcceleration": 7.0, + "maxVelocity": 1.0, + "maxAcceleration": 0.5, "maxAngularVelocity": 540.0, "maxAngularAcceleration": 1200.0, "nominalVoltage": 12.0, @@ -50,5 +50,5 @@ "velocity": 0, "rotation": 0.0 }, - "useDefaultConstraints": true + "useDefaultConstraints": false } \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/Copy of Consistancy Test.path b/src/main/deploy/pathplanner/paths/Copy of Consistancy Test.path index 37de65e..5f81cf0 100644 --- a/src/main/deploy/pathplanner/paths/Copy of Consistancy Test.path +++ b/src/main/deploy/pathplanner/paths/Copy of Consistancy Test.path @@ -33,8 +33,8 @@ "pointTowardsZones": [], "eventMarkers": [], "globalConstraints": { - "maxVelocity": 4.0, - "maxAcceleration": 6.0, + "maxVelocity": 1.0, + "maxAcceleration": 0.5, "maxAngularVelocity": 540.0, "maxAngularAcceleration": 1200.0, "nominalVoltage": 12.0, diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java index c51a1ec..fde8589 100644 --- a/src/main/java/frc/robot/Constants.java +++ b/src/main/java/frc/robot/Constants.java @@ -136,7 +136,7 @@ public static final class DrivebaseConstants { // case NUTHOUSE -> Units.feetToMeters(1); // case FIELD -> Units.feetToMeters(18); // }; - public static final double kMaxLinearSpeed = Units.feetToMeters(30); + public static final double kMaxLinearSpeed = Units.feetToMeters(6); // Set 3/4 of a rotation per second as the max angular velocity (radians/sec) public static final double kMaxAngularSpeed = 1.5 * Math.PI; @@ -243,8 +243,9 @@ public static class OperatorConstants { public static final class AutoConstants { // Drive and Turn PID constants used for PathPlanner - public static final PIDConstants kPPdrivePID = new PIDConstants(3.5, 0.0, 0.0); - public static final PIDConstants kPPsteerPID = new PIDConstants(2, 0.0, 0.0); + public static final PIDConstants kPPdrivePID = new PIDConstants(1.9, 0.0, 0.0); + public static final PIDConstants kPPsteerPID = new PIDConstants(1.9, 0.0, 0.0); + //1 Cordinate = 39.3437945791726 // PathPlanner Config constants public static final RobotConfig kPathPlannerConfig = diff --git a/src/main/java/frc/robot/generated/TunerConstants.java b/src/main/java/frc/robot/generated/TunerConstants.java index 950de1d..587a222 100755 --- a/src/main/java/frc/robot/generated/TunerConstants.java +++ b/src/main/java/frc/robot/generated/TunerConstants.java @@ -88,7 +88,7 @@ public class TunerConstants { private static final double kDriveGearRatio = 6.746031746031747; private static final double kSteerGearRatio = 21.428571428571427; - private static final Distance kWheelRadius = Inches.of(2); + private static final Distance kWheelRadius = Inches.of(1.9875); private static final boolean kInvertLeftSide = false; private static final boolean kInvertRightSide = true; From 07d6620025eaa6c885e734ebb242430114c801c9 Mon Sep 17 00:00:00 2001 From: AquaShock211 <143662909+AquaShock211@users.noreply.github.com> Date: Fri, 31 Jan 2025 17:30:38 -0700 Subject: [PATCH 15/15] Add a measurment for field coordinates to inches --- src/main/deploy/pathplanner/paths/Consistancy Test.path | 2 +- src/main/deploy/pathplanner/paths/Copy of Consistancy Test.path | 2 +- src/main/java/frc/robot/Constants.java | 2 +- 3 files changed, 3 insertions(+), 3 deletions(-) diff --git a/src/main/deploy/pathplanner/paths/Consistancy Test.path b/src/main/deploy/pathplanner/paths/Consistancy Test.path index 660bd28..9172702 100644 --- a/src/main/deploy/pathplanner/paths/Consistancy Test.path +++ b/src/main/deploy/pathplanner/paths/Consistancy Test.path @@ -34,7 +34,7 @@ "eventMarkers": [], "globalConstraints": { "maxVelocity": 1.0, - "maxAcceleration": 0.5, + "maxAcceleration": 4.0, "maxAngularVelocity": 540.0, "maxAngularAcceleration": 1200.0, "nominalVoltage": 12.0, diff --git a/src/main/deploy/pathplanner/paths/Copy of Consistancy Test.path b/src/main/deploy/pathplanner/paths/Copy of Consistancy Test.path index 5f81cf0..f11cc56 100644 --- a/src/main/deploy/pathplanner/paths/Copy of Consistancy Test.path +++ b/src/main/deploy/pathplanner/paths/Copy of Consistancy Test.path @@ -34,7 +34,7 @@ "eventMarkers": [], "globalConstraints": { "maxVelocity": 1.0, - "maxAcceleration": 0.5, + "maxAcceleration": 4.0, "maxAngularVelocity": 540.0, "maxAngularAcceleration": 1200.0, "nominalVoltage": 12.0, diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java index fde8589..1cf0e26 100644 --- a/src/main/java/frc/robot/Constants.java +++ b/src/main/java/frc/robot/Constants.java @@ -245,7 +245,7 @@ public static final class AutoConstants { // Drive and Turn PID constants used for PathPlanner public static final PIDConstants kPPdrivePID = new PIDConstants(1.9, 0.0, 0.0); public static final PIDConstants kPPsteerPID = new PIDConstants(1.9, 0.0, 0.0); - //1 Cordinate = 39.3437945791726 + // 1 Cordinate = 39.3437945791726 // PathPlanner Config constants public static final RobotConfig kPathPlannerConfig =