diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 2940c67..80e6090 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -23,9 +23,13 @@ public class RobotContainer { private double MaxSpeed = TunerConstants.kSpeedAt12Volts.in(MetersPerSecond); // kSpeedAt12Volts desired top speed + private double SlowSpeed = + TunerConstants.kSlowSpeed.in(MetersPerSecond); private double MaxAngularRate = RotationsPerSecond.of(0.75) .in(RadiansPerSecond); // 3/4 of a rotation per second max angular velocity + private double SlowAngularRate = + RotationsPerSecond.of(0.5).in(RadiansPerSecond); /* Setting up bindings for necessary control of the swerve drive platform */ private final SwerveRequest.FieldCentric drive = @@ -73,7 +77,17 @@ private void configureBindings() { * MaxAngularRate) // Drive counterclockwise with negative X (left) )); - //joystick.a().whileTrue(drivetrain.applyRequest(() -> brake)); + joystick.a().whileTrue(drivetrain.applyRequest(() -> brake)); + joystick.rightStick().whileTrue( + drivetrain.applyRequest(() -> + drive + .withVelocityX( + -joystick.getLeftX() * SlowSpeed) + .withVelocityY( + -joystick.getLeftY() * SlowSpeed) + .withRotationalRate( + -joystick.getRightX() * SlowAngularRate) + )); // joystick // .b() // .whileTrue( diff --git a/src/main/java/frc/robot/generated/TunerConstants.java b/src/main/java/frc/robot/generated/TunerConstants.java index 30833de..9ce1a7b 100644 --- a/src/main/java/frc/robot/generated/TunerConstants.java +++ b/src/main/java/frc/robot/generated/TunerConstants.java @@ -96,6 +96,7 @@ public class TunerConstants { // Theoretical free speed (m/s) at 12 V applied output; // This needs to be tuned to your individual robot public static final LinearVelocity kSpeedAt12Volts = MetersPerSecond.of(5.21); //∞ + public static final LinearVelocity kSlowSpeed = MetersPerSecond.of(3); // Every 1 rotation of the azimuth results in kCoupleRatio drive motor turns; // This may need to be tuned to your individual robot @@ -134,6 +135,7 @@ public class TunerConstants { .withDriveMotorClosedLoopOutput(kDriveClosedLoopOutput) .withSlipCurrent(kSlipCurrent) .withSpeedAt12Volts(kSpeedAt12Volts) + .withSpeedAt12Volts(kSlowSpeed) .withDriveMotorType(kDriveMotorType) .withSteerMotorType(kSteerMotorType) .withFeedbackSource(kSteerFeedbackType)