feat: add auto

.gitattributes

@@ -0,0 +1,4 @@
+*.gradle text eol=lf
+*.java text eol=lf
+*.md text eol=lf
+*.xml text eol=lf

src/main/deploy/pathplanner/autos/1 Bucket Auto.auto

@@ -0,0 +1,39 @@
+{
+  "version": "2025.0",
+  "command": {
+    "type": "sequential",
+    "data": {
+      "commands": [
+        {
+          "type": "sequential",
+          "data": {
+            "commands": [
+              {
+                "type": "path",
+                "data": {
+                  "pathName": "Top Bucket Path"
+                }
+              }
+            ]
+          }
+        },
+        {
+          "type": "sequential",
+          "data": {
+            "commands": [
+              {
+                "type": "path",
+                "data": {
+                  "pathName": "Top Bucket Path 2"
+                }
+              }
+            ]
+          }
+        }
+      ]
+    }
+  },
+  "resetOdom": true,
+  "folder": null,
+  "choreoAuto": false
+}

src/main/deploy/pathplanner/paths/Test Path.path → src/main/deploy/pathplanner/paths/Top Bucket Path 2.path

@@ -3,25 +3,25 @@
   "waypoints": [
     {
       "anchor": {
-        "x": 1.0,
-        "y": 5.0
+        "x": 3.2132156139365473,
+        "y": 2.4095227609272882
       },
       "prevControl": null,
       "nextControl": {
-        "x": 0.5625,
-        "y": 4.953125
+        "x": 4.213215613936557,
+        "y": 2.4095227609272882
       },
       "isLocked": false,
       "linkedName": null
     },
     {
       "anchor": {
-        "x": 3.0,
-        "y": 5.0
+        "x": 1.2668568729575171,
+        "y": 2.4095227609272882
       },
       "prevControl": {
-        "x": 2.0,
-        "y": 5.0
+        "x": 0.26685687295751714,
+        "y": 2.4095227609272882
       },
       "nextControl": null,
       "isLocked": false,
@@ -34,18 +34,18 @@
   "eventMarkers": [],
   "globalConstraints": {
     "maxVelocity": 3.0,
-    "maxAcceleration": 3.0,
+    "maxAcceleration": 1.0,
     "maxAngularVelocity": 540.0,
     "maxAngularAcceleration": 720.0,
     "nominalVoltage": 12.0,
     "unlimited": false
   },
   "goalEndState": {
     "velocity": 0,
-    "rotation": 0.0
+    "rotation": -178.83250342051568
   },
   "reversed": false,
-  "folder": null,
+  "folder": "Bucket Paths",
   "idealStartingState": {
     "velocity": 0,
     "rotation": 0.0

src/main/deploy/pathplanner/paths/Top Bucket Path.path

@@ -3,25 +3,25 @@
   "waypoints": [
     {
       "anchor": {
-        "x": 0.4429442528408664,
-        "y": 2.87806054281588
+        "x": 1.0911718962758723,
+        "y": 2.4095227609272882
       },
       "prevControl": null,
       "nextControl": {
-        "x": 1.4429442528408662,
-        "y": 2.87806054281588
+        "x": 2.0911718962758803,
+        "y": 2.4095227609272882
       },
       "isLocked": false,
       "linkedName": null
     },
     {
       "anchor": {
-        "x": 4.302773111664035,
-        "y": 2.87806054281588
+        "x": 3.2132156139365473,
+        "y": 2.4095227609272882
       },
       "prevControl": {
-        "x": 3.3027731116640346,
-        "y": 2.87806054281588
+        "x": 2.2132156139365473,
+        "y": 2.4095227609272882
       },
       "nextControl": null,
       "isLocked": false,
@@ -34,7 +34,7 @@
   "eventMarkers": [],
   "globalConstraints": {
     "maxVelocity": 3.0,
-    "maxAcceleration": 3.0,
+    "maxAcceleration": 1.0,
     "maxAngularVelocity": 540.0,
     "maxAngularAcceleration": 720.0,
     "nominalVoltage": 12.0,
@@ -45,7 +45,7 @@
     "rotation": 0.0
   },
   "reversed": false,
-  "folder": null,
+  "folder": "Bucket Paths",
   "idealStartingState": {
     "velocity": 0,
     "rotation": 0.0

src/main/deploy/pathplanner/settings.json

@@ -2,12 +2,14 @@
   "robotWidth": 0.648,
   "robotLength": 0.648,
   "holonomicMode": true,
-  "pathFolders": [],
+  "pathFolders": [
+    "Bucket Paths"
+  ],
   "autoFolders": [
     "New Folder"
   ],
   "defaultMaxVel": 3.0,
-  "defaultMaxAccel": 3.0,
+  "defaultMaxAccel": 1.0,
   "defaultMaxAngVel": 540.0,
   "defaultMaxAngAccel": 720.0,
   "robotMass": 74.088,

src/main/java/frc/robot/Constants.java

@@ -4,9 +4,17 @@
 
 package frc.robot;
 
+import static edu.wpi.first.units.Units.*;
+
 import com.pathplanner.lib.config.PIDConstants;
 import edu.wpi.first.math.geometry.Translation3d;
 import edu.wpi.first.math.util.Units;
+import edu.wpi.first.units.Current;
+import edu.wpi.first.units.Distance;
+import edu.wpi.first.units.Mass;
+import edu.wpi.first.units.Measure;
+import edu.wpi.first.units.Time;
+import edu.wpi.first.units.Velocity;
 import swervelib.math.Matter;
 
 /**
@@ -18,11 +26,12 @@
  * constants are needed, to reduce verbosity.
  */
 public final class Constants {
-  public static final double ROBOT_MASS = (148 - 20.3) * 0.453592; // 32lbs * kg per pound
+  public static final Measure<Mass> ROBOT_MASS = Pounds.of(125);
   public static final Matter CHASSIS =
-      new Matter(new Translation3d(0, 0, Units.inchesToMeters(8)), ROBOT_MASS);
-  public static final double LOOP_TIME = 0.13; // s, 20ms + 110ms sprk max velocity lag
-  public static final double MAX_SPEED = Units.feetToMeters(14.5);
+      new Matter(new Translation3d(0, 0, Units.inchesToMeters(8)), ROBOT_MASS.in(Kilogram));
+  public static final Measure<Time> LOOP_TIME =
+      Seconds.of(0.13); // s, 20ms + 110ms sprk max velocity lag
+  public static final Measure<Velocity<Distance>> MAX_SPEED = FeetPerSecond.of(14.5);
 
   public static final class AutoConstants {
     public static final PIDConstants TRANSLATION_PID = new PIDConstants(0.7, 0, 0);
@@ -31,16 +40,24 @@ public static final class AutoConstants {
 
   public static final class DrivebaseConstants {
     // Hold time on motor brakes when disabled.
-    public static final double WHEEL_LOCK_TIME = 10; // seconds
+    public static final Measure<Time> WHEEL_LOCK_TIME = Seconds.of(10);
   }
 
   public static class OperatorConstants {
-    public static final int kDriverControllerPort = 0;
+    public static final int DRIVER_CONTROLLER_PORT = 0;
 
     // Joystick deadband.
     public static final double LEFT_X_DEADBAND = 0.1;
     public static final double LEFT_Y_DEADBAND = 0.1;
     public static final double RIGHT_X_DEADBAND = 0.1;
     public static final double TURN_CONSTANT = 6;
   }
+
+  public static class IntakeConstants {
+    public static final int INTAKE_MOTOR = 19;
+    public static final Measure<Current> MOTOR_CURRENT_LIMIT = Amp.of(40);
+    public static final double OUTTAKE_SPEED = 0.4;
+    public static final double INTAKE_SPEED = -0.4;
+    public static final double INTAKE_REST_SPEED = -0.1;
+  }
 }

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

@@ -5,7 +5,6 @@
 package frc.robot;
 
 import edu.wpi.first.wpilibj.TimedRobot;
-import edu.wpi.first.wpilibj.Timer;
 import edu.wpi.first.wpilibj2.command.Command;
 import edu.wpi.first.wpilibj2.command.CommandScheduler;
 
@@ -22,8 +21,6 @@ public class Robot extends TimedRobot {
 
   private RobotContainer m_robotContainer;
 
-  private Timer disabledTimer;
-
   public Robot() {
     instance = this;
   }
@@ -41,11 +38,6 @@ public void robotInit() {
     // Instantiate our RobotContainer.  This will perform all our button bindings, and put our
     // autonomous chooser on the dashboard.
     m_robotContainer = new RobotContainer();
-
-    // Create a timer to disable motor brake a few seconds after disable.  This will let the robot
-    // stop
-    // immediately when disabled, but then also let it be pushed more
-    disabledTimer = new Timer();
   }
 
   /**
@@ -66,19 +58,10 @@ public void robotPeriodic() {
 
   /** This function is called once each time the robot enters Disabled mode. */
   @Override
-  public void disabledInit() {
-    m_robotContainer.setMotorBrake(true);
-    disabledTimer.reset();
-    disabledTimer.start();
-  }
+  public void disabledInit() {}
 
   @Override
-  public void disabledPeriodic() {
-    if (disabledTimer.hasElapsed(Constants.DrivebaseConstants.WHEEL_LOCK_TIME)) {
-      m_robotContainer.setMotorBrake(false);
-      disabledTimer.stop();
-    }
-  }
+  public void disabledPeriodic() {}
 
   /** This autonomous runs the autonomous command selected by your {@link RobotContainer} class. */
   @Override
@@ -107,7 +90,6 @@ public void teleopInit() {
       CommandScheduler.getInstance().cancelAll();
     }
     m_robotContainer.setDriveMode();
-    m_robotContainer.setMotorBrake(true);
   }
 
   /** This function is called periodically during operator control. */

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

@@ -6,9 +6,6 @@
 
 import com.pathplanner.lib.auto.AutoBuilder;
 import edu.wpi.first.math.MathUtil;
-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.wpilibj.Filesystem;
 import edu.wpi.first.wpilibj.RobotBase;
 import edu.wpi.first.wpilibj.smartdashboard.SendableChooser;
@@ -18,6 +15,7 @@
 import edu.wpi.first.wpilibj2.command.button.CommandXboxController;
 import edu.wpi.first.wpilibj2.command.button.Trigger;
 import frc.robot.Constants.OperatorConstants;
+import frc.robot.subsystems.IntakeSubsystem;
 import frc.robot.subsystems.SwerveSubsystem;
 import java.io.File;
 
@@ -31,10 +29,12 @@ public class RobotContainer {
 
   private final SendableChooser<Command> autoChooser;
   // Replace with CommandPS4Controller or CommandJoystick if needed
-  final CommandXboxController driverXbox = new CommandXboxController(0);
+  final CommandXboxController driverXbox =
+      new CommandXboxController(OperatorConstants.DRIVER_CONTROLLER_PORT);
   // The robot's subsystems and commands are defined here...
   private final SwerveSubsystem drivebase =
       new SwerveSubsystem(new File(Filesystem.getDeployDirectory(), "swerve"));
+  private final IntakeSubsystem intake = new IntakeSubsystem();
 
   // Applies deadbands and inverts controls because joysticks are back-right positive while robot
   // controls are front-left positive left stick controls translation right stick controls the
@@ -70,16 +70,16 @@ public RobotContainer() {
    */
   private void configureBindings() {
     driverXbox.a().onTrue((Commands.runOnce(drivebase::zeroGyro)));
-    driverXbox
-        .b()
-        .whileTrue(
-            Commands.deferredProxy(
-                () ->
-                    drivebase.driveToPose(
-                        new Pose2d(new Translation2d(4, 4), Rotation2d.fromDegrees(0)))));
+    driverXbox.b().onTrue(Commands.none());
     driverXbox.start().whileTrue(Commands.none());
     driverXbox.back().whileTrue(Commands.none());
     driverXbox.leftBumper().whileTrue(Commands.runOnce(drivebase::lock, drivebase).repeatedly());
+    driverXbox
+        .leftTrigger()
+        .whileTrue(intake.intakeBucket(intake, Constants.IntakeConstants.INTAKE_SPEED, true));
+    driverXbox
+        .rightTrigger()
+        .whileTrue(intake.intakeBucket(intake, Constants.IntakeConstants.OUTTAKE_SPEED, false));
     driverXbox.rightBumper().onTrue(Commands.none());
     drivebase.setDefaultCommand(
         !RobotBase.isSimulation()

src/main/java/frc/robot/commands/drivebase/AbsoluteFieldDrive.java

@@ -4,6 +4,9 @@
 
 package frc.robot.commands.drivebase;
 
+import static edu.wpi.first.units.Units.Kilograms;
+import static edu.wpi.first.units.Units.Seconds;
+
 import edu.wpi.first.math.geometry.Rotation2d;
 import edu.wpi.first.math.geometry.Translation2d;
 import edu.wpi.first.math.kinematics.ChassisSpeeds;
@@ -64,8 +67,8 @@ public void execute() {
             translation,
             swerve.getFieldVelocity(),
             swerve.getPose(),
-            Constants.LOOP_TIME,
-            Constants.ROBOT_MASS,
+            Constants.LOOP_TIME.in(Seconds),
+            Constants.ROBOT_MASS.in(Kilograms),
             List.of(Constants.CHASSIS),
             swerve.getSwerveDriveConfiguration());
     SmartDashboard.putNumber("LimitedTranslation", translation.getX());