auto align, but somewhat competent

grt192 · Feb 9, 2024 · 6eb28d9 · 6eb28d9
1 parent 1555f79
commit 6eb28d9
Show file tree

Hide file tree

Showing 5 changed files with 157 additions and 1 deletion.
diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java
@@ -4,6 +4,8 @@
 
 package frc.robot;
 
+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.math.util.Units;
 
@@ -122,6 +124,29 @@ public static class ClimbConstants {
     public static final double MAX_WINCH_POWER = 0.6;
   }
 
+  public static class AutoAlignConstants {
+
+    // X: APRILTAG_POS + SUBWOOFER_OFFSET + ROBOT_X_CENTER
+    // ROT: rotated 180 deg from Apriltag pos since robots shoots from back
+    public static final Pose2d BLUE_SPEAKER_POSE = new Pose2d(Units.inchesToMeters(-1.50 + 37.711 + 0), Units.inchesToMeters(218.42), Rotation2d.fromDegrees(0 + 180)); 
+
+    // Source position closer to the centerline
+    // Y: APRILTAG_POS 
+    public static final Pose2d BLUE_SOURCE_POSE = new Pose2d(Units.inchesToMeters(593.68), Units.inchesToMeters(9.68), Rotation2d.fromDegrees(120)); 
+
+    public static final Pose2d BLUE_AMP_POSE = new Pose2d(Units.inchesToMeters(72.5), Units.inchesToMeters(323.00), Rotation2d.fromDegrees(270));
+
+    // X: APRILTAG_POS + SUBWOOFER_OFFSET + ROBOT_X_CENTER
+    // ROT: rotated 180 deg from Apriltag pos since robots shoots from back
+    public static final Pose2d RED_SPEAKER_POSE = new Pose2d(Units.inchesToMeters(652.73 - 37.711 - 0), Units.inchesToMeters(218.42), Rotation2d.fromDegrees(180 - 180));
+
+    // Source position closer to the centerline
+    // Y: APRILTAG_POS 
+    public static final Pose2d RED_SOURCE_POSE = new Pose2d(Units.inchesToMeters(57.54), Units.inchesToMeters(9.68), Rotation2d.fromDegrees(60)); 
+
+    public static final Pose2d RED_AMP_POSE = new Pose2d(Units.inchesToMeters(578.77), Units.inchesToMeters(323.00), Rotation2d.fromDegrees(270));
+  }
+
   public static class LEDConstants {
     public static final int LED_LENGTH = 140;
     public static final int LED_PWM_PORT = 0;

diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java
@@ -4,6 +4,7 @@
 
 package frc.robot;
 
+import frc.robot.Constants.AutoAlignConstants;
 import frc.robot.Constants.OperatorConstants;
 import frc.robot.subsystems.shooter.ShooterFeederSubsystem;
 import frc.robot.subsystems.shooter.ShooterFlywheelSubsystem;
@@ -15,6 +16,7 @@
 import frc.robot.controllers.DualJoystickDriveController;
 import frc.robot.controllers.XboxDriveController;
 import frc.robot.commands.IdleCommand;
+import frc.robot.commands.align.AlignCommand;
 import frc.robot.commands.climb.ClimbLowerCommand;
 import frc.robot.commands.climb.ClimbRaiseCommand;
 import frc.robot.commands.elevator.ElevatorToAMPCommand;
@@ -50,6 +52,7 @@
 import edu.wpi.first.wpilibj.DriverStation;
 import edu.wpi.first.wpilibj.GenericHID;
 import edu.wpi.first.wpilibj.XboxController;
+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.InstantCommand;
@@ -132,6 +135,8 @@ public RobotContainer() {
         // module = new SwerveModule(6, 7, 0);
         // baseSwerveSubsystem = new TestSingleModuleSwerveSubsystem(module);
 
+        SmartDashboard.putData("AUTO ALIGN BLUE AMP", AlignCommand.getAlignCommand(AutoAlignConstants.BLUE_AMP_POSE));
+
         camera1 = new UsbCamera("camera1", 0);
         camera1.setFPS(60);
         camera1.setBrightness(45);

diff --git a/src/main/java/frc/robot/commands/align/AlignCommand.java b/src/main/java/frc/robot/commands/align/AlignCommand.java
@@ -0,0 +1,32 @@
+package frc.robot.commands.align;
+
+import com.pathplanner.lib.auto.AutoBuilder;
+import com.pathplanner.lib.path.PathConstraints;
+
+import edu.wpi.first.math.geometry.Pose2d;
+import edu.wpi.first.math.geometry.Rotation2d;
+import edu.wpi.first.math.util.Units;
+import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
+import edu.wpi.first.wpilibj2.command.Command;
+
+public class AlignCommand {
+
+    public AlignCommand(){
+
+
+    }
+
+    static public Command getAlignCommand(Pose2d targetPose){
+
+        return  AutoBuilder.pathfindToPose(
+            targetPose, 
+            new PathConstraints(
+        4.0, 4.0, 
+                Units.degreesToRadians(360), Units.degreesToRadians(540)
+            ), 
+    0, 
+    2.0
+        );
+    }
+
+}
diff --git a/src/main/java/frc/robot/subsystems/swerve/SwerveSubsystem.java b/src/main/java/frc/robot/subsystems/swerve/SwerveSubsystem.java
@@ -9,12 +9,14 @@
 import edu.wpi.first.math.kinematics.SwerveDriveKinematics;
 import edu.wpi.first.math.kinematics.SwerveModulePosition;
 import edu.wpi.first.math.kinematics.SwerveModuleState;
+import edu.wpi.first.wpilibj.DriverStation;
 import edu.wpi.first.wpilibj.SPI;
 import edu.wpi.first.wpilibj.Timer;
 import edu.wpi.first.wpilibj.shuffleboard.Shuffleboard;
 import edu.wpi.first.wpilibj.shuffleboard.ShuffleboardTab;
 import edu.wpi.first.wpilibj.smartdashboard.Field2d;
 import edu.wpi.first.wpilibj2.command.SubsystemBase;
+import frc.robot.Constants;
 
 import static frc.robot.Constants.SwerveConstants.*;
 
@@ -27,6 +29,12 @@
 import edu.wpi.first.networktables.NetworkTable;
 import edu.wpi.first.networktables.NetworkTableInstance;
 
+import com.pathplanner.lib.auto.AutoBuilder;
+import com.pathplanner.lib.util.HolonomicPathFollowerConfig;
+import com.pathplanner.lib.util.PIDConstants;
+import com.pathplanner.lib.util.ReplanningConfig;
+
+
 public class SwerveSubsystem extends BaseSwerveSubsystem{
     private final AHRS ahrs;
 
@@ -117,6 +125,33 @@ public SwerveSubsystem() {
             // Vision measurement standard deviations: [X, Y, theta]
             MatBuilder.fill(Nat.N3(), Nat.N1(), 0.1, 0.1, 0.01)
         );
+
+        // Configure AutoBuilder
+        AutoBuilder.configureHolonomic(
+            this::getRobotPosition, 
+            this::resetPose, 
+            this::getRobotRelativeChassisSpeeds, 
+            this::setRobotRelativeDrivePowers, 
+            new HolonomicPathFollowerConfig( // HolonomicPathFollowerConfig, this should likely live in your Constants class
+                        new PIDConstants(5.0, 0.0, 0.0), // Translation PID constants
+                        new PIDConstants(5.0, 0.0, 0.0), // Rotation PID constants
+                        4.5, // Max module speed, in m/s
+                        0.4, // Drive base radius in meters. Distance from robot center to furthest module.
+                        new ReplanningConfig() // Default path replanning config. See the API for the options here
+                ),
+            () -> {
+                // 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
+    );
     }
 
     public void periodic() {
@@ -180,7 +215,28 @@ public void setRobotRelativeDrivePowers(double xPower, double yPower, double ang
             MAX_VEL, MAX_VEL, MAX_OMEGA);
     }
 
-
+    public void setRobotRelativeDrivePowers(ChassisSpeeds robotRelativeSpeeds){
+
+        ChassisSpeeds speeds = ChassisSpeeds.fromRobotRelativeSpeeds(
+            robotRelativeSpeeds,
+            getRobotPosition().getRotation()
+        );
+
+        states = kinematics.toSwerveModuleStates(speeds);
+        SwerveDriveKinematics.desaturateWheelSpeeds(
+            states, speeds,
+            MAX_VEL, MAX_VEL, MAX_OMEGA);
+    }
+
+    public ChassisSpeeds getRobotRelativeChassisSpeeds(){
+
+        ChassisSpeeds robotRelativeSpeeds = ChassisSpeeds.fromFieldRelativeSpeeds(
+            kinematics.toChassisSpeeds(states),
+            getRobotPosition().getRotation() // getGyroHeading()
+        );
+
+        return robotRelativeSpeeds;
+    }
 
     public void setSwerveModuleStates(SwerveModuleState[] states){
         this.states = states;

diff --git a/vendordeps/PathplannerLib.json b/vendordeps/PathplannerLib.json
@@ -0,0 +1,38 @@
+{
+    "fileName": "PathplannerLib.json",
+    "name": "PathplannerLib",
+    "version": "2024.2.3",
+    "uuid": "1b42324f-17c6-4875-8e77-1c312bc8c786",
+    "frcYear": "2024",
+    "mavenUrls": [
+        "https://3015rangerrobotics.github.io/pathplannerlib/repo"
+    ],
+    "jsonUrl": "https://3015rangerrobotics.github.io/pathplannerlib/PathplannerLib.json",
+    "javaDependencies": [
+        {
+            "groupId": "com.pathplanner.lib",
+            "artifactId": "PathplannerLib-java",
+            "version": "2024.2.3"
+        }
+    ],
+    "jniDependencies": [],
+    "cppDependencies": [
+        {
+            "groupId": "com.pathplanner.lib",
+            "artifactId": "PathplannerLib-cpp",
+            "version": "2024.2.3",
+            "libName": "PathplannerLib",
+            "headerClassifier": "headers",
+            "sharedLibrary": false,
+            "skipInvalidPlatforms": true,
+            "binaryPlatforms": [
+                "windowsx86-64",
+                "linuxx86-64",
+                "osxuniversal",
+                "linuxathena",
+                "linuxarm32",
+                "linuxarm64"
+            ]
+        }
+    ]
+}