Merge branch 'main' into intake

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

@@ -11,6 +11,8 @@
 
 package frc.robot;
 
+import edu.wpi.first.math.geometry.Translation2d;
+
 /**
  * The Constants class provides a convenient place for teams to hold robot-wide numerical or boolean
  * constants. This class should not be used for any other purpose. All constants should be declared
@@ -24,11 +26,22 @@ public static class Controllers {
     public static final int DRIVER_CONTROLLER_PORT = 0;
   }
 
+  public static class Drive {
+    /** Empty translation to prevent creating 2 Translation2ds every time the drive train stops. */
+    public static Translation2d EMPTY_TRANSLATION = new Translation2d();
+
+    /** The max speed the robot can go */
+    public static double MAX_SPEED = 1;
+
+    /** The max speed the robot can rotate */
+    public static double MAX_ANGULAR_SPEED = Math.PI / 2;
+
   public static class Intake {
     /** Motor id of the Intake motor. */
     public static final int INTAKE_MOTOR_ID = 0;
 
     /** Speed we want to run the Intake at. */
     public static final double INTAKE_MOTOR_SPEED = 0.5;
+
   }
 }

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

@@ -16,13 +16,13 @@
 import edu.wpi.first.wpilibj2.command.CommandScheduler;
 
 public class Robot extends TimedRobot {
-  private Command m_autonomousCommand;
+  private Command autonomousCommand;
 
-  private RobotContainer m_robotContainer;
+  private RobotContainer robotContainer;
 
   @Override
   public void robotInit() {
-    m_robotContainer = new RobotContainer();
+    robotContainer = new RobotContainer();
   }
 
   @Override
@@ -41,10 +41,10 @@ public void disabledExit() {}
 
   @Override
   public void autonomousInit() {
-    m_autonomousCommand = m_robotContainer.getAutonomousCommand();
+    autonomousCommand = robotContainer.getAutonomousCommand();
 
-    if (m_autonomousCommand != null) {
-      m_autonomousCommand.schedule();
+    if (autonomousCommand != null) {
+      autonomousCommand.schedule();
     }
   }
 
@@ -56,8 +56,8 @@ public void autonomousExit() {}
 
   @Override
   public void teleopInit() {
-    if (m_autonomousCommand != null) {
-      m_autonomousCommand.cancel();
+    if (autonomousCommand != null) {
+      autonomousCommand.cancel();
     }
   }
 

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

@@ -16,13 +16,14 @@
 import edu.wpi.first.wpilibj.Filesystem;
 import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
 import edu.wpi.first.wpilibj2.command.SubsystemBase;
+import frc.robot.Constants;
 import java.io.File;
 import java.io.IOException;
 import swervelib.SwerveDrive;
 import swervelib.parser.SwerveParser;
 
 public class Drive extends SubsystemBase {
-  SwerveDrive drive;
+  private SwerveDrive drive;
 
   /** Creates a new Drive. */
   public Drive() {
@@ -59,19 +60,20 @@ public void driveFieldOriented(double x, double y, double z) {
   }
 
   public void stop() {
-    drive.drive(new Translation2d(), 0, false, false, new Translation2d());
+    drive.drive(
+        Constants.Drive.EMPTY_TRANSLATION, 0, false, false, Constants.Drive.EMPTY_TRANSLATION);
   }
 
   public void enterXMode() {
     drive.lockPose();
   }
 
   private double getMaximumSpeed() {
-    return 1; // TODO: move this to constants
+    return Constants.Drive.MAX_SPEED;
   }
 
   private double getMaximumAngularSpeed() {
-    return Math.PI / 2; // TODO: move this to constants
+    return Constants.Drive.MAX_ANGULAR_SPEED;
   }
 
   @Override