Tank drivetrain emulation with swerve modules

grt192 · Dec 1, 2023 · 2496d15 · 2496d15
1 parent b24457c
commit 2496d15
Show file tree

Hide file tree

Showing 2 changed files with 49 additions and 2 deletions.
diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java
@@ -58,6 +58,7 @@
 import frc.robot.subsystems.tiltedelevator.ElevatorState.OffsetState;
 import frc.robot.util.ShuffleboardUtil;
 import frc.robot.subsystems.drivetrain.BaseSwerveSubsystem;
+import frc.robot.subsystems.drivetrain.TankEmulatorSwerveSubsystem;
 import frc.robot.subsystems.drivetrain.MissileShellSwerveSubsystem;
 import frc.robot.subsystems.drivetrain.MissileShellSwerveSweeperSubsystem;
 import frc.robot.subsystems.drivetrain.SwerveSubsystem;
@@ -136,7 +137,7 @@ public RobotContainer() {
         signalLEDSubsystem = new LEDSubsystem(); 
 
         // driveSubsystem = new MissileShellSwerveSubsystem();
-        driveSubsystem = new SwerveSubsystem(photonWrapper, signalLEDSubsystem);
+        driveSubsystem = new TankEmulatorSwerveSubsystem(photonWrapper, signalLEDSubsystem);
         rollerSubsystem = new RollerSubsystem();
         tiltedElevatorSubsystem = new TiltedElevatorSubsystem();
 
@@ -210,7 +211,16 @@ private void configureDriveBindings() {
         driveController.getBalancerButton().whileTrue(balancerCommand);
         driveController.getCameraSwitchButton().onTrue(new InstantCommand(switchableCamera::switchCamera));
 
-        if (driveSubsystem instanceof BaseSwerveSubsystem) {
+        if(driveSubsystem instanceof TankEmulatorSwerveSubsystem){
+            final TankEmulatorSwerveSubsystem tankSubsystem = (TankEmulatorSwerveSubsystem) driveSubsystem;
+            tankSubsystem.setDefaultCommand(new RunCommand(() -> {
+                double forwardPower = driveController.getForwardPower();
+                double rotatePower = driveController.getRotatePower();
+                tankSubsystem.setDrivePowers(forwardPower, rotatePower);
+        }, tankSubsystem));
+        }
+
+        else if (driveSubsystem instanceof BaseSwerveSubsystem) {
             final BaseSwerveSubsystem swerveSubsystem = (BaseSwerveSubsystem) driveSubsystem;
 
             swerveSubsystem.setDefaultCommand(new RunCommand(() -> {

diff --git a/src/main/java/frc/robot/subsystems/drivetrain/TankEmulatorSwerveSubsystem.java b/src/main/java/frc/robot/subsystems/drivetrain/TankEmulatorSwerveSubsystem.java
@@ -0,0 +1,37 @@
+package frc.robot.subsystems.drivetrain;
+
+import edu.wpi.first.math.geometry.Rotation2d;
+import edu.wpi.first.math.kinematics.SwerveModuleState;
+import frc.robot.subsystems.leds.LEDSubsystem;
+import frc.robot.vision.PhotonWrapper;
+
+
+
+public class TankEmulatorSwerveSubsystem extends SwerveSubsystem{
+
+    private static final double SPEED_SCALE = .8 / 2;
+
+    public TankEmulatorSwerveSubsystem(PhotonWrapper photonWrapper, LEDSubsystem ledSubsystem){
+        super(photonWrapper,  ledSubsystem);
+    }
+
+    /**
+     * Makes the swerve modules act as a tank drive
+     * @param forwardPower The forward power of the robot [-1,1]
+     * @param rotatePower The rotational power of the robot, clockwise positive [-1,1]
+     */
+    public void setDrivePowers(double forwardPower, double rotatePower){
+
+        double leftPower = SPEED_SCALE * (forwardPower + rotatePower);
+        double rightPower = SPEED_SCALE * (forwardPower - rotatePower);
+
+        SwerveModuleState[] states = {
+            new SwerveModuleState(MAX_VEL * leftPower, new Rotation2d(0)), //TL
+            new SwerveModuleState(MAX_VEL * rightPower, new Rotation2d(0)), //TR
+            new SwerveModuleState(MAX_VEL * leftPower, new Rotation2d(0)), //BL
+            new SwerveModuleState(MAX_VEL * rightPower, new Rotation2d(0)) //BR
+        };
+
+        setSwerveModuleStates(states);
+    }
+}