Merge pull request #20 from TotalTaxAmount/on-the-fly-path

Fix swerve values, and add OTFP (still WIP)

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

@@ -4,13 +4,19 @@
 
 package frc.robot;
 
+import edu.wpi.first.math.VecBuilder;
+import edu.wpi.first.math.estimator.SwerveDrivePoseEstimator;
+import edu.wpi.first.math.geometry.Pose2d;
+import edu.wpi.first.math.kinematics.SwerveModulePosition;
+import edu.wpi.first.math.util.Units;
 import edu.wpi.first.wpilibj.XboxController;
 import edu.wpi.first.wpilibj2.command.Command;
 import edu.wpi.first.wpilibj2.command.InstantCommand;
 import edu.wpi.first.wpilibj2.command.RunCommand;
 import edu.wpi.first.wpilibj2.command.button.JoystickButton;
 import frc.robot.commands.DriveCommands;
-import frc.robot.commands.SetLEDCommand;
+
+import frc.robot.commands.PathCommand;
 import frc.robot.constants.DrivetrainConstants;
 import frc.robot.constants.LEDConstants;
 import frc.robot.subsystems.SwerveSubsystem;
@@ -28,6 +34,8 @@ public class RobotContainer {
   public XboxController secondaryController = new XboxController(1);
   public SwerveSubsystem swerveSubsystem = new SwerveSubsystem();
 
+
+
   /** The container for the robot. Contains subsystems, OI devices, and commands. */
   public RobotContainer() {
     // Configure the button bindings
@@ -50,16 +58,11 @@ public void configureButtonBindings(){
               swerveSubsystem.zeroGyro();
             })
     );
+
+
     new JoystickButton(primaryController, XboxController.Button.kA.value).whileTrue(
-            new RunCommand(() -> {
-              SetLEDCommand.run(LEDConstants.Status.TEST_1);
-            })
-    );
-    new JoystickButton(primaryController, XboxController.Button.kB.value).whileTrue(
-            new RunCommand(() -> {
-              SetLEDCommand.run(LEDConstants.Status.TEST_2);
-            })
-    );
+            new PathCommand(swerveSubsystem)
+
   }
   /**
    * Use this to pass the autonomous command to the main {@link Robot} class.

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

@@ -0,0 +1,55 @@
+package frc.robot.commands;
+
+import com.pathplanner.lib.auto.AutoBuilder;
+import com.pathplanner.lib.path.GoalEndState;
+import com.pathplanner.lib.path.PathConstraints;
+import com.pathplanner.lib.path.PathPlannerPath;
+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.units.Units;
+import edu.wpi.first.wpilibj2.command.Command;
+import frc.robot.subsystems.SwerveSubsystem;
+
+import java.util.List;
+
+public class PathCommand extends Command {
+
+    private final SwerveSubsystem swerveSubsystem;
+
+    private PathPlannerPath path;
+
+    public PathCommand(SwerveSubsystem swerveSubsystem) {
+        this.swerveSubsystem = swerveSubsystem;
+    }
+
+    @Override
+    public void initialize() {
+        Pose2d startPos = new Pose2d(
+                swerveSubsystem.getPoseEstimator().getEstimatedPosition().getTranslation(),
+                new Rotation2d()
+        );
+
+        Pose2d endPos = new Pose2d(startPos.getTranslation().plus(new Translation2d(0.5, 1)), new Rotation2d());
+
+        List<Translation2d> bezierPoints = PathPlannerPath.bezierFromPoses(startPos, endPos);
+        path = new PathPlannerPath(
+                bezierPoints,
+                new PathConstraints(
+                    4.0, 4.0,
+                        2 * Math.PI, 2.5 * Math.PI
+                ),
+                new GoalEndState(0.0, Rotation2d.fromDegrees(90))
+        );
+
+        path.preventFlipping = true;
+
+        AutoBuilder.followPath(path).schedule();
+
+    }
+
+//    @Override
+//    public void execute() {
+//    }
+
+}

src/main/java/frc/robot/constants/DrivetrainConstants.java

@@ -7,7 +7,7 @@
 
 public class DrivetrainConstants {
 
-    public static final double maxSpeedMetersPerSecond = 4.0;
+    public static final double maxSpeedMetersPerSecond = 1.0;
     public static final double maxAngularSpeed = Math.PI;
 
     public static final double directionSlewRate = 4.0; // rads/sec - turning
@@ -27,20 +27,20 @@ public class DrivetrainConstants {
             new Translation2d(-wheelBase / 2, -trackWidth / 2)
     );
 
-    public static final double frontLeftChassisAngularOffset = -0.57 ;
-    public static final double frontRightChassisAngularOffset = 0.874;
-    public static final double rearLeftChassisAngularOffset = 6.098 + Math.PI;
-    public static final double rearRightChassisAngularOffset = 5.52 + Math.PI/2;
+    public static final double frontLeftChassisAngularOffset = 5.516 + Math.PI/2 + Math.PI;
+    public static final double frontRightChassisAngularOffset = 6.107;
+    public static final double rearLeftChassisAngularOffset = 0.876 + Math.PI;
+    public static final double rearRightChassisAngularOffset = 1.036 + Math.PI/2;
 
-    public static final int frontLeftDrivingPort = 1;
+    public static final int frontLeftDrivingPort = 7;
     public static final int rearLeftDrivingPort = 5;
     public static final int frontRightDrivingPort = 3;
-    public static final int rearRightDrivingPort = 7;
+    public static final int rearRightDrivingPort = 1;
 
-    public static final int frontLeftTurningPort = 2;
+    public static final int frontLeftTurningPort = 8;
     public static final int rearLeftTurningPort = 6;
     public static final int frontRightTurningPort = 4;
-    public static final int rearRightTurningPort = 8;
+    public static final int rearRightTurningPort = 2;
 
     public static final boolean gyroReversed = false;
     public static final boolean turningEncoderReversed = true;

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

@@ -1,6 +1,10 @@
 package frc.robot.subsystems;
 
 import com.kauailabs.navx.frc.AHRS;
+import com.pathplanner.lib.auto.AutoBuilder;
+import com.pathplanner.lib.util.HolonomicPathFollowerConfig;
+import com.pathplanner.lib.util.PIDConstants;
+import com.pathplanner.lib.util.ReplanningConfig;
 import edu.wpi.first.math.VecBuilder;
 import edu.wpi.first.math.estimator.SwerveDrivePoseEstimator;
 import edu.wpi.first.math.filter.SlewRateLimiter;
@@ -15,6 +19,7 @@
 import edu.wpi.first.networktables.NetworkTable;
 import edu.wpi.first.networktables.NetworkTableInstance;
 import edu.wpi.first.util.WPIUtilJNI;
+import edu.wpi.first.wpilibj.DriverStation;
 import edu.wpi.first.wpilibj.Timer;
 import edu.wpi.first.wpilibj2.command.SubsystemBase;
 import frc.robot.constants.DrivetrainConstants;
@@ -24,6 +29,8 @@
 
 import frc.robot.utils.VisionUtils;
 
+import java.util.Optional;
+
 public class SwerveSubsystem extends SubsystemBase {
     // Defining Motors
     private final SwerveModuleControlller frontLeft = new SwerveModuleControlller(
@@ -63,17 +70,13 @@ public class SwerveSubsystem extends SubsystemBase {
     private final SlewRateLimiter rotationLimiter = new SlewRateLimiter(DrivetrainConstants.rotationalSlewRate);
 
     // Slew Rate Time
-
     private double previousTime = WPIUtilJNI.now() * 1e-6;
 
     // Limelight Network Table
     // Relay data to driverstation using network table
     private final NetworkTableUtils limelightTable = new NetworkTableUtils("limelight");
 
-    // Convert Gyro angle to radians(-2pi to 2pi)
-    public double heading() {
-        return Units.degreesToRadians(-1 * (gyro.getAngle() + 180.0) % 360.0);
-    }
+    // Convert Gyro angle to radians(-2pi to 2pi
 
     // Swerve Odometry
     /*
@@ -90,6 +93,7 @@ public double heading() {
      */
 
     private double distanceToTag = 1.0;
+
     private final SwerveDrivePoseEstimator poseEstimator = new SwerveDrivePoseEstimator(
             DrivetrainConstants.driveKinematics,
             gyro.getRotation2d(),
@@ -133,6 +137,27 @@ public double heading() {
     private final DoubleEntry rearleftpos = NetworkTableInstance.getDefault()
             .getTable("Swerve").getDoubleTopic("rlpos").getEntry(rearLeft.getPosition().angle.getRadians());
 
+    public SwerveSubsystem() {
+        // PathPlanner stuff
+        AutoBuilder.configureHolonomic(
+                this::getPose,
+                this::resetOdometry,
+                this::getRobotRelativeSpeeds,
+                this::driveRobotRelative,
+                new HolonomicPathFollowerConfig(
+                        new PIDConstants(0.2, 0.0, 0.0),
+                        new PIDConstants(0.2, 0.0, 0.0),
+                        0.4,
+                         Units.inchesToMeters(14.4),
+                        new ReplanningConfig()
+                ),
+            () -> DriverStation.getAlliance().filter(value -> value != DriverStation.Alliance.Red).isPresent(),
+                this
+
+        );
+    }
+
+
     // Periodic
     @Override
     public void periodic() {
@@ -211,11 +236,50 @@ public void periodic() {
     /**
      * Get robot's pose.
      */
-    private Pose2d getPose() {
+    public Pose2d getPose() {
         return poseEstimator.getEstimatedPosition();
     }
 
-    // Reset odometry function
+    /**
+     * Get current heading of robot
+     * @return Heading of robot in radians
+     */
+    public double heading() {
+        return Units.degreesToRadians(-1 * (gyro.getAngle() + 180.0) % 360.0);
+    }
+
+    /**
+     * Get the pose estimator instance
+     * @return The current pose estimator
+     */
+    public SwerveDrivePoseEstimator getPoseEstimator() {
+        return this.poseEstimator;
+    }
+
+    /**
+     * Drive the robot with {@link ChassisSpeeds} (mainly used for path planner)
+     * @param chassisSpeeds {@link ChassisSpeeds} object
+     */
+    public void driveRobotRelative(ChassisSpeeds chassisSpeeds) {
+        double forward = -chassisSpeeds.vxMetersPerSecond;
+        double sideways = chassisSpeeds.vyMetersPerSecond;
+        double rotation = chassisSpeeds.omegaRadiansPerSecond;
+
+        drive(forward, sideways, rotation, true, true);
+    }
+
+    /**
+     * Get the speed of the chassis relative to the robot
+     * @return {@link ChassisSpeeds} of the current robots speed
+     */
+    public ChassisSpeeds getRobotRelativeSpeeds() {
+        return DrivetrainConstants.driveKinematics.toChassisSpeeds(
+                frontLeft.getState(),
+                frontRight.getState(),
+                rearLeft.getState(),
+                rearRight.getState()
+        );
+    }
 
     /**
      * @param pose Reset robot's position.