diff --git a/src/main/java/frc4388/robot/RobotContainer.java b/src/main/java/frc4388/robot/RobotContainer.java index a490cf2..b2fc839 100644 --- a/src/main/java/frc4388/robot/RobotContainer.java +++ b/src/main/java/frc4388/robot/RobotContainer.java @@ -15,6 +15,7 @@ import edu.wpi.first.wpilibj2.command.button.JoystickButton; import frc4388.robot.Constants.*; import frc4388.robot.subsystems.ArcadeDrive; import frc4388.robot.subsystems.LED; +import frc4388.robot.subsystems.SwerveDrive; import frc4388.utility.LEDPatterns; import frc4388.utility.controller.IHandController; import frc4388.utility.controller.XboxController; @@ -34,6 +35,17 @@ public class RobotContainer { private final ArcadeDrive m_robotArcadeDrive = new ArcadeDrive(m_robotMap.leftFrontMotor, m_robotMap.rightFrontMotor, m_robotMap.leftBackMotor, m_robotMap.rightBackMotor, m_robotMap.driveTrain, m_robotMap.gyroDrive); + private final SwerveDrive m_robotSwerveDrive = new SwerveDrive( + m_robotMap.leftFrontSteerMotor, m_robotMap.leftFrontWheelMotor, + m_robotMap.rightFrontSteerMotor, m_robotMap.rightFrontWheelMotor, + m_robotMap.leftBackSteerMotor, m_robotMap.leftBackWheelMotor, + m_robotMap.rightBackSteerMotor, m_robotMap.rightBackWheelMotor, + m_robotMap.leftFrontEncoder, + m_robotMap.rightFrontEncoder, + m_robotMap.leftBackEncoder, + m_robotMap.rightBackEncoder + ); + private final LED m_robotLED = new LED(m_robotMap.LEDController); /* Controllers */ @@ -47,10 +59,16 @@ public class RobotContainer { configureButtonBindings(); /* Default Commands */ - // drives the robot with a two-axis input from the driver controller + // drives the arcade drive with a two-axis input from the driver controller m_robotArcadeDrive.setDefaultCommand( new RunCommand(() -> m_robotArcadeDrive.driveWithInput(getDriverController().getLeftYAxis(), getDriverController().getRightXAxis()), m_robotArcadeDrive)); + + // drives the arcade drive with a two-axis input from the driver controller + m_robotSwerveDrive.setDefaultCommand( + new RunCommand(() -> m_robotSwerveDrive.driveWithInput(getDriverController().getRightXAxis(), + getDriverController().getRightYAxis(), getDriverController().getLeftXAxis()), m_robotSwerveDrive)); + // continually sends updates to the Blinkin LED controller to keep the lights on m_robotLED.setDefaultCommand(new RunCommand(() -> m_robotLED.updateLED(), m_robotLED)); } diff --git a/src/main/java/frc4388/robot/subsystems/SwerveDrive.java b/src/main/java/frc4388/robot/subsystems/SwerveDrive.java index 0544526..0367c6e 100644 --- a/src/main/java/frc4388/robot/subsystems/SwerveDrive.java +++ b/src/main/java/frc4388/robot/subsystems/SwerveDrive.java @@ -15,9 +15,10 @@ import edu.wpi.first.wpilibj.geometry.Translation2d; import edu.wpi.first.wpilibj.kinematics.ChassisSpeeds; import edu.wpi.first.wpilibj.kinematics.SwerveDriveKinematics; import edu.wpi.first.wpilibj.kinematics.SwerveModuleState; +import edu.wpi.first.wpilibj2.command.SubsystemBase; import frc4388.robot.Constants.SwerveDriveConstants; -public class SwerveDrive +public class SwerveDrive extends SubsystemBase { SwerveDriveKinematics m_kinematics; private WPI_TalonFX m_leftFrontSteerMotor; @@ -61,28 +62,32 @@ public class SwerveDrive m_kinematics = new SwerveDriveKinematics(m_frontLeftLocation, m_frontRightLocation, m_backLeftLocation, m_backRightLocation); } - public void drive(double strafeX, double strafeY, double rotate) + + public void driveWithInput(double strafeX, double strafeY, double rotate) + { + var speeds = new ChassisSpeeds(strafeX, strafeY, rotate * SwerveDriveConstants.ROTATION_SPEED /*in rad/s */); + driveFromSpeeds(speeds); + } + + public void driveFromSpeeds(ChassisSpeeds speeds) { //https://docs.wpilib.org/en/stable/docs/software/kinematics-and-odometry/swerve-drive-kinematics.html - var speeds = new ChassisSpeeds(strafeX, strafeY, rotate * SwerveDriveConstants.ROTATION_SPEED /*in rad/s */); // Convert to module states SwerveModuleState[] moduleStates = m_kinematics.toSwerveModuleStates(speeds); // Front left module state SwerveModuleState leftFront = SwerveModuleState.optimize(moduleStates[0], new Rotation2d(m_leftFrontEncoder.getPosition())); - // Front right module state SwerveModuleState rightFront = SwerveModuleState.optimize(moduleStates[1], new Rotation2d(m_rightFrontEncoder.getPosition())); - // Back left module state SwerveModuleState leftBack = SwerveModuleState.optimize(moduleStates[2], new Rotation2d(m_leftBackEncoder.getPosition())); - // Back right module state SwerveModuleState rightBack = SwerveModuleState.optimize(moduleStates[3], new Rotation2d(m_rightBackEncoder.getPosition())); + //Set the Wheel motor speeds m_leftFrontWheelMotor.set(m_leftFrontSteerMotor.get() + leftFront.speedMetersPerSecond * SwerveDriveConstants.WHEEL_SPEED); m_rightFrontWheelMotor.set(m_rightFrontSteerMotor.get() + rightFront.speedMetersPerSecond * SwerveDriveConstants.WHEEL_SPEED); - m_leftBackWheelMotor.set(m_leftBackSteerMotor.get() + lefBack.speedMetersPerSecond * SwerveDriveConstants.WHEEL_SPEED); + m_leftBackWheelMotor.set(m_leftBackSteerMotor.get() + leftBack.speedMetersPerSecond * SwerveDriveConstants.WHEEL_SPEED); m_rightBackWheelMotor.set(m_rightBackSteerMotor.get() + rightBack.speedMetersPerSecond * SwerveDriveConstants.WHEEL_SPEED); }