Revere comp (#100)

- Autonomous - Added "1-Note (Podium) P Center Sweep" routine for Revere playoffs - Updated Note #1 waypoint to account for drivetrain odometry drift - No longer "Drive to Yaw" when scoring from subwoofer - No longer stow arm after intaking piece. Instead, go directly to the scoring arm angle. - Subsystems - Drive - Re-calibrated angle absolute offsets - Re-calibrated wheel diameter - Reverted rotate (aka drive to yaw) PID constants - Added pose2d logging - Arm - Increased amp score arm angle from 80 to 89 degrees - Fixed the position/velocity reported by the relative encoder - Explicitly enable brake mode for the arm - Revert to using absolute encoder target angle for setAngle - LED - Changed color of "intake piece" event from orange to hot pink - Assist - No longer auto rotate to amp - No longer turn off subsystems when transitioning into Autonomous
DevilBotz2876 · Mar 25, 2024 · 327846c · 327846c
1 parent 9574bdd
commit 327846c
Show file tree

Hide file tree

Showing 29 changed files with 246 additions and 83 deletions.
diff --git a/src/main/deploy/pathplanner/autos/1-Note (Podium) P Center Sweep.auto b/src/main/deploy/pathplanner/autos/1-Note (Podium) P Center Sweep.auto
@@ -0,0 +1,37 @@
+{
+  "version": 1.0,
+  "startingPose": {
+    "position": {
+      "x": 0.75,
+      "y": 4.45
+    },
+    "rotation": -60.0
+  },
+  "command": {
+    "type": "sequential",
+    "data": {
+      "commands": [
+        {
+          "type": "named",
+          "data": {
+            "name": "Score from P (Subwoofer Podium-Side)"
+          }
+        },
+        {
+          "type": "named",
+          "data": {
+            "name": "Turn off Shooter and Intake"
+          }
+        },
+        {
+          "type": "path",
+          "data": {
+            "pathName": "P to 8 Center Sweep"
+          }
+        }
+      ]
+    }
+  },
+  "folder": null,
+  "choreoAuto": false
+}
diff --git a/src/main/deploy/pathplanner/paths/1 to 7.path b/src/main/deploy/pathplanner/paths/1 to 7.path
@@ -4,12 +4,12 @@
     {
       "anchor": {
         "x": 2.6,
-        "y": 4.0
+        "y": 3.9
       },
       "prevControl": null,
       "nextControl": {
         "x": 2.6,
-        "y": 3.0
+        "y": 2.9
       },
       "isLocked": true,
       "linkedName": "\"1\" Wing Podium Note"

diff --git a/src/main/deploy/pathplanner/paths/1 to C.path b/src/main/deploy/pathplanner/paths/1 to C.path
@@ -4,12 +4,12 @@
     {
       "anchor": {
         "x": 2.6,
-        "y": 4.0
+        "y": 3.9
       },
       "prevControl": null,
       "nextControl": {
         "x": 2.246446609406726,
-        "y": 4.353553390593275
+        "y": 4.253553390593275
       },
       "isLocked": true,
       "linkedName": "\"1\" Wing Podium Note"

diff --git a/src/main/deploy/pathplanner/paths/7 to 1.path b/src/main/deploy/pathplanner/paths/7 to 1.path
@@ -33,11 +33,11 @@
     {
       "anchor": {
         "x": 2.6,
-        "y": 4.0
+        "y": 3.9
       },
       "prevControl": {
         "x": 2.6,
-        "y": 3.0
+        "y": 2.9
       },
       "nextControl": null,
       "isLocked": false,

diff --git a/src/main/deploy/pathplanner/paths/C to 1.path b/src/main/deploy/pathplanner/paths/C to 1.path
@@ -17,11 +17,11 @@
     {
       "anchor": {
         "x": 2.6,
-        "y": 4.0
+        "y": 3.9
       },
       "prevControl": {
         "x": 1.104281392901949,
-        "y": 3.886748737840739
+        "y": 3.786748737840739
       },
       "nextControl": null,
       "isLocked": false,

diff --git a/src/main/deploy/pathplanner/paths/P to 1.path b/src/main/deploy/pathplanner/paths/P to 1.path
@@ -16,12 +16,12 @@
     },
     {
       "anchor": {
-        "x": 2.6,
-        "y": 4.0
+        "x": 10.0,
+        "y": 2.0
       },
       "prevControl": {
-        "x": 1.6,
-        "y": 4.0
+        "x": 9.0,
+        "y": 1.9999999999999998
       },
       "nextControl": null,
       "isLocked": true,

diff --git a/src/main/deploy/pathplanner/paths/P to 8 Center Sweep.path b/src/main/deploy/pathplanner/paths/P to 8 Center Sweep.path
@@ -0,0 +1,74 @@
+{
+  "version": 1.0,
+  "waypoints": [
+    {
+      "anchor": {
+        "x": 0.75,
+        "y": 4.45
+      },
+      "prevControl": null,
+      "nextControl": {
+        "x": 0.6810660837086954,
+        "y": 1.4507920853690814
+      },
+      "isLocked": false,
+      "linkedName": "\"P\" Subwoofer (Podium)"
+    },
+    {
+      "anchor": {
+        "x": 10.0,
+        "y": 2.0
+      },
+      "prevControl": {
+        "x": 10.0,
+        "y": 1.0
+      },
+      "nextControl": {
+        "x": 10.0,
+        "y": 3.0
+      },
+      "isLocked": false,
+      "linkedName": "\"1\" Wing Podium Note"
+    },
+    {
+      "anchor": {
+        "x": 10.0,
+        "y": 7.0
+      },
+      "prevControl": {
+        "x": 10.0,
+        "y": 6.46
+      },
+      "nextControl": null,
+      "isLocked": false,
+      "linkedName": null
+    }
+  ],
+  "rotationTargets": [
+    {
+      "waypointRelativePos": 0.55,
+      "rotationDegrees": -125.0,
+      "rotateFast": true
+    }
+  ],
+  "constraintZones": [],
+  "eventMarkers": [],
+  "globalConstraints": {
+    "maxVelocity": 4.0,
+    "maxAcceleration": 4.0,
+    "maxAngularVelocity": 540.0,
+    "maxAngularAcceleration": 720.0
+  },
+  "goalEndState": {
+    "velocity": 0,
+    "rotation": -125.0,
+    "rotateFast": false
+  },
+  "reversed": false,
+  "folder": null,
+  "previewStartingState": {
+    "rotation": -60.92861841213601,
+    "velocity": 0
+  },
+  "useDefaultConstraints": false
+}
diff --git a/src/main/deploy/yagsl/inferno/modules/backleft.json b/src/main/deploy/yagsl/inferno/modules/backleft.json
@@ -3,7 +3,7 @@
     "front": -11.375,
     "left": 11.375
   },
-  "absoluteEncoderOffset": 258.398,
+  "absoluteEncoderOffset": 257.783,
   "drive": {
     "type": "sparkmax",
     "id": 12,

diff --git a/src/main/deploy/yagsl/inferno/modules/backright.json b/src/main/deploy/yagsl/inferno/modules/backright.json
@@ -3,7 +3,7 @@
     "front": -11.375,
     "left": -11.375
   },
-  "absoluteEncoderOffset": 132.539,
+  "absoluteEncoderOffset": 131.924,
   "drive": {
     "type": "sparkmax",
     "id": 13,

diff --git a/src/main/deploy/yagsl/inferno/modules/frontleft.json b/src/main/deploy/yagsl/inferno/modules/frontleft.json
@@ -3,7 +3,7 @@
     "front": 11.375,
     "left": 11.375
   },
-  "absoluteEncoderOffset": 274.658,
+  "absoluteEncoderOffset": 272.725,
   "drive": {
     "type": "sparkmax",
     "id": 10,

diff --git a/src/main/deploy/yagsl/inferno/modules/frontright.json b/src/main/deploy/yagsl/inferno/modules/frontright.json
@@ -3,7 +3,7 @@
     "front": 11.375,
     "left": -11.375
   },
-  "absoluteEncoderOffset": 330.117,
+  "absoluteEncoderOffset": 327.832,
   "drive": {
     "type": "sparkmax",
     "id": 11,

diff --git a/src/main/deploy/yagsl/inferno/modules/physicalproperties.json b/src/main/deploy/yagsl/inferno/modules/physicalproperties.json
@@ -7,7 +7,7 @@
   },
   "conversionFactor": {
     "angle": 16.8,
-    "drive": 0.0453715106249528
+    "drive": 0.0446487974601825
   },
   "rampRate": {
     "drive": 0.25,

diff --git a/src/main/java/frc/robot/commands/arm/ArmToPosition.java b/src/main/java/frc/robot/commands/arm/ArmToPosition.java
@@ -34,6 +34,9 @@ public void initialize() {
     arm.setAngle(targetPositionDegrees);
   }
 
+  @Override
+  public void execute() {}
+
   @Override
   public boolean isFinished() {
     if (Math.abs(arm.getAngle() - targetPositionDegrees) <= ArmConstants.pidAngleErrorInDegrees) {

diff --git a/src/main/java/frc/robot/commands/auto/AutoNamedCommands.java b/src/main/java/frc/robot/commands/auto/AutoNamedCommands.java
@@ -93,19 +93,19 @@ class ScorePieceCommand {
             "A (Subwoofer Amp-Side)",
             () -> AutoConstants.scoreFromASubwooferAmpSide.armAngleInDegrees,
             () -> AutoConstants.scoreFromASubwooferAmpSide.shooterVelocityInRPMs,
-            () -> RobotConfig.drive.getAngle()));
+            null));
     commandList.add(
         new ScorePieceCommand(
             "C (Subwoofer Center)",
             () -> AutoConstants.scoreFromCSubwooferCenter.armAngleInDegrees,
             () -> AutoConstants.scoreFromCSubwooferCenter.shooterVelocityInRPMs,
-            () -> RobotConfig.drive.getAngle()));
+            null));
     commandList.add(
         new ScorePieceCommand(
             "P (Subwoofer Podium-Side)",
             () -> AutoConstants.scoreFromPSubwooferPodiumSide.armAngleInDegrees,
             () -> AutoConstants.scoreFromPSubwooferPodiumSide.shooterVelocityInRPMs,
-            () -> RobotConfig.drive.getAngle()));
+            null));
 
     /* Vision Assisted yaw/angle*/
     commandList.add(

diff --git a/src/main/java/frc/robot/commands/auto/AutoPrepareForIntake.java b/src/main/java/frc/robot/commands/auto/AutoPrepareForIntake.java
@@ -25,7 +25,6 @@ public AutoPrepareForIntake(
           new ParallelCommandGroup(new ArmToPosition(arm, intakeAngle)),
           new AutoIndexPiece(intake, intakeVoltage));
     }
-    addCommands(arm.getStowCommand());
     if (Constants.debugCommands) {
       addCommands(new PrintCommand("  END: " + this.getClass().getSimpleName()));
     }

diff --git a/src/main/java/frc/robot/commands/auto/AutoPrepareForIntakeV2.java b/src/main/java/frc/robot/commands/auto/AutoPrepareForIntakeV2.java
@@ -39,8 +39,7 @@ public AutoPrepareForIntakeV2(Arm arm, Intake intake) {
                                 + "Auto Wait For Piece")), // Wait for piece to be detected (with
             // timeout)
             new ParallelCommandGroup(
-                new InstantCommand(() -> RobotConfig.intake.runVoltage(0)), // turn off intake
-                RobotConfig.arm.getStowCommand() // stow arm
+                new InstantCommand(() -> RobotConfig.intake.runVoltage(0)) // , // turn off intake
                 )));
 
     if (Constants.debugCommands) {

diff --git a/src/main/java/frc/robot/commands/auto/AutoScore.java b/src/main/java/frc/robot/commands/auto/AutoScore.java
@@ -47,10 +47,14 @@ public AutoScore(
     addCommands(
         new ParallelCommandGroup(
             new DriveToYaw(drive, robotYawInDegrees)
-                .withTimeout(DriveConstants.pidTimeoutInSeconds),
-            new ArmToPosition(arm, armAngleInDegrees).withTimeout(ArmConstants.pidTimeoutInSeconds),
+                .withTimeout(DriveConstants.pidTimeoutInSeconds)
+                .onlyIf(() -> robotYawInDegrees != null),
+            new ArmToPosition(arm, armAngleInDegrees)
+                .withTimeout(ArmConstants.pidTimeoutInSeconds)
+                .onlyIf(() -> armAngleInDegrees != null),
             new SetShooterVelocity(shooter, shooterVelocityInRPMs)
-                .withTimeout(ShooterConstants.pidTimeoutInSeconds)));
+                .withTimeout(ShooterConstants.pidTimeoutInSeconds)
+                .onlyIf(() -> shooterVelocityInRPMs != null)));
     addCommands(new AutoScorePiece(intake, shooter));
 
     if (Constants.debugCommands) {

diff --git a/src/main/java/frc/robot/config/RobotConfig.java b/src/main/java/frc/robot/config/RobotConfig.java
@@ -62,26 +62,25 @@ public static class ArmConstants {
     public static double absolutePositionOffset = 0; /* 0-1 */
     public static double absoluteEncoderInversion = 1; /* 1 for none, -1 to invert */
 
-    public static double pidKp = 0.12935;
+    public static double pidKp = 0.1;
     public static double pidKi = 0.0;
-    public static double pidKd = 0.0060174;
+    public static double pidKd = 0.0;
     public static double pidMaxOutput = 0.4;
     public static double pidMinOutput = -0.4;
 
-    public static double ffKs = 0.03402;
-    public static double ffKv = 0.072343;
-    public static double ffKa = 0.00036498;
-    public static double ffKg = 0.88138;
+    public static double ffKs = 0.0;
+    public static double ffKv = 0.0;
+    public static double ffKa = 0.0;
+    public static double ffKg = 0.1;
 
     public static double pidAngleErrorInDegrees = 2.0;
-    public static double pidSettlingTimeInSeconds = 0.0;
+    public static double pidSettlingTimeInSeconds = 0.1;
     public static double pidTimeoutInSeconds = 3.0;
 
     public static double maxAngleInDegrees = 90.0;
     public static double minAngleInDegrees = 0.0;
     public static double maxVelocityInDegreesPerSecond = 90;
-    public static double maxAccelerationInDegreesPerSecondSquared =
-        maxVelocityInDegreesPerSecond * 16;
+    public static double maxAccelerationInDegreesPerSecondSquared = 720;
 
     public static double intakeAngleInDegrees = 1;
     public static double ejectAngleInDegrees = 15;
@@ -106,7 +105,7 @@ public static class ShooterConstants {
 
     /* PID */
     public static double pidVelocityErrorInRPMS = 300;
-    public static double pidSettlingTimeInSeconds = 0.0;
+    public static double pidSettlingTimeInSeconds = 0.1;
     public static double pidTimeoutInSeconds = 2.0;
     public static double pidKp = 0.043566;
     public static double pidKi = 0.0;

diff --git a/src/main/java/frc/robot/config/RobotConfigInferno.java b/src/main/java/frc/robot/config/RobotConfigInferno.java
@@ -30,12 +30,11 @@ public RobotConfigInferno() {
     super(false, false, false, false, false, false, false);
 
     // Inferno has a Swerve drive train
-    // TODO: set DriveConstants.maxVelocityMetersPerSec
-    DriveConstants.rotatePidKp = 0.015;
+    DriveConstants.rotatePidKp = 0.02;
     DriveConstants.rotatePidKi = 0.0;
-    DriveConstants.rotatePidKd = 0.001;
+    DriveConstants.rotatePidKd = 0.0;
     DriveConstants.rotatePidErrorInDegrees = 0.5;
-    DriveConstants.pidTimeoutInSeconds = 2;
+    DriveConstants.pidTimeoutInSeconds = 0.5;
     DriveConstants.pidSettlingTimeInSeconds = 0.5;
 
     drive = new DriveSwerveYAGSL("yagsl/inferno");
@@ -99,7 +98,7 @@ public RobotConfigInferno() {
     ArmConstants.minAngleInDegrees = -1.0;
 
     ArmConstants.intakeAngleInDegrees = 1.5;
-    ArmConstants.ampScoreAngleInDegrees = 80;
+    ArmConstants.ampScoreAngleInDegrees = 89.0;
     ArmConstants.subwooferScoreAngleInDegrees = 9.80;
     ArmConstants.subwooferScoreFromPodiumAngleInDegrees = 33; // min/max = 33.24/34.37
     ArmConstants.noteScoreAngleInDegrees =

diff --git a/src/main/java/frc/robot/controls/DriverControls.java b/src/main/java/frc/robot/controls/DriverControls.java
@@ -400,7 +400,9 @@ public static void setupMainControls(CommandXboxController mainController) {
                 ));
 
     BooleanEvent stateChangedEvent =
-        new BooleanEvent(eventLoop, () -> DevilBotState.stateChanged());
+        new BooleanEvent(
+            eventLoop,
+            () -> (DevilBotState.stateChanged()) && DevilBotState.getState() != State.AUTO);
 
     Trigger stateChangedEventTrigger = stateChangedEvent.rising().castTo(Trigger::new);
     stateChangedEventTrigger.onTrue(getRobotStateTransitionCommand());

diff --git a/src/main/java/frc/robot/subsystems/arm/Arm.java b/src/main/java/frc/robot/subsystems/arm/Arm.java
@@ -24,7 +24,11 @@ public default double getTargetAngle() {
   }
 
   // sets of the angle of the arm
-  public default void setAngle(double degrees) {}
+  public default void setAngle(double degrees) {
+    setAngle(degrees, 0);
+  }
+
+  public default void setAngle(double degrees, double velocityDegreesPerSecond) {}
 
   public default boolean isAbsoluteEncoderConnected() {
     return true;