Added pathplanner and simulation

.pathplanner/settings.json

@@ -0,0 +1,7 @@
+{
+  "robotWidth": 0.75,
+  "robotLength": 1.0,
+  "holonomicMode": true,
+  "generateJSON": false,
+  "generateCSV": false
+}

.vscode/launch.json

@@ -4,18 +4,24 @@
   // For more information, visit: https://go.microsoft.com/fwlink/?linkid=830387
   "version": "0.2.0",
   "configurations": [
-
+    {
+      "type": "java",
+      "name": "Launch Main",
+      "request": "launch",
+      "mainClass": "frc.robot.Main",
+      "projectName": "2023OffseasonSwerve"
+    },
     {
       "type": "wpilib",
       "name": "WPILib Desktop Debug",
       "request": "launch",
-      "desktop": true,
+      "desktop": true
     },
     {
       "type": "wpilib",
       "name": "WPILib roboRIO Debug",
       "request": "launch",
-      "desktop": false,
+      "desktop": false
     }
   ]
 }

networktables.json

@@ -0,0 +1 @@
+[]

simgui-ds.json

@@ -0,0 +1,102 @@
+{
+  "FMS": {
+    "window": {
+      "visible": false
+    }
+  },
+  "Joysticks": {
+    "window": {
+      "visible": false
+    }
+  },
+  "keyboardJoysticks": [
+    {
+      "axisConfig": [
+        {
+          "decKey": 65,
+          "incKey": 68
+        },
+        {
+          "decKey": 87,
+          "incKey": 83
+        },
+        {
+          "decKey": 69,
+          "decayRate": 0.0,
+          "incKey": 82,
+          "keyRate": 0.009999999776482582
+        }
+      ],
+      "axisCount": 3,
+      "buttonCount": 4,
+      "buttonKeys": [
+        90,
+        88,
+        67,
+        86
+      ],
+      "povConfig": [
+        {
+          "key0": 328,
+          "key135": 323,
+          "key180": 322,
+          "key225": 321,
+          "key270": 324,
+          "key315": 327,
+          "key45": 329,
+          "key90": 326
+        }
+      ],
+      "povCount": 1
+    },
+    {
+      "axisConfig": [
+        {
+          "decKey": 74,
+          "incKey": 76
+        },
+        {
+          "decKey": 73,
+          "incKey": 75
+        }
+      ],
+      "axisCount": 2,
+      "buttonCount": 4,
+      "buttonKeys": [
+        77,
+        44,
+        46,
+        47
+      ],
+      "povCount": 0
+    },
+    {
+      "axisConfig": [
+        {
+          "decKey": 263,
+          "incKey": 262
+        },
+        {
+          "decKey": 265,
+          "incKey": 264
+        }
+      ],
+      "axisCount": 2,
+      "buttonCount": 6,
+      "buttonKeys": [
+        260,
+        268,
+        266,
+        261,
+        269,
+        267
+      ],
+      "povCount": 0
+    },
+    {
+      "axisCount": 0,
+      "buttonCount": 0,
+      "povCount": 0
+    }
+  ]
+}

simgui.json

@@ -0,0 +1,19 @@
+{
+  "NTProvider": {
+    "types": {
+      "/FMSInfo": "FMSInfo",
+      "/SmartDashboard/Field": "Field2d"
+    },
+    "windows": {
+      "/SmartDashboard/Field": {
+        "image": "C:\\Users\\anand\\OneDrive\\Documents\\GitHub\\2023CompetitionSeason\\2023-field.png",
+        "window": {
+          "visible": true
+        }
+      }
+    }
+  },
+  "NetworkTables View": {
+    "visible": false
+  }
+}

src/main/deploy/pathplanner/path.path

@@ -0,0 +1,49 @@
+{
+  "waypoints": [
+    {
+      "anchorPoint": {
+        "x": 6.0,
+        "y": 3.0
+      },
+      "prevControl": null,
+      "nextControl": {
+        "x": 7.0,
+        "y": 3.0
+      },
+      "holonomicAngle": 0,
+      "isReversal": false,
+      "velOverride": null,
+      "isLocked": false,
+      "isStopPoint": false,
+      "stopEvent": {
+        "names": [],
+        "executionBehavior": "parallel",
+        "waitBehavior": "none",
+        "waitTime": 0
+      }
+    },
+    {
+      "anchorPoint": {
+        "x": 9.0,
+        "y": 3.0
+      },
+      "prevControl": {
+        "x": 8.0,
+        "y": 3.0
+      },
+      "nextControl": null,
+      "holonomicAngle": 180.0,
+      "isReversal": false,
+      "velOverride": null,
+      "isLocked": false,
+      "isStopPoint": false,
+      "stopEvent": {
+        "names": [],
+        "executionBehavior": "parallel",
+        "waitBehavior": "none",
+        "waitTime": 0
+      }
+    }
+  ],
+  "markers": []
+}

src/main/java/frc/robot/Robot.java

@@ -7,6 +7,7 @@
 import edu.wpi.first.wpilibj.TimedRobot;
 import edu.wpi.first.wpilibj2.command.CommandScheduler;
 import edu.wpi.first.wpilibj2.command.button.CommandXboxController;
+import frc.robot.commands.Autonomous;
 import frc.robot.commands.DriveTeleop;
 import frc.robot.subsystems.Drive;
 
@@ -26,6 +27,9 @@ public class Robot extends TimedRobot {
     // Xbox Controller
     private final CommandXboxController controller_ = new CommandXboxController(0);
 
+    // Autonomous
+    private final Autonomous auto_ = new Autonomous(drive_);
+
     @Override
     public void robotInit() {
         drive_.setDefaultCommand(new DriveTeleop(drive_, robot_state_, controller_));
@@ -38,10 +42,15 @@ public void robotPeriodic() {
     }
 
     @Override
-    public void autonomousInit() {}
+    public void autonomousInit() {
+        // Command that does Path following
+        auto_.run();
+    }
 
     @Override
-    public void autonomousPeriodic() {}
+    public void autonomousPeriodic() {
+        auto_.simulationPeriodic();
+    }
 
     @Override
     public void teleopInit() {

src/main/java/frc/robot/RobotState.java

@@ -2,6 +2,7 @@
 
 import edu.wpi.first.math.estimator.SwerveDrivePoseEstimator;
 import edu.wpi.first.math.geometry.Pose2d;
+import edu.wpi.first.math.geometry.Rotation2d;
 import frc.robot.subsystems.Drive;
 
 public class RobotState {
@@ -38,6 +39,13 @@ public double getDegree() {
         return pose_estimator_.getEstimatedPosition().getRotation().getDegrees();
     }
 
+    // Get Rotation2d
+    public Rotation2d getRotation2d() {
+        return pose_estimator_.getEstimatedPosition().getRotation();
+    }
+
     // Reset Position
-    public void reset(Pose2d pose) {}
+    public void resetPose(Pose2d pose) {
+        pose_estimator_.resetPosition(getRotation2d(), drive_.getSwerveModulePositions(), pose);
+    }
 }

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

@@ -0,0 +1,56 @@
+package frc.robot.commands;
+
+import com.pathplanner.lib.PathConstraints;
+import com.pathplanner.lib.PathPlanner;
+import com.pathplanner.lib.PathPlannerTrajectory;
+import com.pathplanner.lib.PathPlannerTrajectory.PathPlannerState;
+
+import edu.wpi.first.wpilibj.Timer;
+import edu.wpi.first.wpilibj.smartdashboard.Field2d;
+import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
+import edu.wpi.first.wpilibj2.command.Command;
+import frc.robot.subsystems.Drive;
+
+public class Autonomous {
+    // Instance variables
+    private final Drive drive_;
+    private final DriveTrajectory drive_traj_;
+    private final Field2d field_ = new Field2d();
+    private final PathPlannerTrajectory trajectory = PathPlanner.loadPath(
+        "path", new PathConstraints(4, 3));
+
+    // Constructor
+    public Autonomous(Drive drive) {
+        // Initialize the field in the sim
+        // TO DO probably need to change this to be instantiated in the robot class
+        SmartDashboard.putData("Field", field_);
+
+        drive_ = drive;
+        drive_traj_ = new DriveTrajectory(drive_);
+    }
+
+    // Methods
+    public PathPlannerTrajectory getTrajectory(){
+        return trajectory;
+    }
+
+    public Command run() {
+        return drive_traj_.followTrajectoryCommand(getTrajectory(), true);
+    }
+
+    public void simulationPeriodic() {
+        // Timer 
+        Timer timer = new Timer();
+
+        timer.start();
+        // Sample the trajectory
+        while (timer.get() <= trajectory.getTotalTimeSeconds()){
+            // Get the state of the robot at the current time
+            // Ensure it is a PathPlannerState because only then will it have the holonomicRotation
+            PathPlannerState state = (PathPlannerState) trajectory.sample(timer.get());
+
+            // Set Robot position in simulation
+            field_.setRobotPose(state.poseMeters.getX(), state.poseMeters.getY(), state.holonomicRotation);
+        }
+    }
+}

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

@@ -0,0 +1,57 @@
+package frc.robot.commands;
+
+import com.pathplanner.lib.PathPlannerTrajectory;
+import com.pathplanner.lib.commands.PPSwerveControllerCommand;
+
+import edu.wpi.first.math.controller.PIDController;
+import edu.wpi.first.wpilibj2.command.Command;
+import edu.wpi.first.wpilibj2.command.InstantCommand;
+import edu.wpi.first.wpilibj2.command.SequentialCommandGroup;
+import frc.robot.RobotState;
+import frc.robot.subsystems.Drive;
+
+public class DriveTrajectory {
+    // Instance variables
+    // private final Autonomous auto_path_ = new Autonomous();
+    private final Drive drive_;
+    private final RobotState robot_state_;
+
+    public DriveTrajectory(Drive drive){
+        drive_ = drive;
+        robot_state_ = new RobotState(drive_);
+    }
+
+
+    // Method to run Holonomic Trajectory path following
+    // Need to make it look cleaner but its proof of concept
+    public Command followTrajectoryCommand(PathPlannerTrajectory traj, boolean isFirstPath){
+        return new SequentialCommandGroup(
+            new InstantCommand(() -> {
+                if(isFirstPath)
+                    robot_state_.resetPose(traj.getInitialHolonomicPose());
+            }),
+            new PPSwerveControllerCommand(
+                traj,
+                robot_state_::getPosition,
+                drive_.getKinematics(),
+                Constants.xController,
+                Constants.yController,
+                Constants.thetaController,
+                drive_::setModuleStates,
+                false,
+                drive_
+            )
+        );
+    }
+
+
+    // Constants Class
+    private static class Constants{
+        // NOTE BEFORE RUNNING: CHANGE GAINS!
+        public static final PIDController xController = new PIDController(1, 0, 0);
+        public static final PIDController yController = new PIDController(1, 0, 0);
+        public static final PIDController thetaController = new PIDController(0.5, 0, 0);
+    }
+
+
+}