diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index aee3f21..fb57041 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -6,6 +6,9 @@ import static edu.wpi.first.units.Units.MetersPerSecond; +import edu.wpi.first.util.sendable.Sendable; +import edu.wpi.first.util.sendable.SendableBuilder; +import edu.wpi.first.wpilibj.drive.RobotDriveBase; import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.Commands; import edu.wpi.first.wpilibj2.command.Subsystem; @@ -29,12 +32,10 @@ public RobotContainer() { setUpDriverButtonBindings(); setUpOperatorButtonBindings(); } - - private void setUpDefaultCommands() { drivetrain.setDefaultCommand( - drivetrain.tankDrive(() -> commandxboxcontroller.getRightY(), () -> commandxboxcontroller.getLeftY()) + drivetrain.arcadeDrive(() -> commandxboxcontroller.getLeftY(), () -> commandxboxcontroller.getRightX()) ); diff --git a/src/main/java/frc/robot/subsystems/Drivetrain.java b/src/main/java/frc/robot/subsystems/Drivetrain.java index e3754fa..0bb5923 100644 --- a/src/main/java/frc/robot/subsystems/Drivetrain.java +++ b/src/main/java/frc/robot/subsystems/Drivetrain.java @@ -16,6 +16,7 @@ import edu.wpi.first.units.measure.Distance; import edu.wpi.first.units.measure.LinearVelocity; +import edu.wpi.first.wpilibj.drive.DifferentialDrive; import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.SubsystemBase; import frc.robot.Constants.MotorIdConstants; @@ -37,6 +38,8 @@ public class Drivetrain extends SubsystemBase { private final double MAX_SPEED_METERS_PER_SECOND = 4.0; + private final DifferentialDrive drive; + public Drivetrain() { SparkMaxConfig rightFrontConfig = new SparkMaxConfig(); SparkMaxConfig leftFrontConfig = new SparkMaxConfig(); @@ -102,6 +105,7 @@ public Drivetrain() { motorRightClosedLoopController = rightFrontMotorController.getClosedLoopController(); motorLeftClosedLoopController = leftFrontMotorController.getClosedLoopController(); + drive = new DifferentialDrive(leftFrontMotorController, rightFrontMotorController); } public void setRightSpeed(LinearVelocity speed) { @@ -113,8 +117,14 @@ public void setLeftSpeed(LinearVelocity speed) { } public Command tankDrive(DoubleSupplier rightSpeed, DoubleSupplier leftSpeed) { return this.run(()->{ - setLeftSpeed((MetersPerSecond.of(leftSpeed.getAsDouble() * MAX_SPEED_METERS_PER_SECOND))); - setRightSpeed((MetersPerSecond.of(rightSpeed.getAsDouble() * MAX_SPEED_METERS_PER_SECOND))); + setLeftSpeed((MetersPerSecond.of(leftSpeed.getAsDouble() * MAX_SPEED_METERS_PER_SECOND))); + setRightSpeed((MetersPerSecond.of(rightSpeed.getAsDouble() * MAX_SPEED_METERS_PER_SECOND))); + } ); + } + + public Command arcadeDrive(DoubleSupplier xSpeed, DoubleSupplier zRotation) { + return this.run(()->{ + drive.arcadeDrive(xSpeed.getAsDouble(), zRotation.getAsDouble()); } ); } }