Ray pathplanner

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

@@ -3,10 +3,13 @@
 import com.ctre.phoenix6.configs.CANcoderConfiguration;
 import com.ctre.phoenix6.configs.TalonFXConfiguration;
 import com.ctre.phoenix6.signals.SensorDirectionValue;
+import com.pathplanner.lib.path.PathConstraints;
 import com.revrobotics.spark.config.SparkBaseConfig.IdleMode;
 import com.revrobotics.spark.config.SoftLimitConfig;
 import com.revrobotics.spark.config.SparkMaxConfig;
 
+import edu.wpi.first.math.geometry.Pose2d;
+import edu.wpi.first.math.geometry.Rotation2d;
 import edu.wpi.first.math.geometry.Translation2d;
 import frc.FSLib2025.swerve.SwerveModuleConstants;
 
@@ -16,6 +19,7 @@ public static final class RobotConstants {
         public static final double PERIODIC_INTERVAL = 0.02; // the periodic ,in seconds
         public static final String CANBUS_NAME = "GTX7130";
         public static final int DRIVE_CONTROLLER_PORT = 0;
+        public static final int OPERATOR_CONTROLLER_PORT = 1;
     }
 
     public static final class SwerveConstants {
@@ -85,6 +89,12 @@ public static final class SwerveConstants {
             STEER_MOTOR_CONFIGURATION.inverted(false);
             STEER_MOTOR_CONFIGURATION.idleMode(IdleMode.kBrake);
         }
+
+        public static final PathConstraints constraints = new PathConstraints(
+            4.0, 
+            4.0, 
+            Rotation2d.fromDegrees(180).getRotations(), 
+            Rotation2d.fromDegrees(360).getRotations());
     }
 
     public static final class SuperstructureConstants {
@@ -224,4 +234,36 @@ public static final class SuperstructureConstants {
         public static final double INTAKE_KI = 0;
         public static final double INTAKE_KD = 0;
     }
+
+    public static final class FieldConstants {
+        // starting pose
+        public static final Pose2d Left = new Pose2d(new Translation2d(7.231, 7.615), Rotation2d.fromDegrees(180));
+        public static final Pose2d Mid = new Pose2d(new Translation2d(8.068, 6.178), Rotation2d.fromDegrees(180));
+        public static final Pose2d Right = new Pose2d(new Translation2d(8.068, 5.067), Rotation2d.fromDegrees(180));
+        // REEF scoring pose
+        public static final Pose2d A = new Pose2d(new Translation2d(3.180, 4.082), Rotation2d.fromDegrees(0));
+        public static final Pose2d B = new Pose2d(new Translation2d(3.109, 3.847), Rotation2d.fromDegrees(0));
+        public static final Pose2d C = new Pose2d(new Translation2d(3.654, 2.885), Rotation2d.fromDegrees(60));
+        public static final Pose2d D = new Pose2d(new Translation2d(3.920, 2.731), Rotation2d.fromDegrees(60));
+        public static final Pose2d E = new Pose2d(new Translation2d(5.058, 2.713), Rotation2d.fromDegrees(120));
+        public static final Pose2d F = new Pose2d(new Translation2d(5.325, 2.880), Rotation2d.fromDegrees(120));
+        public static final Pose2d G = new Pose2d(new Translation2d(5.883, 3.851), Rotation2d.fromDegrees(180));
+        public static final Pose2d H = new Pose2d(new Translation2d(5.883, 4.190), Rotation2d.fromDegrees(180));
+        public static final Pose2d I = new Pose2d(new Translation2d(5.341, 5.341), Rotation2d.fromDegrees(-120));
+        public static final Pose2d J = new Pose2d(new Translation2d(5.072, 5.320), Rotation2d.fromDegrees(-120));
+        public static final Pose2d K = new Pose2d(new Translation2d(3.887, 5.375), Rotation2d.fromDegrees(-60));
+        public static final Pose2d L = new Pose2d(new Translation2d(3.623, 5.215), Rotation2d.fromDegrees(-60));
+        // Algae & Coral
+        public static final Pose2d LA = new Pose2d(new Translation2d(1.843, 5.848), Rotation2d.fromDegrees(180));
+        public static final Pose2d LC = new Pose2d(new Translation2d(1.843, 5.848), Rotation2d.fromDegrees(180));
+        public static final Pose2d MA = new Pose2d(new Translation2d(1.843, 4.020), Rotation2d.fromDegrees(180));
+        public static final Pose2d MC = new Pose2d(new Translation2d(1.843, 4.020), Rotation2d.fromDegrees(180));
+        public static final Pose2d RA = new Pose2d(new Translation2d(1.843, 2.166), Rotation2d.fromDegrees(180));
+        public static final Pose2d RC = new Pose2d(new Translation2d(1.843, 2.166), Rotation2d.fromDegrees(180));
+        // PROCESSOR
+        public static final Pose2d PRO = new Pose2d(new Translation2d(6.347, 0.573), Rotation2d.fromDegrees(-90));
+        // Coral Station
+        public static final Pose2d CSR = new Pose2d(new Translation2d(1.666, 0.693), Rotation2d.fromDegrees(-126));
+        public static final Pose2d CSL = new Pose2d(new Translation2d(1.666, 7.338), Rotation2d.fromDegrees(126));
+    }
 }

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

@@ -1,37 +1,39 @@
 package frc.robot;
 
+import java.util.function.BooleanSupplier;
+
 import com.pathplanner.lib.auto.AutoBuilder;
 
 import edu.wpi.first.wpilibj.smartdashboard.SendableChooser;
 import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
 import edu.wpi.first.wpilibj2.command.Command;
+import edu.wpi.first.wpilibj2.command.Commands;
 import edu.wpi.first.wpilibj2.command.button.CommandXboxController;
+import frc.robot.Constants.FieldConstants;
 import frc.robot.Constants.RobotConstants;
+import frc.robot.commands.GeneratePath;
 import frc.robot.commands.TeleopSwerve;
 import frc.robot.subsystems.Swerve;
 
 public class RobotContainer {
-
-    public CommandXboxController controller = new CommandXboxController(RobotConstants.DRIVE_CONTROLLER_PORT);
 
-    private final Swerve swerve = new Swerve();
+    public CommandXboxController m_Driver = new CommandXboxController(RobotConstants.DRIVE_CONTROLLER_PORT);
+    public CommandXboxController m_Operator = new CommandXboxController(RobotConstants.OPERATOR_CONTROLLER_PORT);
+
+    private final Swerve m_Swerve = new Swerve();
 
-    private final TeleopSwerve teleopSwerve = new TeleopSwerve(swerve, controller);
+    private final TeleopSwerve teleopSwerve = new TeleopSwerve(m_Swerve, m_Driver);
 
     private final SendableChooser<Command> autoChooser;
 
     public RobotContainer() {
-        swerve.setDefaultCommand(teleopSwerve);
+        m_Swerve.setDefaultCommand(teleopSwerve);
 
-        configureButtonBindings();
         autoChooser = AutoBuilder.buildAutoChooser();
 
         SmartDashboard.putData("Auto Chooser", autoChooser);
     }
 
-    private void configureButtonBindings() {
-    }
-
     public Command getAutonomousCommand() {
         return autoChooser.getSelected();
     }

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

@@ -0,0 +1,38 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+package frc.robot.commands;
+
+import java.io.IOException;
+
+import org.json.simple.parser.ParseException;
+
+import com.pathplanner.lib.auto.AutoBuilder;
+import com.pathplanner.lib.path.PathConstraints;
+import com.pathplanner.lib.path.PathPlannerPath;
+import com.pathplanner.lib.util.FileVersionException;
+
+import edu.wpi.first.math.geometry.Pose2d;
+import edu.wpi.first.math.geometry.Rotation2d;
+import edu.wpi.first.wpilibj2.command.Command;
+import frc.robot.Constants.SwerveConstants;
+
+public class GeneratePath {
+
+  public Command pathFindToPose(Pose2d targetPose) {
+    return AutoBuilder.pathfindToPose(targetPose, SwerveConstants.CONCTRAINTS);
+  }
+
+  public Command pathFindThenFollow(String path) throws FileVersionException, IOException, ParseException {
+    return AutoBuilder.pathfindThenFollowPath(PathPlannerPath.fromPathFile(path), SwerveConstants.CONCTRAINTS);
+  }
+
+  public Command followPath(String path) throws FileVersionException, IOException, ParseException {
+    return AutoBuilder.followPath(PathPlannerPath.fromPathFile(path));
+  }
+
+  public Command followChoreoPath(String path) throws FileVersionException, IOException, ParseException {
+    return AutoBuilder.followPath(PathPlannerPath.fromChoreoTrajectory(path));
+  }
+}

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

@@ -1,17 +1,19 @@
 package frc.robot.commands;
 
+import edu.wpi.first.math.MathUtil;
 import edu.wpi.first.math.filter.SlewRateLimiter;
 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.wpilibj2.command.Command;
 import edu.wpi.first.wpilibj2.command.button.CommandXboxController;
+import frc.robot.Constants.FieldConstants;
 import frc.robot.Constants.SwerveConstants;
 import frc.robot.subsystems.Swerve;
 
 public class TeleopSwerve extends Command {
 
-    private final Swerve swerve;
+    private final Swerve m_Swerve;
 
     private SlewRateLimiter xLimiter = new SlewRateLimiter(3.0);
     private SlewRateLimiter yLimiter = new SlewRateLimiter(3.0);
@@ -23,45 +25,46 @@ public class TeleopSwerve extends Command {
 
     private double reduction = 1;
 
-    private CommandXboxController controller;
+    private CommandXboxController m_Driver;
 
-    public TeleopSwerve(Swerve swerve, CommandXboxController controller) {
-        this.swerve = swerve;
-        this.controller = controller;
-        addRequirements(swerve);
+    public TeleopSwerve(Swerve m_Swerve, CommandXboxController m_Driver) {
+        this.m_Swerve = m_Swerve;
+        this.m_Driver = m_Driver;
+        addRequirements(m_Swerve);
     }
 
     @Override
     public void execute() {
 
-        if (controller.getHID().getAButtonPressed()) {
-            swerve.setOdometryPosition(new Pose2d());
-            swerve.setGyroYaw(new Rotation2d());
+        if (m_Driver.getHID().getBackButtonPressed()) {
+            m_Swerve.setOdometryPosition(new Pose2d());
+            m_Swerve.setGyroYaw(new Rotation2d());
         }
 
-        if (controller.getHID().getRightBumperButton()) {
-            reduction = 0.3;
-        } else {
-            reduction = 1;
+        double slow = 1;
+        if (m_Driver.getHID().getLeftBumperButton() || m_Driver.getHID().getRightBumperButton())  {
+            slow = 0.5;
         }
 
-        xSpeed = xLimiter.calculate(-controller.getLeftY() * reduction);
-        ySpeed = yLimiter.calculate(-controller.getLeftX() * reduction);
-        rotSpeed = rotLimiter.calculate(-controller.getRightX() * reduction);
+        double deadband = 0.04;
+        double xyMultiplier = 1;
+        double zMultiplier = 0.75;
 
-        // square the input to inprove driving experience
+        xSpeed = xLimiter.calculate(MathUtil.applyDeadband(-m_Driver.getLeftY(), deadband));
+        ySpeed = yLimiter.calculate(MathUtil.applyDeadband(-m_Driver.getLeftX(), deadband));
+        rotSpeed = rotLimiter.calculate(MathUtil.applyDeadband(-m_Driver.getRightX(), deadband));
         xSpeed = Math.copySign(xSpeed * xSpeed, xSpeed);
-        ySpeed = Math.copySign(ySpeed * ySpeed, ySpeed);        
-        rotSpeed = Math.copySign(rotSpeed * rotSpeed, rotSpeed);
-
-        swerve.drive(
-                new Translation2d(xSpeed, ySpeed).times(SwerveConstants.MAX_MODULE_SPEED),
-                rotSpeed * SwerveConstants.MAX_MODULE_ROTATIONAL_SPEED,
+        ySpeed = Math.copySign(ySpeed * ySpeed, ySpeed);
+        // rotSpeed = Math.copySign(rotSpeed * rotSpeed, rotSpeed);
+        m_Swerve.drive(
+                new Translation2d(xSpeed, ySpeed).times(SwerveConstants.MAX_MODULE_SPEED).times(xyMultiplier).times(slow),
+                rotSpeed * SwerveConstants.MAX_MODULE_ROTATIONAL_SPEED * zMultiplier * slow,
+                // 0,
                 true);
     }
 
     @Override
     public void end(boolean interrupted) {
-        swerve.drive(new Translation2d(), 0, false);
+        m_Swerve.drive(new Translation2d(), 0, false);
     }
 }

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

@@ -24,6 +24,14 @@
 
 public class Swerve extends SubsystemBase {
 
+    private static Swerve m_Instance = null;
+    public static Swerve getInstance() {
+        if (m_Instance == null) {
+            m_Instance = new Swerve();
+        }
+        return m_Instance;
+    }
+
     private Pigeon2 pigeon = new Pigeon2(SwerveConstants.PIGEON_ID, RobotConstants.CANBUS_NAME);
 
     private SwerveModule[] modules = new SwerveModule[] {