Added FieldConstants.java

Added capability for choreo trajectory following

src/main/deploy/pathplanner/autos/Example Auto.auto

@@ -20,7 +20,7 @@
         {
           "type": "path",
           "data": {
-            "pathName": "Example Path"
+            "pathName": "ShootAndTaxi1"
           }
         }
       ]

src/main/java/frc/robot/FieldConstants.java

@@ -0,0 +1,51 @@
+package frc.robot;
+
+import edu.wpi.first.math.geometry.Translation2d;
+import edu.wpi.first.math.util.Units;
+
+/**
+ * Contains various field dimensions and useful reference points. Dimensions are in meters, and sets
+ * of corners start in the lower left moving clockwise. <b>All units in Meters</b> <br>
+ * <br>
+ *
+ * <p>All translations and poses are stored with the origin at the rightmost point on the BLUE
+ * ALLIANCE wall.<br>
+ * <br>
+ * Length refers to the <i>x</i> direction (as described by wpilib) <br>
+ * Width refers to the <i>y</i> direction (as described by wpilib)
+ */
+public class FieldConstants {
+  public static double fieldLength = Units.inchesToMeters(653.22);
+  public static double fieldWidth = Units.inchesToMeters(323.28);
+  public static double wingX = Units.inchesToMeters(229.201);
+
+  public static double podiumX = Units.inchesToMeters(121);
+  public static double startingLineX = Units.inchesToMeters(74.111);
+
+  public static final class StagingLocations {
+    public static double centerlineX = Units.inchesToMeters(fieldLength / 2);
+    // need to update
+    public static double centerlineFirstY = Units.inchesToMeters(-1);
+    public static double centerlineSeparationY = Units.inchesToMeters(66);
+    public static double spikeX = Units.inchesToMeters(114);
+    // need
+    public static double spikeFirstY = Units.inchesToMeters(-1);
+    public static double spikeSeparationY = Units.inchesToMeters(57);
+
+    public static Translation2d[] centerlineTranslations = new Translation2d[5];
+    public static Translation2d[] spikeTranslations = new Translation2d[3];
+
+    static {
+      for (int i = 0; i < centerlineTranslations.length; i++) {
+        centerlineTranslations[i] =
+            new Translation2d(centerlineX, centerlineFirstY + (i * centerlineSeparationY));
+      }
+    }
+
+    static {
+      for (int i = 0; i < spikeTranslations.length; i++) {
+        spikeTranslations[i] = new Translation2d(spikeX, spikeFirstY + (i * spikeSeparationY));
+      }
+    }
+  }
+}

src/main/java/frc/robot/RobotContainer.java

@@ -13,7 +13,6 @@
 
 package frc.robot;
 
-import com.pathplanner.lib.auto.AutoBuilder;
 import edu.wpi.first.math.geometry.Pose2d;
 import edu.wpi.first.math.geometry.Rotation2d;
 import edu.wpi.first.wpilibj.GenericHID;
@@ -98,14 +97,7 @@ public RobotContainer() {
         break;
     }
 
-    // Set up auto routines
-    //    NamedCommands.registerCommand(
-    //        "Run Flywheel",
-    //        Commands.startEnd(
-    //                () -> flywheel.runVelocity(flywheelSpeedInput.get()), flywheel::stop,
-    // flywheel)
-    //            .withTimeout(5.0));
-    autoChooser = new LoggedDashboardChooser<>("Auto Choices", AutoBuilder.buildAutoChooser());
+    autoChooser = new LoggedDashboardChooser<>("Auto Choices");
 
     // Set up feedforward characterization
     autoChooser.addOption(
@@ -148,6 +140,11 @@ private void configureButtonBindings() {
                             new Pose2d(drive.getPose().getTranslation(), new Rotation2d())),
                     drive)
                 .ignoringDisable(true));
+    controller
+        .a()
+        .whileTrue(
+            Commands.run(() -> shooter.runFlywheelVelocity(flywheelSpeedInput.get()), shooter))
+        .whileFalse(Commands.run(() -> shooter.runFlywheelVelocity(0.0), shooter));
   }
 
   /**

src/main/java/frc/robot/commands/FeedForwardCharacterization.java

@@ -24,7 +24,7 @@
 
 public class FeedForwardCharacterization extends Command {
   private static final double START_DELAY_SECS = 2.0;
-  private static final double RAMP_VOLTS_PER_SEC = 0.1;
+  private static final double RAMP_VOLTS_PER_SEC = 0.4;
 
   private FeedForwardCharacterizationData data;
   private final Consumer<Double> voltageConsumer;

src/main/java/frc/robot/subsystems/drive/Drive.java

@@ -13,11 +13,6 @@
 
 package frc.robot.subsystems.drive;
 
-import com.pathplanner.lib.auto.AutoBuilder;
-import com.pathplanner.lib.pathfinding.Pathfinding;
-import com.pathplanner.lib.util.HolonomicPathFollowerConfig;
-import com.pathplanner.lib.util.PathPlannerLogging;
-import com.pathplanner.lib.util.ReplanningConfig;
 import edu.wpi.first.math.geometry.Pose2d;
 import edu.wpi.first.math.geometry.Rotation2d;
 import edu.wpi.first.math.geometry.Translation2d;
@@ -28,18 +23,16 @@
 import edu.wpi.first.math.kinematics.SwerveModuleState;
 import edu.wpi.first.math.util.Units;
 import edu.wpi.first.wpilibj.DriverStation;
-import edu.wpi.first.wpilibj.DriverStation.Alliance;
 import edu.wpi.first.wpilibj2.command.SubsystemBase;
-import frc.robot.util.LocalADStarAK;
 import java.util.concurrent.locks.Lock;
 import java.util.concurrent.locks.ReentrantLock;
 import org.littletonrobotics.junction.AutoLogOutput;
 import org.littletonrobotics.junction.Logger;
 
 public class Drive extends SubsystemBase {
-  private static final double MAX_LINEAR_SPEED = Units.feetToMeters(14.5);
-  private static final double TRACK_WIDTH_X = Units.inchesToMeters(25.0);
-  private static final double TRACK_WIDTH_Y = Units.inchesToMeters(25.0);
+  private static final double MAX_LINEAR_SPEED = Units.inchesToMeters(140.0);
+  private static final double TRACK_WIDTH_X = Units.inchesToMeters(26.0);
+  private static final double TRACK_WIDTH_Y = Units.inchesToMeters(26.0);
   private static final double DRIVE_BASE_RADIUS =
       Math.hypot(TRACK_WIDTH_X / 2.0, TRACK_WIDTH_Y / 2.0);
   private static final double MAX_ANGULAR_SPEED = MAX_LINEAR_SPEED / DRIVE_BASE_RADIUS;
@@ -64,29 +57,6 @@ public Drive(
     modules[1] = new Module(frModuleIO, 1);
     modules[2] = new Module(blModuleIO, 2);
     modules[3] = new Module(brModuleIO, 3);
-
-    // Configure AutoBuilder for PathPlanner
-    AutoBuilder.configureHolonomic(
-        this::getPose,
-        this::setPose,
-        () -> kinematics.toChassisSpeeds(getModuleStates()),
-        this::runVelocity,
-        new HolonomicPathFollowerConfig(
-            MAX_LINEAR_SPEED, DRIVE_BASE_RADIUS, new ReplanningConfig()),
-        () ->
-            DriverStation.getAlliance().isPresent()
-                && DriverStation.getAlliance().get() == Alliance.Red,
-        this);
-    Pathfinding.setPathfinder(new LocalADStarAK());
-    PathPlannerLogging.setLogActivePathCallback(
-        (activePath) -> {
-          Logger.recordOutput(
-              "Odometry/Trajectory", activePath.toArray(new Pose2d[activePath.size()]));
-        });
-    PathPlannerLogging.setLogTargetPoseCallback(
-        (targetPose) -> {
-          Logger.recordOutput("Odometry/TrajectorySetpoint", targetPose);
-        });
   }
 
   public void periodic() {

src/main/java/frc/robot/subsystems/drive/DriveConstants.java

@@ -0,0 +1,3 @@
+package frc.robot.subsystems.drive;
+
+public class DriveConstants {}

src/main/java/frc/robot/subsystems/kitbotshooter/KitbotFeederIO.java

@@ -4,14 +4,14 @@
 
 public interface KitbotFeederIO {
   @AutoLog
-  class FeederIOInputs {
+  class KitbotFeederIOInputs {
     public double feederPositionRads = 0.0;
     public double feederVelocityRadPerSec = 0.0;
     public double feederAppliedVolts = 0.0;
     public double feederCurrentAmps = 0.0;
   }
 
-  default void updateInputs(FeederIOInputs inputs) {}
+  default void updateInputs(KitbotFeederIOInputs inputs) {}
   ;
 
   /** run at voltage */

src/main/java/frc/robot/subsystems/kitbotshooter/KitbotFeederIOSim.java

@@ -12,7 +12,7 @@ public class KitbotFeederIOSim implements KitbotFeederIO {
   private double inputVolts = 0.0;
 
   @Override
-  public void updateInputs(FeederIOInputs inputs) {
+  public void updateInputs(KitbotFeederIOInputs inputs) {
     positionRads += sim.getAngularVelocityRadPerSec() * (Constants.loopPeriodMs / 1000.0);
     inputs.feederPositionRads = positionRads;
     inputs.feederVelocityRadPerSec = sim.getAngularVelocityRadPerSec();

src/main/java/frc/robot/subsystems/kitbotshooter/KitbotFeederIOSparkMax.java

@@ -30,7 +30,7 @@ public KitbotFeederIOSparkMax() {
   }
 
   @Override
-  public void updateInputs(FeederIOInputs inputs) {
+  public void updateInputs(KitbotFeederIOInputs inputs) {
     inputs.feederPositionRads = Units.rotationsToRadians(encoder.getPosition()) / GEARING;
     inputs.feederVelocityRadPerSec = Units.rotationsToRadians(encoder.getVelocity()) / GEARING;
     inputs.feederAppliedVolts = motor.getAppliedOutput();

src/main/java/frc/robot/subsystems/kitbotshooter/KitbotFlywheelIO.java

@@ -4,14 +4,14 @@
 
 public interface KitbotFlywheelIO {
   @AutoLog
-  class FlywheelIOInputs {
+  class KitbotFlywheelIOInputs {
     public double[] flywheelPositionRads = new double[] {};
     public double[] flywheelVelocityRadPerSec = new double[] {};
     public double[] flywheelAppliedVolts = new double[] {};
     public double[] flywheelCurrentAmps = new double[] {};
   }
 
-  default void updateInputs(FlywheelIOInputs inputs) {}
+  default void updateInputs(KitbotFlywheelIOInputs inputs) {}
   ;
 
   /** run to velocity */

src/main/java/frc/robot/subsystems/kitbotshooter/KitbotFlywheelIOSim.java

@@ -19,7 +19,7 @@ public class KitbotFlywheelIOSim implements KitbotFlywheelIO {
   private double inputVolts = 0.0;
 
   @Override
-  public void updateInputs(FlywheelIOInputs inputs) {
+  public void updateInputs(KitbotFlywheelIOInputs inputs) {
     positionRads += sim.getAngularVelocityRadPerSec() * (Constants.loopPeriodMs / 1000.0);
     inputs.flywheelPositionRads = new double[] {positionRads, positionRads};
     inputs.flywheelVelocityRadPerSec = new double[2];

src/main/java/frc/robot/subsystems/kitbotshooter/KitbotFlywheelIOSparkMax.java

@@ -54,7 +54,7 @@ public KitbotFlywheelIOSparkMax() {
   }
 
   @Override
-  public void updateInputs(FlywheelIOInputs inputs) {
+  public void updateInputs(KitbotFlywheelIOInputs inputs) {
     inputs.flywheelPositionRads = getPosition();
     inputs.flywheelVelocityRadPerSec = getVelocity();
     inputs.flywheelAppliedVolts =

src/main/java/frc/robot/subsystems/kitbotshooter/KitbotShooter.java

@@ -16,8 +16,8 @@ public class KitbotShooter extends SubsystemBase {
   private KitbotFlywheelIO kitbotFlywheelIO;
   private KitbotFeederIO kitbotFeederIO;
 
-  private FlywheelIOInputsAutoLogged flywheelInputs = new FlywheelIOInputsAutoLogged();
-  private FeederIOInputsAutoLogged feederInputs = new FeederIOInputsAutoLogged();
+  private KitbotFlywheelIOInputsAutoLogged flywheelInputs = new KitbotFlywheelIOInputsAutoLogged();
+  private KitbotFeederIOInputsAutoLogged feederInputs = new KitbotFeederIOInputsAutoLogged();
 
   private Mode currentMode = null;
 

src/main/java/frc/robot/util/AllianceFlipUtil.java

@@ -0,0 +1,66 @@
+package frc.robot.util;
+
+import edu.wpi.first.math.geometry.Pose2d;
+import edu.wpi.first.math.geometry.Rotation2d;
+import edu.wpi.first.math.geometry.Translation2d;
+import edu.wpi.first.wpilibj.DriverStation;
+import edu.wpi.first.wpilibj.DriverStation.Alliance;
+import frc.robot.FieldConstants;
+import frc.robot.util.trajectory.HolonomicDriveController;
+
+/** Utility functions for flipping from the blue to red alliance. */
+public class AllianceFlipUtil {
+  /** Flips an x coordinate to the correct side of the field based on the current alliance color. */
+  public static double apply(double xCoordinate) {
+    if (shouldFlip()) {
+      return FieldConstants.fieldLength - xCoordinate;
+    } else {
+      return xCoordinate;
+    }
+  }
+
+  /** Flips a translation to the correct side of the field based on the current alliance color. */
+  public static Translation2d apply(Translation2d translation) {
+    if (shouldFlip()) {
+      return new Translation2d(apply(translation.getX()), translation.getY());
+    } else {
+      return translation;
+    }
+  }
+
+  /** Flips a rotation based on the current alliance color. */
+  public static Rotation2d apply(Rotation2d rotation) {
+    if (shouldFlip()) {
+      return new Rotation2d(-rotation.getCos(), rotation.getSin());
+    } else {
+      return rotation;
+    }
+  }
+
+  /** Flips a pose to the correct side of the field based on the current alliance color. */
+  public static Pose2d apply(Pose2d pose) {
+    if (shouldFlip()) {
+      return new Pose2d(apply(pose.getTranslation()), apply(pose.getRotation()));
+    } else {
+      return pose;
+    }
+  }
+
+  /**
+   * Flips a trajectory state to the correct side of the field based on the current alliance color.
+   */
+  public static HolonomicDriveController.HolonomicDriveState apply(
+      HolonomicDriveController.HolonomicDriveState state) {
+    if (shouldFlip()) {
+      return new HolonomicDriveController.HolonomicDriveState(
+          apply(state.pose()), -state.velocityX(), state.velocityY(), -state.angularVelocity());
+    } else {
+      return state;
+    }
+  }
+
+  public static boolean shouldFlip() {
+    return DriverStation.getAlliance().isPresent()
+        && DriverStation.getAlliance().get() == Alliance.Red;
+  }
+}