Merge remote-tracking branch 'refs/remotes/origin/main'

Gongoliers · Apr 25, 2024 · e7508c7 · e7508c7
2 parents 8d1cb5c + 73873fe
commit e7508c7
Show file tree

Hide file tree

Showing 15 changed files with 301 additions and 14 deletions.
diff --git a/simgui.json b/simgui.json
@@ -34,6 +34,12 @@
         }
       },
       "/Shuffleboard/Odometry/Field/Field": {
+        "bottom": 1476,
+        "height": 8.210550308227539,
+        "left": 150,
+        "right": 2961,
+        "top": 79,
+        "width": 16.541748046875,
         "window": {
           "visible": true
         }

diff --git a/src/main/deploy/pathplanner/autos/cmp.elims.987.auto b/src/main/deploy/pathplanner/autos/cmp.elims.987.auto
@@ -0,0 +1,38 @@
+{
+  "version": 1.0,
+  "startingPose": {
+    "position": {
+      "x": 1.3842846515935527,
+      "y": 1.5341195874973776
+    },
+    "rotation": 0
+  },
+  "command": {
+    "type": "sequential",
+    "data": {
+      "commands": [
+        {
+          "type": "parallel",
+          "data": {
+            "commands": [
+              {
+                "type": "named",
+                "data": {
+                  "name": "intake"
+                }
+              },
+              {
+                "type": "path",
+                "data": {
+                  "pathName": "clean.source.race7"
+                }
+              }
+            ]
+          }
+        }
+      ]
+    }
+  },
+  "folder": null,
+  "choreoAuto": false
+}
diff --git a/src/main/deploy/pathplanner/autos/cmp.source.sweep.auto b/src/main/deploy/pathplanner/autos/cmp.source.sweep.auto
@@ -0,0 +1,57 @@
+{
+  "version": 1.0,
+  "startingPose": {
+    "position": {
+      "x": 1.3842846515935527,
+      "y": 1.5341195874973776
+    },
+    "rotation": 0
+  },
+  "command": {
+    "type": "sequential",
+    "data": {
+      "commands": [
+        {
+          "type": "parallel",
+          "data": {
+            "commands": [
+              {
+                "type": "path",
+                "data": {
+                  "pathName": "clean.source.race8"
+                }
+              },
+              {
+                "type": "named",
+                "data": {
+                  "name": "intake"
+                }
+              }
+            ]
+          }
+        },
+        {
+          "type": "parallel",
+          "data": {
+            "commands": [
+              {
+                "type": "named",
+                "data": {
+                  "name": "shoot"
+                }
+              },
+              {
+                "type": "path",
+                "data": {
+                  "pathName": "clean.mid.sweep"
+                }
+              }
+            ]
+          }
+        }
+      ]
+    }
+  },
+  "folder": null,
+  "choreoAuto": false
+}
diff --git a/src/main/deploy/pathplanner/paths/clean.center.to1.path b/src/main/deploy/pathplanner/paths/clean.center.to1.path
@@ -8,8 +8,8 @@
       },
       "prevControl": null,
       "nextControl": {
-        "x": 1.8962114847306633,
-        "y": 6.452980598268889
+        "x": 1.44283883432246,
+        "y": 6.253586715447326
       },
       "isLocked": false,
       "linkedName": "center.subwoofer"
@@ -20,8 +20,8 @@
         "y": 6.8157068696448375
       },
       "prevControl": {
-        "x": 1.9815373154284097,
-        "y": 6.511225119454518
+        "x": 1.618501382509183,
+        "y": 6.218454205809981
       },
       "nextControl": null,
       "isLocked": false,

diff --git a/src/main/deploy/pathplanner/paths/clean.mid.sweep.path b/src/main/deploy/pathplanner/paths/clean.mid.sweep.path
@@ -0,0 +1,52 @@
+{
+  "version": 1.0,
+  "waypoints": [
+    {
+      "anchor": {
+        "x": 7.907220607593853,
+        "y": 0.7846260485673593
+      },
+      "prevControl": null,
+      "nextControl": {
+        "x": 8.574738290703399,
+        "y": 1.1242403083950245
+      },
+      "isLocked": false,
+      "linkedName": null
+    },
+    {
+      "anchor": {
+        "x": 8.352232396333552,
+        "y": 5.785153253616065
+      },
+      "prevControl": {
+        "x": 8.235124030875737,
+        "y": 5.293298118693242
+      },
+      "nextControl": null,
+      "isLocked": false,
+      "linkedName": null
+    }
+  ],
+  "rotationTargets": [],
+  "constraintZones": [],
+  "eventMarkers": [],
+  "globalConstraints": {
+    "maxVelocity": 4.0,
+    "maxAcceleration": 4.0,
+    "maxAngularVelocity": 540.0,
+    "maxAngularAcceleration": 720.0
+  },
+  "goalEndState": {
+    "velocity": 0,
+    "rotation": 51.115503566285454,
+    "rotateFast": false
+  },
+  "reversed": false,
+  "folder": null,
+  "previewStartingState": {
+    "rotation": 0,
+    "velocity": 0
+  },
+  "useDefaultConstraints": true
+}
diff --git a/src/main/deploy/pathplanner/paths/clean.source.race7.path b/src/main/deploy/pathplanner/paths/clean.source.race7.path
@@ -0,0 +1,52 @@
+{
+  "version": 1.0,
+  "waypoints": [
+    {
+      "anchor": {
+        "x": 1.3842846515935527,
+        "y": 1.5692520971347208
+      },
+      "prevControl": null,
+      "nextControl": {
+        "x": 2.8012958736331157,
+        "y": 1.4287220585853442
+      },
+      "isLocked": false,
+      "linkedName": null
+    },
+    {
+      "anchor": {
+        "x": 8.094593992326358,
+        "y": 2.3070347995189566
+      },
+      "prevControl": {
+        "x": 7.005486193568678,
+        "y": 1.8386013376876962
+      },
+      "nextControl": null,
+      "isLocked": false,
+      "linkedName": null
+    }
+  ],
+  "rotationTargets": [],
+  "constraintZones": [],
+  "eventMarkers": [],
+  "globalConstraints": {
+    "maxVelocity": 4.0,
+    "maxAcceleration": 4.0,
+    "maxAngularVelocity": 540.0,
+    "maxAngularAcceleration": 720.0
+  },
+  "goalEndState": {
+    "velocity": 0,
+    "rotation": 24.676863170336997,
+    "rotateFast": false
+  },
+  "reversed": false,
+  "folder": null,
+  "previewStartingState": {
+    "rotation": 0,
+    "velocity": 0
+  },
+  "useDefaultConstraints": true
+}
diff --git a/src/main/deploy/pathplanner/paths/clean.source.race8.path b/src/main/deploy/pathplanner/paths/clean.source.race8.path
@@ -0,0 +1,52 @@
+{
+  "version": 1.0,
+  "waypoints": [
+    {
+      "anchor": {
+        "x": 1.325730468864645,
+        "y": 1.545830424043158
+      },
+      "prevControl": null,
+      "nextControl": {
+        "x": 2.496814123442796,
+        "y": 1.3350353662190912
+      },
+      "isLocked": false,
+      "linkedName": null
+    },
+    {
+      "anchor": {
+        "x": 7.965774790322761,
+        "y": 0.7846260485673593
+      },
+      "prevControl": {
+        "x": 6.911799501202425,
+        "y": 0.9134452505709579
+      },
+      "nextControl": null,
+      "isLocked": false,
+      "linkedName": null
+    }
+  ],
+  "rotationTargets": [],
+  "constraintZones": [],
+  "eventMarkers": [],
+  "globalConstraints": {
+    "maxVelocity": 4.0,
+    "maxAcceleration": 4.0,
+    "maxAngularVelocity": 540.0,
+    "maxAngularAcceleration": 720.0
+  },
+  "goalEndState": {
+    "velocity": 0,
+    "rotation": 0,
+    "rotateFast": false
+  },
+  "reversed": false,
+  "folder": null,
+  "previewStartingState": {
+    "rotation": 0,
+    "velocity": 0
+  },
+  "useDefaultConstraints": true
+}
diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java
@@ -14,6 +14,7 @@
 import frc.robot.shooter.Shooter;
 import frc.robot.superstructure.Superstructure;
 import frc.robot.superstructure.SuperstructureMechanism;
+import frc.robot.superstructure.SuperstructureState;
 import frc.robot.swerve.Swerve;
 
 /** Initializes subsystems and commands. */
@@ -91,6 +92,10 @@ private void configureBindings() {
     operatorController.rightBumper().onTrue(superstructure.podium());
     operatorController.rightTrigger().onTrue(superstructure.subwoofer());
 
+    operatorController.povLeft().onTrue(superstructure.spool(SuperstructureState.SUBWOOFER_PRE));
+    operatorController.povRight().onTrue(superstructure.spool(SuperstructureState.SUBWOOFER));
+    operatorController.povUp().onTrue(superstructure.feed(SuperstructureState.SUBWOOFER));
+
     operatorController.a().onTrue(superstructure.amp());
     operatorController.b().onTrue(superstructure.bloop());
     operatorController.x().onTrue(superstructure.stow());

diff --git a/src/main/java/frc/robot/shooter/ShooterConstants.java b/src/main/java/frc/robot/shooter/ShooterConstants.java
@@ -100,7 +100,7 @@ public static class FlywheelConstants {
     public static final double MAXIMUM_SPEED = 60;
 
     public static final SlewRateLimiter ACCELERATION_LIMITER =
-        new SlewRateLimiter(MotionProfileCalculator.calculateAcceleration(MAXIMUM_SPEED, 0.25));
+        new SlewRateLimiter(MotionProfileCalculator.calculateAcceleration(MAXIMUM_SPEED, 0.3));
 
     public static final double TOLERANCE = 5;
   }

diff --git a/src/main/java/frc/robot/shooter/ShooterState.java b/src/main/java/frc/robot/shooter/ShooterState.java
@@ -17,6 +17,9 @@ public record ShooterState(
 
   public static final ShooterState EJECT = new ShooterState(0, SerializerConstants.EJECT_SPEED);
 
+  public static final ShooterState SUBWOOFER_PRE =
+      new ShooterState(FlywheelConstants.SPEAKER_SPEED * 0.5, SerializerConstants.FAST_FEED_SPEED);
+
   public static final ShooterState SUBWOOFER =
       new ShooterState(FlywheelConstants.SPEAKER_SPEED, SerializerConstants.FAST_FEED_SPEED);
 

diff --git a/src/main/java/frc/robot/superstructure/Superstructure.java b/src/main/java/frc/robot/superstructure/Superstructure.java
@@ -209,4 +209,15 @@ public Command eject() {
         .andThen(hold(SuperstructureState.EJECT))
         .withName("EJECT");
   }
+
+  public Command spool(SuperstructureState shot) {
+    return pull(shot)
+      .andThen(ready(shot));
+  }
+
+  public Command feed(SuperstructureState shot) {
+    return ready(shot)
+      .andThen(Commands.waitSeconds(SuperstructureConstants.READY_PAUSE_DURATION))
+      .andThen(hold(shot));
+  }
 }
diff --git a/src/main/java/frc/robot/superstructure/SuperstructureConstants.java b/src/main/java/frc/robot/superstructure/SuperstructureConstants.java
@@ -2,7 +2,7 @@
 
 public class SuperstructureConstants {
 
-  public static final double PULL_DURATION = 0.15;
+  public static final double PULL_DURATION = 0.2;
 
   public static final double READY_DURATION = 1.0;
 

diff --git a/src/main/java/frc/robot/superstructure/SuperstructureState.java b/src/main/java/frc/robot/superstructure/SuperstructureState.java
@@ -21,8 +21,11 @@ public record SuperstructureState(
   public static final SuperstructureState EJECT =
       new SuperstructureState(ArmState.EJECT, IntakeState.EJECT, ShooterState.EJECT);
 
-  public static final SuperstructureState SUBWOOFER =
-      new SuperstructureState(ArmState.SUBWOOFER, IntakeState.IDLE, ShooterState.SUBWOOFER);
+  public static final SuperstructureState SUBWOOFER_PRE =
+      new SuperstructureState(ArmState.SUBWOOFER, IntakeState.IDLE, ShooterState.SUBWOOFER_PRE);
+
+public static final SuperstructureState SUBWOOFER =
+    new SuperstructureState(ArmState.SUBWOOFER, IntakeState.IDLE, ShooterState.SUBWOOFER);
 
   public static final SuperstructureState PODIUM =
       new SuperstructureState(ArmState.PODIUM, IntakeState.IDLE, ShooterState.PODIUM);

diff --git a/src/main/java/frc/robot/swerve/SteerMotorPIDF.java b/src/main/java/frc/robot/swerve/SteerMotorPIDF.java
@@ -41,7 +41,15 @@ public boolean atSetpoint() {
   public double calculate(Rotation2d measurement, Rotation2d setpoint) {
     double feedbackVolts = feedback.calculate(measurement.getRotations(), setpoint.getRotations());
 
-    double feedforwardVolts = feedforward.calculate(0.0);
+    double feedforwardVolts = 0.0;
+
+    if (feedback.atSetpoint() == false) {
+      if (measurement.getRadians() > setpoint.getRadians()) {
+        feedforwardVolts = feedforward.ks;
+      } else {
+        feedforwardVolts = -feedforward.ks;
+      }
+    }
 
     return feedbackVolts + feedforwardVolts;
   }