From 2496d1515b5c6bea408bce015fbbf3633417772d Mon Sep 17 00:00:00 2001 From: penguin212 Date: Fri, 1 Dec 2023 01:07:06 -0800 Subject: [PATCH] Tank drivetrain emulation with swerve modules --- src/main/java/frc/robot/RobotContainer.java | 14 ++++++- .../TankEmulatorSwerveSubsystem.java | 37 +++++++++++++++++++ 2 files changed, 49 insertions(+), 2 deletions(-) create mode 100644 src/main/java/frc/robot/subsystems/drivetrain/TankEmulatorSwerveSubsystem.java diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 6246fff2..a7ecceae 100644 --- 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 new file mode 100644 index 00000000..7941c483 --- /dev/null +++ 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); + } +}