Merge pull request #8 from Coconuts2486-FRC/Az-RBSI_template_sync_bf2…

…0be4 [bot] Update Robot Code with latest version of Az-RBSI
Coconuts2486-FRC · Feb 3, 2025 · 87fbc45 · 87fbc45
2 parents 2453ce8 + 041c69e
commit 87fbc45
Show file tree

Hide file tree

Showing 9 changed files with 84 additions and 122 deletions.
diff --git a/src/main/deploy/pathplanner/autos/Test.auto 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
+}
diff --git 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.400179398148147,
-        "y": 6.184303023726215
+        "x": 9.35,
+        "y": 6.18
       },
       "prevControl": null,
       "nextControl": {
-        "x": 8.752472205652685,
-        "y": 6.17933470428246
+        "x": 9.141987426415538,
+        "y": 6.318675049056306
       },
       "isLocked": false,
       "linkedName": null
     },
     {
       "anchor": {
         "x": 8.18,
-        "y": 6.184303023726215
+        "y": 6.18
       },
       "prevControl": {
-        "x": 8.642916609125686,
-        "y": 6.1899481811172015
+        "x": 8.56,
+        "y": 6.17
       },
       "nextControl": null,
       "isLocked": false,
@@ -33,15 +33,15 @@
   "pointTowardsZones": [],
   "eventMarkers": [],
   "globalConstraints": {
-    "maxVelocity": 1.0,
-    "maxAcceleration": 4.0,
+    "maxVelocity": 0.5,
+    "maxAcceleration": 1.0,
     "maxAngularVelocity": 540.0,
-    "maxAngularAcceleration": 1200.0,
+    "maxAngularAcceleration": 720.0,
     "nominalVoltage": 12.0,
     "unlimited": false
   },
   "goalEndState": {
-    "velocity": 0.0,
+    "velocity": 0,
     "rotation": 0.0
   },
   "reversed": false,

diff --git a/src/main/deploy/pathplanner/settings.json b/src/main/deploy/pathplanner/settings.json
@@ -1,32 +1,21 @@
 {
-  "robotWidth": 0.813,
-  "robotLength": 0.813,
+  "robotWidth": 0.9,
+  "robotLength": 0.9,
   "holonomicMode": true,
   "pathFolders": [],
   "autoFolders": [],
-  "defaultMaxVel": 4.0,
-  "defaultMaxAccel": 7.0,
+  "defaultMaxVel": 3.0,
+  "defaultMaxAccel": 3.0,
   "defaultMaxAngVel": 540.0,
-  "defaultMaxAngAccel": 1200.0,
-  "defaultNominalVoltage": 12.0,
-  "robotMass": 55.0,
+  "defaultMaxAngAccel": 720.0,
+  "robotMass": 74.088,
   "robotMOI": 6.883,
+  "robotWheelbase": 0.546,
   "robotTrackwidth": 0.546,
   "driveWheelRadius": 0.048,
-  "driveGearing": 6.75,
+  "driveGearing": 5.143,
   "maxDriveSpeed": 5.45,
   "driveMotorType": "krakenX60FOC",
-  "driveCurrentLimit": 50.0,
-  "wheelCOF": 1.1,
-  "flModuleX": 0.263525,
-  "flModuleY": 0.263525,
-  "frModuleX": 0.263525,
-  "frModuleY": -0.263525,
-  "blModuleX": -0.263525,
-  "blModuleY": 0.263525,
-  "brModuleX": -0.263525,
-  "brModuleY": -0.263525,
-  "bumperOffsetX": 0.0,
-  "bumperOffsetY": 0.0,
-  "robotFeatures": []
+  "driveCurrentLimit": 60.0,
+  "wheelCOF": 1.2
 }
diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java
@@ -76,8 +76,6 @@ 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
-
   /** Enumerate the robot types (name your robots here) */
   public static enum RobotType {
     DEVBOT, // Development / Alpha / Practice Bot
@@ -137,12 +135,7 @@ 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 = Units.feetToMeters(6);
+    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;
@@ -249,9 +242,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(1.9, 0.0, 0.0);
-    public static final PIDConstants kPPsteerPID = new PIDConstants(1.9, 0.0, 0.0);
-    // 1 Cordinate = 39.3437945791726
+    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 =

diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java
@@ -35,6 +35,7 @@
 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,32 +194,6 @@ public RobotContainer() {
     configureBindings();
   }
 
-  //   // 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<Waypoint> 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() {
 
@@ -288,10 +263,6 @@ 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()
@@ -351,38 +322,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/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(1.9875);
+  private static final Distance kWheelRadius = Inches.of(2);
 
   private static final boolean kInvertLeftSide = false;
   private static final boolean kInvertRightSide = true;
@@ -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/Module.java b/src/main/java/frc/robot/subsystems/drive/Module.java
@@ -59,7 +59,7 @@ public void periodic() {
     int sampleCount = inputs.odometryTimestamps.length; // All signals are sampled together
     odometryPositions = new SwerveModulePosition[sampleCount];
     for (int i = 0; i < sampleCount; i++) {
-      double positionMeters = inputs.odometryDrivePositionsRad[i] * kWheelRadiusInches;
+      double positionMeters = inputs.odometryDrivePositionsRad[i] * kWheelRadiusMeters;
       Rotation2d angle = inputs.odometryTurnPositions[i];
       odometryPositions[i] = new SwerveModulePosition(positionMeters, angle);
     }

diff --git a/src/main/java/frc/robot/subsystems/drive/SwerveConstants.java b/src/main/java/frc/robot/subsystems/drive/SwerveConstants.java
@@ -113,12 +113,8 @@ public class SwerveConstants {
         kCoupleRatio = TunerConstants.FrontLeft.CouplingGearRatio;
         kDriveGearRatio = TunerConstants.FrontLeft.DriveMotorGearRatio;
         kSteerGearRatio = TunerConstants.FrontLeft.SteerMotorGearRatio;
-        // kWheelRadiusMeters = TunerConstants.FrontLeft.WheelRadius;
-        // kWheelRadiusInches = Units.metersToInches(kWheelRadiusMeters);
-
-        kWheelRadiusInches = TunerConstants.FrontLeft.WheelRadius;
-        kWheelRadiusMeters = Units.inchesToMeters(kWheelRadiusInches);
-
+        kWheelRadiusMeters = TunerConstants.FrontLeft.WheelRadius;
+        kWheelRadiusInches = Units.metersToInches(kWheelRadiusMeters);
         kCANbusName = TunerConstants.DrivetrainConstants.CANBusName;
         kPigeonId = TunerConstants.DrivetrainConstants.Pigeon2Id;
         kSteerInertia = TunerConstants.FrontLeft.SteerInertia;

diff --git a/src/main/java/frc/robot/util/RBSIEnum.java b/src/main/java/frc/robot/util/RBSIEnum.java
@@ -59,9 +59,4 @@ public static enum MotorIdleMode {
     COAST, // Allow the motor to coast when idle
     BRAKE // Hold motor position when idle
   }
-
-  //   public static enum PracticeSpace {
-  //     NUTHOUSE,
-  //     FIELD
-  //   }
 }