diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java index f82d44f6..38e23153 100644 --- a/src/main/java/frc/robot/Constants.java +++ b/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; + } } diff --git a/src/main/java/frc/robot/Robot.java b/src/main/java/frc/robot/Robot.java index 302be34a..4e67627a 100644 --- a/src/main/java/frc/robot/Robot.java +++ b/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(); } } diff --git a/src/main/java/frc/robot/subsystems/Drive.java b/src/main/java/frc/robot/subsystems/Drive.java index f1914cb5..4eb9ad0a 100644 --- a/src/main/java/frc/robot/subsystems/Drive.java +++ b/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,7 +60,8 @@ 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() { @@ -67,11 +69,11 @@ public void enterXMode() { } 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