Fixed remaining errors

Author: Liam Teltow

src/main/java/frc/robot/Constants.java

@@ -17,13 +17,18 @@
 import org.lasarobotics.utils.PIDConstants;
 import org.lasarobotics.vision.AprilTagCamera.Resolution;
 
+import com.pathplanner.lib.config.ModuleConfig;
+import com.revrobotics.spark.config.SparkFlexConfig;
+
 import edu.wpi.first.apriltag.AprilTag;
 import edu.wpi.first.apriltag.AprilTagFieldLayout;
 import edu.wpi.first.apriltag.AprilTagFields;
 import edu.wpi.first.math.geometry.Rotation2d;
 import edu.wpi.first.math.geometry.Rotation3d;
 import edu.wpi.first.math.geometry.Transform3d;
 import edu.wpi.first.math.geometry.Translation3d;
+import edu.wpi.first.math.system.plant.DCMotor;
+import edu.wpi.first.math.trajectory.TrapezoidProfile;
 import edu.wpi.first.units.Units;
 import edu.wpi.first.units.measure.Angle;
 import edu.wpi.first.units.measure.Current;
@@ -38,6 +43,8 @@ public static class Field {
     public static final AprilTagFieldLayout FIELD_LAYOUT = AprilTagFieldLayout.loadField(AprilTagFields.k2024Crescendo);
     public static final AprilTag BLUE_SPEAKER = getTag(7).get();
     public static final AprilTag RED_SPEAKER = getTag(4).get();
+    public static final AprilTag BLUE_AMP = getTag(6).get();
+
 
     /**
      * Get AprilTag from field
@@ -55,6 +62,19 @@ public static class HID {
     public static final Dimensionless CONTROLLER_DEADBAND = Units.Percent.of(10);
   }
 
+  
+  public static class WiggleStick {
+    
+    public static final SparkFlexConfig WIGGLE_STICK_CONFIG = new SparkFlexConfig();
+
+    public static final TrapezoidProfile.Constraints CONSTRAINTS = new TrapezoidProfile.Constraints(10, 20);
+  }
+
+  public static class SmartDashboard {
+    public static final String SMARTDASHBOARD_DEFAULT_TAB = "SmartDashboard";
+    public static final String SMARTDASHBOARD_AUTO_MODE = "Auto Mode";
+  }
+
   public static class Drive {
     public static final DriveWheel DRIVE_WHEEL = DriveWheel.create(Units.Inches.of(4.0), Units.Value.of(1.3), Units.Value.of(1.2));
     public static final PIDConstants DRIVE_PID = PIDConstants.of(0.3, 0.0, 0.001, 0.0, 0.0);
@@ -86,6 +106,8 @@ public static class Drive {
 
     public static final SwerveX2Module.GearRatio CTRE_GEAR_RATIO = SwerveX2Module.GearRatio.X4_3;
     public static final MAXSwerveModule.GearRatio REV_GEAR_RATIO = MAXSwerveModule.GearRatio.L3;
+
+    public static final ModuleConfig MODULE_CONFIG = new ModuleConfig(DRIVE_WHEEL.diameter.div(2), Units.MetersPerSecond.of(5.172), 1.3, DCMotor.getKrakenX60Foc(1), DRIVE_CURRENT_LIMIT, 1);
   }
 
   public static class CTREDriveHardware {

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

@@ -11,12 +11,11 @@
 import edu.wpi.first.wpilibj2.command.Commands;
 import edu.wpi.first.wpilibj2.command.button.CommandXboxController;
 import frc.robot.subsystems.WaggleSubsystem;
-import frc.robot.subsystems.drive.AutoTrajectory;
 import frc.robot.subsystems.drive.CTREDriveSubsystem;
 import frc.robot.subsystems.drive.REVDriveSubsystem;
 
 public class RobotContainer {
-  private static final CTREDriveSubsystem CTREDRIVE_SUBSYSTEM = new CTREDriveSubsystem(
+  private static final CTREDriveSubsystem CTRE_DRIVE_SUBSYSTEM = new CTREDriveSubsystem(
     CTREDriveSubsystem.initializeHardware(),
     Constants.Drive.DRIVE_ROTATE_PID,
     Constants.Drive.DRIVE_AUTO_AIM_PID, Constants.Drive.DRIVE_CONTROL_CENTRICITY,
@@ -27,7 +26,7 @@ public class RobotContainer {
     Constants.Drive.DRIVE_LOOKAHEAD
   );
 
-  private static final REVDriveSubsystem REVDRIVE_SUBSYSTEM = new REVDriveSubsystem(
+  private static final REVDriveSubsystem REV_DRIVE_SUBSYSTEM = new REVDriveSubsystem(
     REVDriveSubsystem.initializeHardware(),
     Constants.Drive.DRIVE_ROTATE_PID,
     Constants.Drive.DRIVE_AUTO_AIM_PID, Constants.Drive.DRIVE_CONTROL_CENTRICITY,
@@ -46,16 +45,16 @@ public class RobotContainer {
 
   public RobotContainer() {
     // Set drive command
-    CTREDRIVE_SUBSYSTEM.setDefaultCommand(
-      CTREDRIVE_SUBSYSTEM.driveCommand(
+    CTRE_DRIVE_SUBSYSTEM.setDefaultCommand(
+      CTRE_DRIVE_SUBSYSTEM.driveCommand(
         () -> PRIMARY_CONTROLLER.getLeftY(),
         () -> PRIMARY_CONTROLLER.getLeftX(),
         () -> PRIMARY_CONTROLLER.getRightX()
       )
     );
 
     // Setup AutoBuilder
-    CTREDRIVE_SUBSYSTEM.configureAutoBuilder();
+    CTRE_DRIVE_SUBSYSTEM.configureAutoBuilder();
 
     autoModeChooser();
     SmartDashboard.putData(Constants.SmartDashboard.SMARTDASHBOARD_AUTO_MODE, m_automodeChooser);
@@ -66,19 +65,9 @@ public RobotContainer() {
 
   private void configureBindings() {
     // Start button - toggle traction control
-    PRIMARY_CONTROLLER.start().onTrue(CTREDRIVE_SUBSYSTEM.toggleTractionControlCommand());
+    PRIMARY_CONTROLLER.start().onTrue(CTRE_DRIVE_SUBSYSTEM.toggleTractionControlCommand());
 
-    // A button - go to amp
-    PRIMARY_CONTROLLER.a().whileTrue(
-      CTREDRIVE_SUBSYSTEM.goToPoseCommand(
-        Constants.Field.AMP
-      )
-    );
-
-    // B button - go to source
-    PRIMARY_CONTROLLER.b().whileTrue(CTREDRIVE_SUBSYSTEM.goToPoseCommand(Constants.Field.SOURCE));
-
-    PRIMARY_CONTROLLER.povLeft().onTrue(CTREDRIVE_SUBSYSTEM.resetPoseCommand(() -> new Pose2d()));
+    PRIMARY_CONTROLLER.povLeft().onTrue(CTRE_DRIVE_SUBSYSTEM.resetPoseCommand(() -> new Pose2d()));
 
     // Left/right bumper - wiggle stick
     PRIMARY_CONTROLLER.leftBumper().onTrue(WIGGLE_STICK.setPositionCommand(0.0));
@@ -90,9 +79,6 @@ private void configureBindings() {
    */
   private void autoModeChooser() {
     m_automodeChooser.setDefaultOption("Do nothing", Commands.none());
-    m_automodeChooser.addOption("Leave", new AutoTrajectory(CTREDRIVE_SUBSYSTEM, "Leave").getCommand());
-    m_automodeChooser.addOption("Preload + 3 Ring", new AutoTrajectory(CTREDRIVE_SUBSYSTEM, "Preload + 3 Ring").getCommand());
-    m_automodeChooser.addOption("Preload + 1", new AutoTrajectory(CTREDRIVE_SUBSYSTEM, "Preload + 1").getCommand());
   }
 
   /**

src/main/java/frc/robot/subsystems/WaggleSubsystem.java

@@ -5,11 +5,13 @@
 package frc.robot.subsystems;
 
 import org.lasarobotics.hardware.revrobotics.Spark;
-import org.lasarobotics.hardware.revrobotics.Spark.FeedbackSensor;
 import org.lasarobotics.hardware.revrobotics.Spark.MotorKind;
-import org.lasarobotics.hardware.revrobotics.SparkPIDConfig;
 
-import com.revrobotics.CANSparkBase.IdleMode;
+import com.revrobotics.spark.SparkBase.ControlType;
+import com.revrobotics.spark.config.SparkBaseConfig;
+import com.revrobotics.spark.config.SparkBaseConfig.IdleMode;
+import com.revrobotics.spark.config.SparkFlexConfig;
+import com.revrobotics.spark.config.SparkMaxConfig;
 
 import edu.wpi.first.math.trajectory.TrapezoidProfile.Constraints;
 import edu.wpi.first.wpilibj2.command.Command;
@@ -18,33 +20,38 @@
 
 public class WaggleSubsystem extends SubsystemBase {
   Spark m_motor;
-  SparkPIDConfig m_config;
+  SparkBaseConfig m_config;
   Constraints m_constraint;
 
   /** Creates a new wiggleStick. */
-  public WaggleSubsystem(SparkPIDConfig config, Constraints constraint) {
+  public WaggleSubsystem(SparkBaseConfig config, Constraints constraint) {
     m_motor = new Spark(new Spark.ID("wiggleStick", 20), MotorKind.NEO);
     m_config = config;
     m_constraint = constraint;
-
+    m_config = (m_motor.getKind().equals(MotorKind.NEO_VORTEX)) ? new SparkFlexConfig() : new SparkMaxConfig();
     double conversionFactor = 1.0;
-    m_motor.initializeSparkPID(config, FeedbackSensor.NEO_ENCODER);
-    m_motor.setPositionConversionFactor(FeedbackSensor.NEO_ENCODER, conversionFactor);
-    m_motor.setVelocityConversionFactor(FeedbackSensor.NEO_ENCODER, conversionFactor);
+    m_config.encoder.positionConversionFactor(conversionFactor);
+    m_config.encoder.velocityConversionFactor(conversionFactor / 60);
+    m_config.closedLoop.pidf(0.2, 0.0, 0.0, 0);
+    m_config.closedLoop.maxMotion.maxAcceleration(20);
+    m_config.closedLoop.maxMotion.maxVelocity(10);
 
-    m_motor.enablePIDWrapping(0.0, 15.0);
+    m_config.closedLoop.positionWrappingEnabled(true);
     m_motor.setIdleMode(IdleMode.kCoast);
     m_motor.resetEncoder();
   }
 
-  public void setPosition(double position) {
-    m_motor.smoothMotion(position, m_constraint);
+  /** Moves to the given position */
+  private void setPosition(double position) {
+    m_motor.set(position, ControlType.kMAXMotionPositionControl);
   }
 
+  /** Gets the position using the encoders */
   public double getPosition() {
     return m_motor.getInputs().encoderPosition;
   }
 
+  /** Command to set the position of the robot */
   public Command setPositionCommand(double position) {
     return runOnce(() -> setPosition(position));
 

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

@@ -4,13 +4,17 @@
 
 package frc.robot.subsystems.drive;
 
+import java.io.IOException;
 import java.util.List;
+import java.util.Optional;
 
+import org.json.simple.parser.ParseException;
 import org.lasarobotics.drive.swerve.SwerveDrive;
 
 import com.pathplanner.lib.auto.AutoBuilder;
 import com.pathplanner.lib.commands.PathPlannerAuto;
 import com.pathplanner.lib.path.GoalEndState;
+import com.pathplanner.lib.path.IdealStartingState;
 import com.pathplanner.lib.path.PathConstraints;
 import com.pathplanner.lib.path.PathPlannerPath;
 
@@ -28,8 +32,10 @@ public class AutoTrajectory {
    * Create new path trajectory using PathPlanner path
    * @param driveSubsystem DriveSubsystem to drive the robot
    * @param autoName PathPlanner auto name
+   * @throws IOException if attempting to load a file that does not exist or cannot be read
+   * @throws ParseException If JSON within file cannot be parsed
    */
-  public AutoTrajectory(SwerveDrive driveSubsystem, String autoName) {
+  public AutoTrajectory(SwerveDrive driveSubsystem, String autoName) throws IOException, ParseException {
     this.m_driveSubsystem = driveSubsystem;
 
     // Get path
@@ -44,18 +50,19 @@ public AutoTrajectory(SwerveDrive driveSubsystem, String autoName) {
    */
   public AutoTrajectory(SwerveDrive driveSubsystem, List<Pose2d> waypoints, PathConstraints pathConstraints) {
     this.m_driveSubsystem = driveSubsystem;
+    IdealStartingState startingState = new IdealStartingState(0, null);
 
     // Generate path from waypoints
     m_auto = new Pair<String, List<PathPlannerPath>>("", List.of(new PathPlannerPath(
-      PathPlannerPath.bezierFromPoses(waypoints),
+      PathPlannerPath.waypointsFromPoses(waypoints),
       pathConstraints,
-      new GoalEndState(0.0, waypoints.get(waypoints.size() - 1).getRotation())
+      startingState, new GoalEndState(0.0, waypoints.get(waypoints.size() - 1).getRotation())
     )));
   }
 
   /** Return initial pose */
-  public Pose2d getInitialPose() {
-    return m_auto.getSecond().get(0).getPreviewStartingHolonomicPose();
+  public Optional<Pose2d> getInitialPose() {
+    return m_auto.getSecond().get(0).getStartingHolonomicPose();
   }
 
   /**
@@ -68,7 +75,7 @@ public Command getCommand() {
       : new PathPlannerAuto(m_auto.getFirst());
 
     return Commands.sequence(
-      m_driveSubsystem.resetPoseCommand(() -> getInitialPose()),
+      m_driveSubsystem.resetPoseCommand(() -> getInitialPose().orElseThrow(null)),
       autoCommand,
       m_driveSubsystem.stopCommand(),
       m_driveSubsystem.lockCommand()

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

@@ -12,15 +12,23 @@
 import org.lasarobotics.hardware.kauailabs.NavX2;
 import org.lasarobotics.utils.PIDConstants;
 import org.lasarobotics.vision.AprilTagCamera;
+
+import com.pathplanner.lib.auto.AutoBuilder;
+import com.pathplanner.lib.config.RobotConfig;
+import com.pathplanner.lib.controllers.PPHolonomicDriveController;
+
 import edu.wpi.first.apriltag.AprilTagFieldLayout;
 import edu.wpi.first.apriltag.AprilTagFields;
+import edu.wpi.first.units.Units;
 import edu.wpi.first.units.measure.Angle;
 import edu.wpi.first.units.measure.Dimensionless;
 import edu.wpi.first.units.measure.Time;
+import edu.wpi.first.wpilibj.DriverStation;
 import frc.robot.Constants;
 
 public class CTREDriveSubsystem extends SwerveDrive {
 
+  Hardware m_drivetrainHardware;
   /**
    * Create an instance of DriveSubsystem
    * <p>
@@ -40,6 +48,7 @@ public CTREDriveSubsystem(Hardware drivetrainHardware, PIDConstants rotatePIDF,
                         PolynomialSplineFunction throttleInputCurve, PolynomialSplineFunction rotateInputCurve,
                         Angle rotateScalar, Dimensionless deadband, Time lookAhead) {
     super(drivetrainHardware, rotatePIDF, aimPIDF, controlCentricity, throttleInputCurve, rotateInputCurve, rotateScalar, deadband, lookAhead);
+    m_drivetrainHardware = drivetrainHardware;
   }
 
   /**
@@ -157,4 +166,40 @@ public static Hardware initializeHardware() {
 
     return drivetrainHardware;
   }
+
+  /**Configures the AutoBuilder so that PathPlanner can use its built-in commands when running autos.
+   * Uses the PathPlanner AutoBuilder method. 
+  */
+
+  public void configureAutoBuilder() {
+    AutoBuilder.configure(
+            () -> getPose(), // Robot pose supplier
+            (pose) -> resetPose(pose), // Method to reset odometry (will be called if your auto has a starting pose)
+            () -> getChassisSpeeds(), // ChassisSpeeds supplier. MUST BE ROBOT RELATIVE
+            (speeds, feedforwards) -> autoDrive(speeds), // Method that will drive the robot given ROBOT RELATIVE ChassisSpeeds. Also optionally outputs individual module feedforwards
+            new PPHolonomicDriveController( // PPHolonomicController is the built in path following controller for holonomic drive trains
+                    new com.pathplanner.lib.config.PIDConstants(5.0, 0.0, 0.0), // Translation PID constants
+                    new com.pathplanner.lib.config.PIDConstants(5.0, 0.0, 0.0) // Rotation PID constants
+            ),
+            new RobotConfig(Constants.Drive.MASS, // The robot configuration
+                            Units.KilogramSquareMeters.of(1),
+                            Constants.Drive.MODULE_CONFIG,
+                            m_drivetrainHardware.lFrontModule().getModuleCoordinate(),
+                            m_drivetrainHardware.rFrontModule().getModuleCoordinate(),
+                            m_drivetrainHardware.lRearModule().getModuleCoordinate(),
+                            m_drivetrainHardware.rRearModule().getModuleCoordinate()),
+            () -> {
+              // Boolean supplier that controls when the path will be mirrored for the red alliance
+              // This will flip the path being followed to the red side of the field.
+              // THE ORIGIN WILL REMAIN ON THE BLUE SIDE
+
+              var alliance = DriverStation.getAlliance();
+              if (alliance.isPresent()) {
+                return alliance.get() == DriverStation.Alliance.Red;
+              }
+              return false;
+            },
+            this // Reference to this subsystem to set requirements
+    );
+  }
 }