Merge branch 'main' of https://github.com/StuyPulse/2024-Kitbot

Author: anivanchen

.Glass/glass.json

@@ -22,6 +22,17 @@
         "Launcher": {
           "Amp": {
             "open": true
+          }
+        },
+        "Swerve": {
+          "Modules": {
+            "Back Left": {
+              "open": true
+            },
+            "Back Right": {
+              "open": true
+            },
+            "open": true
           },
           "open": true
         },
@@ -30,7 +41,15 @@
     },
     "types": {
       "/FMSInfo": "FMSInfo",
-      "/SmartDashboard/Autonomous": "String Chooser"
+      "/SmartDashboard/Autonomous": "String Chooser",
+      "/SmartDashboard/Field": "Field2d"
+    },
+    "windows": {
+      "/SmartDashboard/Autonomous": {
+        "window": {
+          "visible": true
+        }
+      }
     }
   },
   "NetworkTables Info": {
@@ -83,5 +102,6 @@
         }
       ]
     }
-  }
+  },
+  "enterKey": 92
 }

.pathplanner/settings.json

@@ -0,0 +1,12 @@
+{
+  "robotWidth": 0.9,
+  "robotLength": 0.9,
+  "holonomicMode": false,
+  "pathFolders": [],
+  "autoFolders": [],
+  "defaultMaxVel": 3.0,
+  "defaultMaxAccel": 3.0,
+  "defaultMaxAngVel": 540.0,
+  "defaultMaxAngAccel": 720.0,
+  "maxModuleSpeed": 4.5
+}

build.gradle

@@ -1,6 +1,6 @@
 plugins {
     id "java"
-    id "edu.wpi.first.GradleRIO" version "2024.1.1"
+    id "edu.wpi.first.GradleRIO" version "2024.2.1"
     id "com.diffplug.spotless" version "6.22.0"
 }
 

src/main/deploy/pathplanner/autos/One Piece.auto

@@ -0,0 +1,31 @@
+{
+  "version": 1.0,
+  "startingPose": null,
+  "command": {
+    "type": "sequential",
+    "data": {
+      "commands": [
+        {
+          "type": "path",
+          "data": {
+            "pathName": "1 Note Part 1"
+          }
+        },
+        {
+          "type": "path",
+          "data": {
+            "pathName": "1 Note Part 2"
+          }
+        },
+        {
+          "type": "named",
+          "data": {
+            "name": "LauncherLaunch"
+          }
+        }
+      ]
+    }
+  },
+  "folder": null,
+  "choreoAuto": false
+}

src/main/deploy/pathplanner/navgrid.json

@@ -0,0 +1 @@
+{"field_size":{"x":16.54,"y":8.21},"nodeSizeMeters":0.3,"grid":[[true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true],[true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true],[true,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,true,true],[true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true],[true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true],[true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true],[true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true],[true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true],[true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true],[true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true],[true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true],[true,true,false,false,false,false,false,false,false,false,true,true,true,false,false,false,false,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,false,false,false,false,true,true,true,false,false,false,false,false,false,false,false,false,true,true],[true,true,false,false,false,false,false,false,false,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,false,false,false,false,false,false,false,false,true,true],[true,true,false,false,false,false,false,false,false,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,false,false,false,false,false,false,false,true,true,true],[true,true,true,false,false,false,false,false,false,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,false,false,false,false,false,false,true,true,true,true],[true,true,true,true,false,false,false,false,false,false,true,true,true,false,false,false,false,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,false,false,false,false,true,true,true,false,false,false,false,false,false,true,true,true,true,true],[true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,true],[true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,true],[true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,true],[true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,true],[true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,true],[true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true],[true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true],[true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true],[true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true],[true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true],[true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true],[true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true]]}

src/main/deploy/pathplanner/paths/1 Note Part 1.path

@@ -0,0 +1,49 @@
+{
+  "version": 1.0,
+  "waypoints": [
+    {
+      "anchor": {
+        "x": 0.8574827844100136,
+        "y": 4.028517436664504
+      },
+      "prevControl": null,
+      "nextControl": {
+        "x": 0.8617584596280307,
+        "y": 4.017650095485377
+      },
+      "isLocked": false,
+      "linkedName": null
+    },
+    {
+      "anchor": {
+        "x": 2.8779768624352067,
+        "y": 4.781214428169598
+      },
+      "prevControl": {
+        "x": 2.0779768624352064,
+        "y": 4.781214428169598
+      },
+      "nextControl": null,
+      "isLocked": false,
+      "linkedName": null
+    }
+  ],
+  "rotationTargets": [],
+  "constraintZones": [],
+  "eventMarkers": [],
+  "globalConstraints": {
+    "maxVelocity": 3.0,
+    "maxAcceleration": 3.0,
+    "maxAngularVelocity": 540.0,
+    "maxAngularAcceleration": 720.0
+  },
+  "goalEndState": {
+    "velocity": 0,
+    "rotation": 0,
+    "rotateFast": false
+  },
+  "reversed": false,
+  "folder": null,
+  "previewStartingState": null,
+  "useDefaultConstraints": false
+}

src/main/deploy/pathplanner/paths/1 Note Part 2.path

@@ -0,0 +1,49 @@
+{
+  "version": 1.0,
+  "waypoints": [
+    {
+      "anchor": {
+        "x": 2.88,
+        "y": 4.78
+      },
+      "prevControl": null,
+      "nextControl": {
+        "x": 2.235378507794053,
+        "y": 4.985437304207942
+      },
+      "isLocked": false,
+      "linkedName": null
+    },
+    {
+      "anchor": {
+        "x": 1.3921797241742635,
+        "y": 5.5760149436965545
+      },
+      "prevControl": {
+        "x": 2.4431478839872605,
+        "y": 5.5760149436965545
+      },
+      "nextControl": null,
+      "isLocked": false,
+      "linkedName": null
+    }
+  ],
+  "rotationTargets": [],
+  "constraintZones": [],
+  "eventMarkers": [],
+  "globalConstraints": {
+    "maxVelocity": 3.0,
+    "maxAcceleration": 3.0,
+    "maxAngularVelocity": 540.0,
+    "maxAngularAcceleration": 720.0
+  },
+  "goalEndState": {
+    "velocity": 0,
+    "rotation": -50.079173387765984,
+    "rotateFast": false
+  },
+  "reversed": true,
+  "folder": null,
+  "previewStartingState": null,
+  "useDefaultConstraints": false
+}

src/main/java/com/stuypulse/robot/RobotContainer.java

@@ -5,9 +5,12 @@
 
 package com.stuypulse.robot;
 
+import com.pathplanner.lib.auto.AutoBuilder;
+import com.pathplanner.lib.auto.NamedCommands;
 import com.stuypulse.robot.commands.auton.DoNothingAuton;
 import com.stuypulse.robot.commands.drivetrain.DrivetrainDrive;
 import com.stuypulse.robot.commands.launcher.LaunchPrepare;
+import com.stuypulse.robot.commands.launcher.LaunchPrepare;
 import com.stuypulse.robot.commands.launcher.LauncherHoldSpeed;
 import com.stuypulse.robot.commands.launcher.LauncherLaunchSpeaker;
 import com.stuypulse.robot.commands.launcher.LauncherIntakeNote;
@@ -44,6 +47,7 @@ public class RobotContainer {
     public RobotContainer() {
         configureDefaultCommands();
         configureButtonBindings();
+        configureNamedCommands();
         configureAutons();
     }
 
@@ -56,6 +60,14 @@ private void configureDefaultCommands() {
         launcher.setDefaultCommand(new LauncherHoldSpeed(Settings.Launcher.LAUNCHER_SPEAKER_SPEED));
     }
 
+    /**********************/
+    /*** NAMED COMMANDS ***/
+    /**********************/
+
+    private void configureNamedCommands() {
+        NamedCommands.registerCommand("LauncherLaunch", new LauncherLaunchSpeaker());
+    }
+
     /***************/
     /*** BUTTONS ***/
     /***************/
@@ -70,7 +82,7 @@ private void configureDriverBindings() {
             .whileTrue(new LauncherIntakeNote());
 
         driver.getBottomButton()
-            .whileTrue(new LaunchPrepare(Settings.Launcher.LAUNCHER_AMP_SPEED, Settings.Launcher.AMP_THRESHOLD_RPM).andThen(new LauncherLaunch(Settings.Launcher.FEEDER_AMP_SPEED, Settings.Launcher.LAUNCHER_AMP_SPEED)));
+            .whileTrue(new LaunchPrepare(Settings.Launcher.LAUNCHER_SPEAKER_SPEED, Settings.Launcher.SPEAKER_THRESHOLD_RPM).andThen(new LauncherLaunchSpeaker()));
     }
 
     private void configureOperatorBindings() {}
@@ -80,7 +92,7 @@ private void configureOperatorBindings() {}
     /**************/
 
     public void configureAutons() {
-        autonChooser.setDefaultOption("Do Nothing", new DoNothingAuton());
+        autonChooser = AutoBuilder.buildAutoChooser();
 
         SmartDashboard.putData("Autonomous", autonChooser);
     }

src/main/java/com/stuypulse/robot/constants/Settings.java

@@ -49,10 +49,10 @@ public interface Launcher {
         double LAUNCHER_INTAKE_SPEED = -1;
         double FEEDER_INTAKE_SPEED = -0.2;
 
-        SmartNumber AMP_THRESHOLD_RPM = new SmartNumber("Launcher/Amp/Threshold RPM", 4000);
+        SmartNumber AMP_THRESHOLD_RPM = new SmartNumber("Launcher/Amp/Threshold RPM", 2250);
 
-        SmartNumber LAUNCHER_AMP_SPEED = new SmartNumber("Launcher/Amp/Launcher Speed", 0.7);
-        SmartNumber FEEDER_AMP_SPEED = new SmartNumber("Launcher/Amp/Feeder Speed", 0.7);
+        SmartNumber LAUNCHER_AMP_SPEED = new SmartNumber("Launcher/Amp/Launcher Speed", 0.4);
+        SmartNumber FEEDER_AMP_SPEED = new SmartNumber("Launcher/Amp/Feeder Speed", 0.4);
     }
 
     public interface Alignment {

src/main/java/com/stuypulse/robot/subsystems/drivetrain/AbstractDrivetrain.java

@@ -2,6 +2,7 @@
 
 import com.stuypulse.stuylib.math.Angle;
 
+import edu.wpi.first.math.geometry.Rotation2d;
 import edu.wpi.first.wpilibj.RobotBase;
 import edu.wpi.first.wpilibj2.command.SubsystemBase;
 

src/main/java/com/stuypulse/robot/subsystems/drivetrain/Drivetrain.java

@@ -7,13 +7,27 @@
 import com.stuypulse.robot.constants.Ports;
 import com.stuypulse.robot.constants.Settings;
 import com.stuypulse.stuylib.math.Angle;
-
+import com.pathplanner.lib.auto.AutoBuilder;
+import com.pathplanner.lib.util.ReplanningConfig;
+
+import edu.wpi.first.math.geometry.Pose2d;
+import edu.wpi.first.math.geometry.Rotation2d;
+import edu.wpi.first.math.kinematics.ChassisSpeeds;
+import edu.wpi.first.math.kinematics.DifferentialDriveKinematics;
+import edu.wpi.first.math.kinematics.DifferentialDriveOdometry;
+import edu.wpi.first.math.kinematics.DifferentialDriveWheelPositions;
+import edu.wpi.first.math.util.Units;
+import edu.wpi.first.wpilibj.DriverStation;
 import edu.wpi.first.wpilibj.drive.DifferentialDrive;
 import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
 
 public class Drivetrain extends AbstractDrivetrain {
+
     private final DifferentialDrive drivetrain;
 
+    private final DifferentialDriveKinematics kinematics;
+    private final DifferentialDriveOdometry odometry;
+
     private final CANSparkMax leftFront;
     private final CANSparkMax leftBack;
     private final CANSparkMax rightFront;
@@ -36,6 +50,25 @@ public Drivetrain() {
         Motors.Drivetrain.RIGHT.configure(rightBack);
 
         drivetrain = new DifferentialDrive(leftFront, rightFront);
+
+        kinematics = new DifferentialDriveKinematics(Units.inchesToMeters(24));
+        odometry = new DifferentialDriveOdometry(new Rotation2d(), getLeftDistance(), getRightDistance());
+
+        AutoBuilder.configureRamsete(
+            this::getPose,
+            this::resetPose,
+            this::getCurrentSpeeds,
+            this::drive,
+            new ReplanningConfig(),
+            () -> {
+                var alliance = DriverStation.getAlliance();
+                if (alliance.isPresent()) {
+                    return alliance.get() == DriverStation.Alliance.Red;
+                }
+                return false;
+            },
+            this
+        );
     }
 
     //********** GETTERS **********//
@@ -74,8 +107,28 @@ public double getDistance() {
     public Angle getAngle() {
         return Angle.fromDegrees(Math.toDegrees(getLeftDistance() - getRightDistance() / Settings.Drivetrain.TRACK_WIDTH));
     }
-    
-    //********** Drive Methods **********//
+
+    private DifferentialDriveWheelPositions getWheelPositions() {
+        return new DifferentialDriveWheelPositions(getLeftDistance(), getRightDistance());
+    }
+ 
+    public Pose2d getPose() {
+        return odometry.getPoseMeters();
+    }
+
+    public void resetPose(Pose2d pose) {
+        odometry.resetPosition(new Rotation2d(), getWheelPositions(), pose);
+    }
+
+    private double getAngularVelocity() {
+        return (getLeftSpeed() - getRightSpeed()) / Units.inchesToMeters(26);
+    }
+
+    public ChassisSpeeds getCurrentSpeeds() {
+        return new ChassisSpeeds(getVelocity(), 0, getAngularVelocity());
+    }
+
+     //********** Drive Methods **********//
     public void tankDriveVolts(double leftVolts, double rightVolts) {
         leftFront.setVoltage(leftVolts);
         rightFront.setVoltage(rightVolts);
@@ -94,6 +147,10 @@ public void curvatureDrive(double speed, double rotation, boolean isQuickTurn) {
         drivetrain.curvatureDrive(speed, rotation, isQuickTurn);
     }
 
+    public void drive(ChassisSpeeds speeds) {
+        drivetrain.arcadeDrive(speeds.vxMetersPerSecond, speeds.omegaRadiansPerSecond);
+    }
+
     public CANSparkMax[] getMotors() {
         return new CANSparkMax[] {
             rightFront, leftFront, rightBack, leftBack
@@ -122,6 +179,8 @@ public void stop() {
 
     @Override
     public void periodicChild() {
+        odometry.update(new Rotation2d(), getWheelPositions());
+
         SmartDashboard.putNumber("Drivetrain/Left Speed", getLeftSpeed());
         SmartDashboard.putNumber("Drivetrain/Right Speed", getRightSpeed());