Merge branch 'simpleRobotMultiConfig' of https://github.com/DevilBotz…

…2876/Crescendo2024 into TankSwerveFusion
DevilBotz2876 · Jan 27, 2024 · 66bf170 · 66bf170
2 parents 05c7d3a + 57d6b1f
commit 66bf170
Show file tree

Hide file tree

Showing 4 changed files with 66 additions and 28 deletions.
diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java
@@ -14,6 +14,7 @@
 import frc.robot.commands.ShooterEnable;
 import frc.robot.commands.drive.DriveCommand;
 import frc.robot.commands.drive.DriveTank;
+import frc.robot.subsystems.drive.DriveBase;
 import frc.robot.subsystems.drive.DriveSwerveYAGSL;
 import frc.robot.subsystems.drive.DriveTrain;
 import frc.robot.subsystems.intake.IntakeBase;
@@ -27,16 +28,39 @@ public class RobotContainer {
   public final CommandXboxController controller;
   public final ShooterSubsystem shooter;
   public final IntakeBase intake;
-  public final DriveSwerveYAGSL drive;
-  private final Joystick m_controller = new Joystick(0);
+  public final DriveBase drive;
 
-  public String checkDriveMode = "tank";
+  public enum RobotModel {
+    PHOENIX, // Practice Swerve Bot
+    SHERMAN, // Practice Tank Bot
+  }
+
+  public enum DriveType {
+    NONE,
+    SWERVE,
+    TANK
+  }
 
   public RobotContainer() {
+    RobotModel model = RobotModel.PHOENIX;
+
     controller = new CommandXboxController(0);
 
     boolean hasIntake = false;
     boolean hasShooter = false;
+    DriveType driveType = DriveType.NONE;
+
+    switch (model) {
+      case PHOENIX:
+        driveType = DriveType.SWERVE;
+        break;
+      case SHERMAN:
+        driveType = DriveType.TANK;
+        hasIntake = true;
+        hasShooter = true;
+        break;
+      default:
+    }
 
     if (hasShooter) {
       shooter = new ShooterSubsystem(new ShooterIOSparkMax());
@@ -50,17 +74,34 @@ public RobotContainer() {
       intake = new IntakeBase(new IntakeIOSim());
     }
 
-    drive = new DriveSwerveYAGSL();
+    switch (driveType) {
+      case SWERVE:
+        drive = new DriveSwerveYAGSL();
+        break;
+      case TANK:
+        drive = new DriveTrain();
+        // Once DriveTrain implements "runVelocity", we can delete starting from this...
+        DriveTrain m_drivetrain = (DriveTrain) drive;
+        m_drivetrain.setDefaultCommand(
+          new DriveTank(
+            m_drivetrain,
+            () -> MathUtil.applyDeadband(-controller.getLeftY(), 0.05),
+            () -> MathUtil.applyDeadband(-controller.getLeftX(), 0.05)
+          )
+        );
+        // ...upto these lines
+        break;
+      default:
+        drive = null;
+    }
 
     configureBindings();
   }
 
   private void configureBindings() {
-    switch (checkDriveMode) {
-      case "swerve" :
-        shooter.setDefaultCommand(new InstantCommand(() -> shooter.disable(), shooter));
-
+      shooter.setDefaultCommand(new InstantCommand(() -> shooter.disable(), shooter));
       controller.rightTrigger().whileTrue(new ShooterEnable(shooter));
+
       intake.setDefaultCommand(
         new IntakeBaseCommand(
           intake,
@@ -69,31 +110,18 @@ private void configureBindings() {
 
       drive.setDefaultCommand(
         new DriveCommand(
-          drive,
-          () -> MathUtil.applyDeadband(-controller.getLeftY(), 0.01),
-          () -> MathUtil.applyDeadband(-controller.getLeftX(), 0.01),
-          () -> MathUtil.applyDeadband(-controller.getRightX(), 0.01)));
-      // TODO: Move deadband to constants file
+            drive,
+            () -> MathUtil.applyDeadband(-controller.getLeftY(), 0.05),
+            () -> MathUtil.applyDeadband(-controller.getLeftX(), 0.05),
+            () -> MathUtil.applyDeadband(-controller.getRightX(), 0.05)));
+    // TODO: Move deadband to constants file
 
       controller
         .start()
         .onTrue(
             new InstantCommand(() -> drive.setFieldOrientedDrive(!drive.isFieldOrientedDrive())));
 
       controller.back().onTrue(new InstantCommand(() -> drive.resetOdometry()));
-      break;
-
-      case "tank" :
-        DriveTrain m_drivetrain = new DriveTrain();
-        m_drivetrain.setDefaultCommand(
-          new DriveTank(
-            m_drivetrain,
-            m_controller::getY,
-            m_controller::getX
-          )
-        );
-        break;
-    }
   }
 
   public Command getAutonomousCommand() {

diff --git a/src/main/java/frc/robot/subsystems/drive/Drive.java b/src/main/java/frc/robot/subsystems/drive/Drive.java
@@ -23,4 +23,14 @@ public interface Drive {
    * @return speed in radian/sec
    */
   public double getMaxAngularSpeed();
+
+  public default void setFieldOrientedDrive(boolean enable) {}
+  ;
+
+  public default boolean isFieldOrientedDrive() {
+    return false;
+  }
+
+  public default void resetOdometry() {}
+  ;
 }
diff --git a/src/main/java/frc/robot/subsystems/drive/DriveSwerveYAGSL.java b/src/main/java/frc/robot/subsystems/drive/DriveSwerveYAGSL.java
@@ -11,7 +11,7 @@
 
 public class DriveSwerveYAGSL extends DriveBase {
   private final double maximumSpeed =
-      Units.feetToMeters(2); // * TODO: Calculate actual max speed */
+      Units.feetToMeters(4.5); // * TODO: Calculate actual max speed */
   private final File swerveJsonDirectory =
       new File(Filesystem.getDeployDirectory(), "swervePracticeBot");
   private SwerveDrive swerveDrive;

diff --git a/src/main/java/frc/robot/subsystems/drive/DriveTrain.java b/src/main/java/frc/robot/subsystems/drive/DriveTrain.java
@@ -6,7 +6,7 @@
 import edu.wpi.first.wpilibj.drive.DifferentialDrive;
 import edu.wpi.first.wpilibj2.command.SubsystemBase;
 
-public class DriveTrain extends SubsystemBase {
+public class DriveTrain extends DriveBase {
     // Define talons
     private static final WPI_TalonSRX leftMaster = new WPI_TalonSRX(10);
     private static final WPI_TalonSRX rightMaster = new WPI_TalonSRX(11);