Minor cleanup

kglasnapp · Aug 31, 2024 · 766d759 · 766d759
1 parent 42543bc
commit 766d759
Show file tree

Hide file tree

Showing 6 changed files with 593 additions and 580 deletions.
diff --git a/src/main/java/frc/robot/Autonomous.java b/src/main/java/frc/robot/Autonomous.java
diff --git a/src/main/java/frc/robot/Robot.java b/src/main/java/frc/robot/Robot.java
@@ -99,7 +99,7 @@ public void disabledPeriodic() {
   @Override
   public void autonomousInit() {
     // cmd = RobotContainer.autonomousChooser.getSelected();
-    Integer firstStepWait = RobotContainer.autonomousChooserFirtWait.getSelected();
+    Integer firstStepWait = RobotContainer.autonomousChooserFirstWait.getSelected();
     boolean autoAim = RobotContainer.autonomousAim.getSelected();
     Integer firstStep = RobotContainer.autonomousChooserFirstStep.getSelected();
     Integer lastStep = RobotContainer.autonomousChooserLastStep.getSelected();

diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java
@@ -39,7 +39,6 @@
 import edu.wpi.first.wpilibj2.command.button.CommandXboxController;
 import frc.robot.commands.ChangeNormalModeCommand;
 import frc.robot.commands.ChangeTurboModeCommand;
-import frc.robot.commands.DefaultDriveCommand;
 import frc.robot.commands.IntakeNoteCommand;
 import frc.robot.commands.ShootCommand;
 import frc.robot.commands.TiltHomeCommand;
@@ -86,16 +85,16 @@ public class RobotContainer {
   private SlewRateLimiter sRX = new SlewRateLimiter(DrivetrainSubsystem.MAX_ANGULAR_VELOCITY_RADIANS_PER_SECOND / 1.0);
 
   public static SendableChooser<Boolean> autonomousAim = new SendableChooser<>();
-  public static SendableChooser<Integer> autonomousChooserFirtWait = new SendableChooser<>();
+  public static SendableChooser<Integer> autonomousChooserFirstWait = new SendableChooser<>();
   public static SendableChooser<Integer> autonomousChooserFirstStep = new SendableChooser<>();
   public static SendableChooser<Integer> autonomousChooserLastStep = new SendableChooser<>();
   public PoseSubsystem poseSubsystem;
   public static CoralSubsystem coralSubsystem = new CoralSubsystem();
   public TiltSubsystem tilt = new TiltSubsystem();
 
+  // TODO: Move these to a separate file
   public final static Pose2d BLUE_SPEAKER = new Pose2d(-0.0381, 5.54, new Rotation2d());
   public final static Pose2d RED_SPEAKER = new Pose2d(16.57, 5.54, new Rotation2d(Math.toRadians(180)));
-
   public final static Pose2d BLUE_AMP = new Pose2d(72.5, 323, new Rotation2d(Math.toRadians(270)));
   public final static Pose2d RED_AMP = new Pose2d(578.77, 323, new Rotation2d(Math.toRadians(270)));
 
@@ -113,13 +112,17 @@ public RobotContainer() {
 
     drivetrainSubsystem.setDefaultCommand(new DefaultDriveCommand(
         drivetrainSubsystem,
-        () -> sLY.calculate(-modifyAxis(driveController.getLeftY())
+        () -> sLY.calculate(modifyAxis(driveController.getLeftY())
             * DrivetrainSubsystem.MAX_VELOCITY_METERS_PER_SECOND),
-        () -> sLX.calculate(-modifyAxis(driveController.getLeftX())
+        () -> sLX.calculate(modifyAxis(driveController.getLeftX())
             * DrivetrainSubsystem.MAX_VELOCITY_METERS_PER_SECOND),
         () -> sRX.calculate(-modifyAxis(driveController.getRightX())
             * DrivetrainSubsystem.MAX_ANGULAR_VELOCITY_RADIANS_PER_SECOND),
-        driveController.y()));// Set precision based upon left bumper
+        // Set precision based upon left bumper
+        // Not used, currently implemented with separate commands
+        driveController.y(),
+        // Set robot oriented control based upon left bumper
+        driveController.leftBumper()));
 
     poseSubsystem = new PoseSubsystem(drivetrainSubsystem, "limelight");
     configureButtonBindings();

diff --git a/src/main/java/frc/robot/commands/DefaultDriveCommand.java b/src/main/java/frc/robot/commands/DefaultDriveCommand.java
@@ -10,6 +10,7 @@
 import frc.robot.Robot;
 import frc.robot.RobotContainer;
 import frc.robot.subsystems.DrivetrainSubsystem;
+import frc.robot.utilities.SwerveModule;
 
 public class DefaultDriveCommand extends Command {
 
@@ -27,25 +28,32 @@ public class DefaultDriveCommand extends Command {
     private final DoubleSupplier m_translationYSupplier;
     private final DoubleSupplier m_rotationSupplier;
     private final BooleanSupplier precisionActivator;
+    private final BooleanSupplier robotOrientedActivator;
 
     public DefaultDriveCommand(DrivetrainSubsystem drivetrainSubsystem,
             DoubleSupplier translationXSupplier,
             DoubleSupplier translationYSupplier,
             DoubleSupplier rotationSupplier,
-            BooleanSupplier precisionActivator) {
+            BooleanSupplier precisionActivator, BooleanSupplier robotOrientedActivator) {
         this.m_drivetrainSubsystem = drivetrainSubsystem;
         this.m_translationXSupplier = translationXSupplier;
         this.m_translationYSupplier = translationYSupplier;
         this.m_rotationSupplier = rotationSupplier;
         this.precisionActivator = precisionActivator;
+        this.robotOrientedActivator = robotOrientedActivator;
+
         logf("Default Drive Command started precision:%b\n", this.precisionActivator.getAsBoolean());
         addRequirements(drivetrainSubsystem);
     }
 
     @Override
     public void execute() {
-        // You can use `new ChassisSpeeds(...)` for robot-oriented movement instead of
-        // field-oriented movement
+        // Currently implemented with separate commands
+        // if (precisionActivator.getAsBoolean()) {
+        // SwerveModule.setPowerRatio(SwerveModule.PRECISION);
+        // } else {
+        // SwerveModule.setPowerRatio(SwerveModule.TURBO);
+        // }
 
         if (Robot.count % 20 == 0) {
             if (m_translationXSupplier.getAsDouble() != 0 &&
@@ -54,28 +62,28 @@ public void execute() {
                         m_translationYSupplier.getAsDouble(), m_rotationSupplier.getAsDouble());
             }
         }
+
         // this allows for Robot Oriented driving
-        if (RobotContainer.getLeftBumper()) {
+        if (robotOrientedActivator.getAsBoolean()) {
             m_drivetrainSubsystem.drive(
                     new ChassisSpeeds(
-                            -m_translationXSupplier.getAsDouble(),
-                            -m_translationYSupplier.getAsDouble(),
+                            m_translationXSupplier.getAsDouble(),
+                            m_translationYSupplier.getAsDouble(),
                             m_rotationSupplier.getAsDouble()));
 
         } else {
             m_drivetrainSubsystem.drive(
                     ChassisSpeeds.fromFieldRelativeSpeeds(
-                            -m_translationXSupplier.getAsDouble(),
-                            -m_translationYSupplier.getAsDouble(),
+                            m_translationXSupplier.getAsDouble(),
+                            m_translationYSupplier.getAsDouble(),
                             m_rotationSupplier.getAsDouble(),
                             m_drivetrainSubsystem.getGyroscopeRotation()));
         }
-
-        // FIXME: Precision mode isnt implemented here???
     }
 
     @Override
     public void end(boolean interrupted) {
         m_drivetrainSubsystem.drive(new ChassisSpeeds(0.0, 0.0, 0.0));
+        // SwerveModule.setPowerRatio(SwerveModule.DEFAULT);
     }
 }
diff --git a/src/main/java/frc/robot/commands/DriveCommand.java b/src/main/java/frc/robot/commands/DriveCommand.java
@@ -22,7 +22,6 @@ public DriveCommand(DrivetrainSubsystem drivetrainSubsystem, double xSpeed, doub
 
     @Override
     public void initialize() {
-
     }
 
     @Override
@@ -42,7 +41,7 @@ public boolean isFinished() {
 
     @Override
     public void end(boolean interrupted) {
-        
+
     }
-    
+
 }
diff --git a/src/main/java/frc/robot/utilities/SwerveModule.java b/src/main/java/frc/robot/utilities/SwerveModule.java
@@ -20,8 +20,9 @@ public class SwerveModule {
   public static final double TURBO = 1;
   public static final double NORMAL = 5;
   public static final double PRECISION = 10;
+  public static final double DEFAULT = TURBO;
 
-  private static double powerRatio = SwerveModule.TURBO;
+  private static double powerRatio = DEFAULT;
 
   public static final double openLoopRamp = 0.25;
   public static final double closedLoopRamp = 0.0;
@@ -108,10 +109,12 @@ public SwerveModule(int moduleNumber, SwerveModuleType swerve_type, SwerveModule
     // lastAngle = getState().angle;
   }
 
+  // FIXME: This should prob be in drivetrain subsystem
   public static double getPowerRatio() {
     return powerRatio;
   }
 
+  // FIXME: This should prob be in drivetrain subsystem
   public static void setPowerRatio(double powerRatio) {
     SwerveModule.powerRatio = powerRatio;
   }