Autonomous

src/main/deploy/pathplanner/First Test Path.path

@@ -0,0 +1 @@
+{"waypoints":[{"anchorPoint":{"x":3.8047524370910804,"y":3.0374957301638745},"prevControl":null,"nextControl":{"x":4.214656705175479,"y":3.0164749984672383},"holonomicAngle":0.0,"isReversal":false,"velOverride":null,"isLocked":false},{"anchorPoint":{"x":4.340781095355294,"y":3.6155658518213594},"prevControl":{"x":4.372312192900248,"y":3.4473999982482733},"nextControl":{"x":4.372312192900248,"y":3.4473999982482733},"holonomicAngle":0.0,"isReversal":true,"velOverride":null,"isLocked":false},{"anchorPoint":{"x":5.0,"y":3.0},"prevControl":{"x":4.4879262172317445,"y":2.9534128033773315},"nextControl":{"x":5.5120737827682555,"y":3.0465871966226685},"holonomicAngle":0.0,"isReversal":false,"velOverride":null,"isLocked":false},{"anchorPoint":{"x":5.0029341437993216,"y":2.4173841451131177},"prevControl":{"x":5.189186585269722,"y":2.6373923782330317},"nextControl":null,"holonomicAngle":89.09777883846981,"isReversal":false,"velOverride":null,"isLocked":false}],"maxVelocity":1.0,"maxAcceleration":1.0,"isReversed":null}

src/main/java/frc4388/robot/RobotContainer.java

@@ -5,11 +5,24 @@
 package frc4388.robot;
 
 
+import com.pathplanner.lib.PathPlanner;
+import com.pathplanner.lib.PathPlannerTrajectory;
+import com.pathplanner.lib.commands.PPSwerveControllerCommand;
+
+import edu.wpi.first.math.controller.HolonomicDriveController;
+import edu.wpi.first.math.controller.PIDController;
+import edu.wpi.first.math.controller.ProfiledPIDController;
 import edu.wpi.first.math.geometry.Pose2d;
+import edu.wpi.first.math.geometry.Rotation2d;
+import edu.wpi.first.math.trajectory.Trajectory;
+import edu.wpi.first.math.trajectory.TrajectoryConfig;
+import edu.wpi.first.math.trajectory.TrapezoidProfile;
 import edu.wpi.first.wpilibj.Joystick;
 import edu.wpi.first.wpilibj2.command.Command;
 import edu.wpi.first.wpilibj2.command.InstantCommand;
+import edu.wpi.first.wpilibj2.command.RamseteCommand;
 import edu.wpi.first.wpilibj2.command.RunCommand;
+import edu.wpi.first.wpilibj2.command.SwerveControllerCommand;
 import edu.wpi.first.wpilibj2.command.button.JoystickButton;
 
 import frc4388.robot.Constants.*;
@@ -83,7 +96,7 @@ public class RobotContainer {
     
     new JoystickButton(getDriverJoystick(), XboxController.A_BUTTON)
 
-      .whenPressed(() -> resetOdometry());
+      .whenPressed(() -> zeroOdometry(new Pose2d(0, 0, new Rotation2d(0))));
 
     /* Operator Buttons */
     // activates "Lit Mode"
@@ -99,8 +112,41 @@ public class RobotContainer {
    * @return the command to run in autonomous
    */
   public Command getAutonomousCommand() {
-    // no auto
+    // https://github.com/mjansen4857/pathplanner/wiki <-- Pathplanner Wiki
-    return new InstantCommand();
+
+    TrajectoryConfig config = new TrajectoryConfig(1.0, 1.0);
+
+    // PATH PLANNER TRAJECTORY IMPLEMENTATION
+    
+    // PathPlannerTrajectory testFirstPath = PathPlanner.loadPath("First Test Path", 1.0, 1.0); 
+    // PPSwerveControllerCommand command = new PPSwerveControllerCommand(
+    //   testFirstPath, 
+    //   pose, 
+    //   SwerveDrive.m_kinematics, 
+    //   new PIDController(0.5, 0, 0), 
+    //   new PIDController(0.5, 0, 0), 
+    //   new ProfiledPIDController(0.5, 0, 0, new TrapezoidProfile.Constraints(1.0, 1.0)), 
+    //   outputModuleStates, 
+    //   m_robotSwerveDrive
+    // );
+
+    // WPILIB TRAJECTORY IMPLEMENTATION
+    Trajectory testFirstPath = PathPlanner.loadPath("First Test Path", 1.0, 1.0);
+    SwerveControllerCommand swerveControllerCommand = new SwerveControllerCommand(
+      testFirstPath,
+      m_robotSwerveDrive::getOdometry,
+      m_robotSwerveDrive.m_kinematics, 
+      new PIDController(0.5, 1.0, 1.0), 
+      new PIDController(0.5, 1.0, 1.0), 
+      new ProfiledPIDController(0.5, 1.0, 1.0, new TrapezoidProfile.Constraints(1.0, 1.0)), 
+      m_robotSwerveDrive::setModuleStates, 
+      m_robotSwerveDrive
+    );
+
+    m_robotSwerveDrive.resetOdometry(testFirstPath.getInitialPose());
+    return swerveControllerCommand.andThen(() -> m_robotSwerveDrive.driveWithInput(new double[] {0, 0}, 0, true));
+
+    //return new InstantCommand();
   }
 
   /**
@@ -114,8 +160,8 @@ public class RobotContainer {
     return m_robotSwerveDrive.getOdometry();
   }
 
-  public void resetOdometry() {
+  public void zeroOdometry(Pose2d pose) {
-    m_robotSwerveDrive.resetOdometry();
+    m_robotSwerveDrive.resetOdometry(pose);
   }
   /**
    * Add your docs here.

src/main/java/frc4388/robot/subsystems/SwerveDrive.java

@@ -4,8 +4,10 @@
 
 package frc4388.robot.subsystems;
 
+import com.ctre.phoenix.sensors.PigeonIMU;
 import com.ctre.phoenix.sensors.PigeonIMU_StatusFrame;
 import com.ctre.phoenix.sensors.WPI_PigeonIMU;
+import com.ctre.phoenix.sensors.PigeonIMU.FusionStatus;
 
 import edu.wpi.first.math.VecBuilder;
 import edu.wpi.first.math.estimator.SwerveDrivePoseEstimator;
@@ -38,14 +40,15 @@ public class SwerveDrive extends SubsystemBase {
   Translation2d m_backLeftLocation = new Translation2d(Units.inchesToMeters(-halfHeight), Units.inchesToMeters(halfWidth));
   Translation2d m_backRightLocation = new Translation2d(Units.inchesToMeters(-halfHeight), Units.inchesToMeters(-halfWidth));
       
-  private SwerveDriveKinematics m_kinematics = new SwerveDriveKinematics(m_frontLeftLocation, m_frontRightLocation, m_backLeftLocation, m_backRightLocation);
+  public SwerveDriveKinematics m_kinematics = new SwerveDriveKinematics(m_frontLeftLocation, m_frontRightLocation, m_backLeftLocation, m_backRightLocation);
 
   public SwerveModule[] modules;
   public WPI_PigeonIMU m_gyro;
+  protected FusionStatus fstatus = new FusionStatus();
 
   /* Here we use SwerveDrivePoseEstimator so that we can fuse odometry readings. The numbers used
   below are robot specific, and should be tuned. */
-  private SwerveDrivePoseEstimator m_poseEstimator;
+  public SwerveDrivePoseEstimator m_poseEstimator;
 
   public double speedAdjust = SwerveDriveConstants.JOYSTICK_TO_METERS_PER_SECOND_SLOW;
   public boolean ignoreAngles;
@@ -111,11 +114,15 @@ public class SwerveDrive extends SubsystemBase {
               fieldRelative
                 ? ChassisSpeeds.fromFieldRelativeSpeeds(xSpeedMetersPerSecond, ySpeedMetersPerSecond, rot * SwerveDriveConstants.ROTATION_SPEED, m_gyro.getRotation2d())
                 : new ChassisSpeeds(xSpeedMetersPerSecond, ySpeedMetersPerSecond, rot * SwerveDriveConstants.ROTATION_SPEED));
-       SwerveDriveKinematics.desaturateWheelSpeeds(states, Units.feetToMeters(SwerveDriveConstants.MAX_SPEED_FEET_PER_SEC));
+      setModuleStates(states);
-       for (int i = 0; i < states.length; i++) {
+  }
-          SwerveModule module = modules[i];
+
-          SwerveModuleState state = states[i];
+  public void setModuleStates(SwerveModuleState[] desiredStates) {
-          module.setDesiredState(state, ignoreAngles);
+    SwerveDriveKinematics.desaturateWheelSpeeds(desiredStates, Units.feetToMeters(SwerveDriveConstants.MAX_SPEED_FEET_PER_SEC));
+    for (int i = 0; i < desiredStates.length; i++) {
+      SwerveModule module = modules[i];
+      SwerveModuleState state = desiredStates[i];
+      module.setDesiredState(state, ignoreAngles);
     }
   }
   
@@ -123,9 +130,11 @@ public class SwerveDrive extends SubsystemBase {
   public void periodic() {
     //System.err.println(m_gyro.getFusedHeading() +"    aaa");
     updateOdometry();
-    SmartDashboard.putNumber("Pigeon Fused Heading", m_gyro.getFusedHeading());
+    SmartDashboard.putNumber("Pigeon Fused Heading", m_gyro.getFusedHeading(fstatus));
     SmartDashboard.putNumber("Pigeon Yaw", m_gyro.getYaw());
     SmartDashboard.putNumber("Pigeon Get Angle", m_gyro.getAngle());
+    SmartDashboard.putNumber("Pigeon Rotation 2D", m_gyro.getRotation2d().getDegrees());
+    SmartDashboard.putStringArray("Fusion Status", new String[] {"Is Fusing: "+fstatus.bIsFusing, "Is Valid: "+fstatus.bIsValid, "Heading: "+fstatus.heading});
 
     // m_gyro.setStatusFramePeriod(PigeonIMU_StatusFrame.CondStatus_9_SixDeg_YPR, 1, SwerveDriveConstants.SWERVE_TIMEOUT_MS);
     // m_gyro.setStatusFramePeriod(PigeonIMU_StatusFrame.CondStatus_6_SensorFusion, 1, SwerveDriveConstants.SWERVE_TIMEOUT_MS);
@@ -171,8 +180,8 @@ public class SwerveDrive extends SubsystemBase {
   /**
    * Resets the odometry of the robot to (x=0, y=0, theta=0).
    */
-  public void resetOdometry() {
+  public void resetOdometry(Pose2d pose) {
-    m_poseEstimator.resetPosition(new Pose2d(0, 0, new Rotation2d(0)), m_gyro.getRotation2d());
+    m_poseEstimator.resetPosition(pose, m_gyro.getRotation2d());
   }
 
   /** Updates the field relative position of the robot. */

vendordeps/PathplannerLib.json

@@ -0,0 +1,33 @@
+{
+    "fileName": "PathplannerLib.json",
+    "name": "PathplannerLib",
+    "version": "2022.1.0",
+    "uuid": "1b42324f-17c6-4875-8e77-1c312bc8c786",
+    "mavenUrls": [
+        "https://3015rangerrobotics.github.io/pathplannerlib/repo"
+    ],
+    "jsonUrl": "https://3015rangerrobotics.github.io/pathplannerlib/PathplannerLib.json",
+    "javaDependencies": [
+        {
+            "groupId": "com.pathplanner.lib",
+            "artifactId": "PathplannerLib-java",
+            "version": "2022.1.0"
+        }
+    ],
+    "jniDependencies": [],
+    "cppDependencies": [
+        {
+            "groupId": "com.pathplanner.lib",
+            "artifactId": "PathplannerLib-cpp",
+            "version": "2022.1.0",
+            "libName": "PathplannerLib",
+            "headerClassifier": "headers",
+            "sharedLibrary": false,
+            "skipInvalidPlatforms": true,
+            "binaryPlatforms": [
+                "osxx86-64",
+                "linuxathena"
+            ]
+        }
+    ]
+}