Auto development

src/main/java/org/littletonrobotics/frc2024/Constants.java

@@ -27,7 +27,7 @@
  */
 public final class Constants {
   public static final int loopPeriodMs = 20;
-  private static RobotType robotType = RobotType.DEVBOT;
+  private static RobotType robotType = RobotType.SIMBOT;
   public static final boolean tuningMode = true;
 
   private static boolean invalidRobotAlertSent = false;

src/main/java/org/littletonrobotics/frc2024/FieldConstants.java

@@ -86,6 +86,26 @@ public static final class Speaker {
             (bottomLeftSpeaker.getZ() + bottomRightSpeaker.getZ()) / 2.0);
   }
 
+  public static final class Subwoofer {
+    public static Pose2d ampFaceCorner =
+        new Pose2d(
+            Units.inchesToMeters(35.775),
+            Units.inchesToMeters(239.366),
+            Rotation2d.fromDegrees(-120));
+
+    public static Pose2d sourceFaceCorner =
+        new Pose2d(
+            Units.inchesToMeters(35.775),
+            Units.inchesToMeters(197.466),
+            Rotation2d.fromDegrees(120));
+
+    public static Pose2d centerFace =
+        new Pose2d(
+            Units.inchesToMeters(35.775),
+            Units.inchesToMeters(218.416),
+            Rotation2d.fromDegrees(180));
+  }
+
   public static double aprilTagWidth = Units.inchesToMeters(6.50);
   public static AprilTagFieldLayout aprilTags;
 

src/main/java/org/littletonrobotics/frc2024/RobotContainer.java

@@ -202,11 +202,11 @@ public RobotContainer() {
             flywheels,
             flywheels::runRightCharacterizationVolts,
             flywheels::getRightCharacterizationVelocity));
-    autoChooser.addOption("Arm get static current", arm.getStaticCurrent());
+    // autoChooser.addOption("Arm get static current", arm.getStaticCurrent());
 
     AutoCommands autoCommands = new AutoCommands(drive, superstructure);
 
-    autoChooser.addOption("Drive Straight", autoCommands.driveStraight());
+    autoChooser.addOption("Davis Auto", autoCommands.davisEthicalAuto());
 
     // Testing autos paths
     // Function<File, Optional<Command>> trajectoryCommandFactory =

src/main/java/org/littletonrobotics/frc2024/commands/auto/AutoCommands.java

@@ -10,8 +10,8 @@
 import org.littletonrobotics.frc2024.util.trajectory.HolonomicTrajectory;
 
 public class AutoCommands {
-  private Drive drive;
-  private Superstructure superstructure;
+  private final Drive drive;
+  private final Superstructure superstructure;
 
   public AutoCommands(Drive drive, Superstructure superstructure) {
     this.drive = drive;
@@ -38,7 +38,18 @@ private Command reset(String path) {
             RobotState.getInstance().resetPose(AllianceFlipUtil.apply(trajectory.getStartPose())));
   }
 
+  public Command davisEthicalAuto() {
+    return sequence(
+        reset("driveToCenterline4"),
+        path("driveToCenterline4"),
+        path("driveToCenterline3"),
+        path("driveToPodium"));
+  }
+  ;
+
   public Command driveStraight() {
-    return reset("driveStraight").andThen(path("driveStraight"));
+    return reset("driveToCenterline4")
+        .andThen(path("driveToCenterline4"), path("driveToCenterline3"), path("driveToPodium"));
   }
+  ;
 }

src/main/java/org/littletonrobotics/frc2024/subsystems/drive/DriveConstants.java

@@ -29,6 +29,8 @@ public static class KrakenDriveConstants {
                 Units.inchesToMeters(2.0),
                 Units.inchesToMeters(26.0),
                 Units.inchesToMeters(26.0),
+                Units.inchesToMeters(37),
+                Units.inchesToMeters(33),
                 Units.feetToMeters(13.05),
                 Units.feetToMeters(30.02),
                 8.86,
@@ -38,17 +40,19 @@ public static class KrakenDriveConstants {
                 Units.inchesToMeters(2.0),
                 Units.inchesToMeters(26.0),
                 Units.inchesToMeters(26.0),
+                Units.inchesToMeters(37),
+                Units.inchesToMeters(33),
                 Units.feetToMeters(12.16),
                 Units.feetToMeters(21.32),
                 7.93,
                 29.89);
       };
   public static final Translation2d[] moduleTranslations =
       new Translation2d[] {
-        new Translation2d(driveConfig.trackwidthX() / 2.0, driveConfig.trackwidthY() / 2.0),
-        new Translation2d(driveConfig.trackwidthX() / 2.0, -driveConfig.trackwidthY() / 2.0),
-        new Translation2d(-driveConfig.trackwidthX() / 2.0, driveConfig.trackwidthY() / 2.0),
-        new Translation2d(-driveConfig.trackwidthX() / 2.0, -driveConfig.trackwidthY() / 2.0)
+        new Translation2d(driveConfig.trackWidthX() / 2.0, driveConfig.trackWidthY() / 2.0),
+        new Translation2d(driveConfig.trackWidthX() / 2.0, -driveConfig.trackWidthY() / 2.0),
+        new Translation2d(-driveConfig.trackWidthX() / 2.0, driveConfig.trackWidthY() / 2.0),
+        new Translation2d(-driveConfig.trackWidthX() / 2.0, -driveConfig.trackWidthY() / 2.0)
       };
   public static final SwerveDriveKinematics kinematics =
       new SwerveDriveKinematics(moduleTranslations);
@@ -175,14 +179,16 @@ public static class KrakenDriveConstants {
 
   public record DriveConfig(
       double wheelRadius,
-      double trackwidthX,
-      double trackwidthY,
+      double trackWidthX,
+      double trackWidthY,
+      double bumperWidthX,
+      double bumperWidthY,
       double maxLinearVelocity,
       double maxLinearAcceleration,
       double maxAngularVelocity,
       double maxAngularAcceleration) {
     public double driveBaseRadius() {
-      return Math.hypot(trackwidthX / 2.0, trackwidthY / 2.0);
+      return Math.hypot(trackWidthX / 2.0, trackWidthY / 2.0);
     }
   }
 

src/main/java/org/littletonrobotics/frc2024/subsystems/drive/trajectory/DriveTrajectories.java

@@ -4,37 +4,97 @@
 
 import edu.wpi.first.math.geometry.Pose2d;
 import edu.wpi.first.math.geometry.Rotation2d;
+import edu.wpi.first.math.geometry.Transform2d;
+import edu.wpi.first.math.geometry.Translation2d;
 import edu.wpi.first.math.util.Units;
 import java.util.HashMap;
 import java.util.List;
 import java.util.Map;
 import lombok.experimental.ExtensionMethod;
+import org.littletonrobotics.frc2024.FieldConstants;
+import org.littletonrobotics.frc2024.subsystems.drive.DriveConstants;
 
 @ExtensionMethod({TrajectoryGenerationHelpers.class})
 public class DriveTrajectories {
   public static final Map<String, List<PathSegment>> paths = new HashMap<>();
 
+  // Starting locations
+  private static final Pose2d startingAmpFace =
+      FieldConstants.Subwoofer.ampFaceCorner.transformBy(
+          new Transform2d(
+              -DriveConstants.driveConfig.bumperWidthX() / 2,
+              -DriveConstants.driveConfig.bumperWidthY() / 2,
+              new Rotation2d()));
+  private static final Pose2d startingSourceFace =
+      FieldConstants.Subwoofer.sourceFaceCorner.transformBy(
+          new Transform2d(
+              -DriveConstants.driveConfig.bumperWidthX() / 2,
+              DriveConstants.driveConfig.bumperWidthY() / 2,
+              new Rotation2d()));
+  private static final Pose2d startingCenterFace =
+      FieldConstants.Subwoofer.centerFace.transformBy(
+          new Transform2d(DriveConstants.driveConfig.bumperWidthX() / 2, 0, new Rotation2d(0)));
+
+  // Center intake locations
+  private static final double intakeOffset = 0.5;
+  private static final Pose2d[] intakingPoses = new Pose2d[5];
+
   static {
+    // Find locations for intaking centerline gamepieces
+    for (int i = 0; i < 5; i++) {
+      Translation2d centerLineTranslation =
+          FieldConstants.StagingLocations.centerlineTranslations[i];
+      intakingPoses[i] =
+          new Pose2d(
+              centerLineTranslation.minus(new Translation2d(intakeOffset, 0)),
+              new Rotation2d(Math.PI));
+    }
+
+    // Add paths
     paths.put(
-        "driveStraight",
+        "driveToCenterline4",
         List.of(
             PathSegment.newBuilder()
+                .addPoseWaypoint(startingSourceFace)
+                .addTranslationWaypoint(new Translation2d(3.5, 2.5))
+                .addPoseWaypoint(intakingPoses[0])
+                .addPoseWaypoint(getShootingPose(new Translation2d(3.5, 2.5)))
+                .build()));
+
+    paths.put(
+        "driveToCenterline3",
+        List.of(
+            PathSegment.newBuilder()
+                .addPoseWaypoint(getShootingPose(new Translation2d(3.5, 2.5)))
+                .addTranslationWaypoint(new Translation2d(5.5, 1.4))
+                .addPoseWaypoint(intakingPoses[1])
+                .addTranslationWaypoint(new Translation2d(5.5, 1.4))
+                .addPoseWaypoint(getShootingPose(new Translation2d(3.5, 2.5)))
+                .build()));
+    paths.put(
+        "driveToPodium",
+        List.of(
+            PathSegment.newBuilder()
+                .addPoseWaypoint(getShootingPose(new Translation2d(3.5, 2.5)))
+                .addTranslationWaypoint(new Translation2d(1.6 - Units.inchesToMeters(20), 3.75))
                 .addPoseWaypoint(
                     new Pose2d(
-                        Units.inchesToMeters(60.112),
-                        Units.inchesToMeters(161.638),
-                        new Rotation2d()))
-                .addPoseWaypoint(
-                    new Pose2d(
-                        Units.inchesToMeters(130), Units.inchesToMeters(70), new Rotation2d()))
-                .addPoseWaypoint(
-                    new Pose2d(
-                        Units.inchesToMeters(318.640),
-                        Units.inchesToMeters(29.003),
-                        new Rotation2d()))
-                .addPoseWaypoint(
-                    new Pose2d(
-                        Units.inchesToMeters(130), Units.inchesToMeters(70), new Rotation2d()))
+                        FieldConstants.StagingLocations.spikeTranslations[0].getX()
+                            - Units.inchesToMeters(20),
+                        FieldConstants.StagingLocations.spikeTranslations[0].getY(),
+                        new Rotation2d(Math.PI)))
                 .build()));
   }
+
+  // calculate Pose2d of robot given a translation
+  private static Pose2d getShootingPose(Translation2d translation) {
+    return new Pose2d(
+        translation,
+        FieldConstants.Speaker.centerSpeakerOpening
+            .toTranslation2d()
+            .minus(translation)
+            .getAngle());
+  }
+
+  private DriveTrajectories() {}
 }

src/main/java/org/littletonrobotics/frc2024/subsystems/drive/trajectory/GenerateTrajectories.java

@@ -26,10 +26,10 @@ public static void main(String[] args) {
         VehicleModel.newBuilder()
             .setMass(70)
             .setMoi(6)
-            .setVehicleLength(DriveConstants.driveConfig.trackwidthX())
-            .setVehicleWidth(DriveConstants.driveConfig.trackwidthY())
+            .setVehicleLength(DriveConstants.driveConfig.trackWidthX())
+            .setVehicleWidth(DriveConstants.driveConfig.trackWidthY())
             .setWheelRadius(DriveConstants.driveConfig.wheelRadius())
-            .setMaxWheelTorque(2)
+            .setMaxWheelTorque(6)
             .setMaxWheelOmega(
                 DriveConstants.moduleLimits.maxDriveVelocity()
                     / DriveConstants.driveConfig.wheelRadius())

src/main/java/org/littletonrobotics/frc2024/util/trajectory/HolonomicTrajectory.java

@@ -11,8 +11,11 @@
 import java.io.IOException;
 import java.io.InputStream;
 import java.nio.file.Path;
+import lombok.experimental.ExtensionMethod;
+import org.littletonrobotics.frc2024.subsystems.drive.trajectory.TrajectoryGenerationHelpers;
 import org.littletonrobotics.vehicletrajectoryservice.VehicleTrajectoryServiceOuterClass.VehicleState;
 
+@ExtensionMethod({TrajectoryGenerationHelpers.class})
 public class HolonomicTrajectory {
   private final Trajectory trajectory;
 
@@ -85,18 +88,17 @@ public VehicleState sample(double timeSeconds) {
 
     double s = (timeSeconds - before.getTime()) / (after.getTime() - before.getTime());
 
-    Pose2d beforePose =
-        new Pose2d(
-            before.getState().getX(),
-            before.getState().getY(),
-            new Rotation2d(before.getState().getTheta()));
-    Pose2d afterPose =
-        new Pose2d(
-            after.getState().getX(),
-            after.getState().getY(),
-            new Rotation2d(after.getState().getTheta()));
-
-    Pose2d interpolatedPose = beforePose.interpolate(afterPose, s);
+    double interpolatedPoseX =
+        MathUtil.interpolate(before.getState().getX(), after.getState().getX(), s);
+    double interpolatedPoseY =
+        MathUtil.interpolate(before.getState().getY(), after.getState().getY(), s);
+    Rotation2d interpolatedRotation =
+        before
+            .getState()
+            .getPose()
+            .getRotation()
+            .interpolate(after.getState().getPose().getRotation(), s);
+
     double interpolatedVelocityX =
         MathUtil.interpolate(before.getState().getVx(), after.getState().getVx(), s);
     double interpolatedVelocityY =
@@ -105,9 +107,9 @@ public VehicleState sample(double timeSeconds) {
         MathUtil.interpolate(before.getState().getOmega(), after.getState().getOmega(), s);
 
     return VehicleState.newBuilder()
-        .setX(interpolatedPose.getTranslation().getX())
-        .setY(interpolatedPose.getTranslation().getY())
-        .setTheta(interpolatedPose.getRotation().getRadians())
+        .setX(interpolatedPoseX)
+        .setY(interpolatedPoseY)
+        .setTheta(interpolatedRotation.getRadians())
         .setVx(interpolatedVelocityX)
         .setVy(interpolatedVelocityY)
         .setOmega(interpolatedAngularVelocity)