From 15280d31eca6b8f1a2b7773d4e0e3ac83a818d6d Mon Sep 17 00:00:00 2001 From: Ozzie Hassel Date: Sat, 15 Feb 2025 16:08:49 -0500 Subject: [PATCH] Changed climb bindings, intake speed, and drive speed multipliers --- src/main/java/frc/robot/RobotContainer.java | 38 +++++++++---------- .../frc/robot/subsystems/climb/Climb.java | 13 ++++--- .../subsystems/climb/ClimbConstants.java | 6 +-- .../frc/robot/subsystems/drive/Drive.java | 4 ++ .../frc/robot/subsystems/drive/GyroIO.java | 2 + .../robot/subsystems/drive/GyroIORedux.java | 9 ++++- 6 files changed, 40 insertions(+), 32 deletions(-) diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 8a9db1d..fe37a02 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -143,28 +143,20 @@ private void configureButtonBindings() { drive.setDefaultCommand( DriveCommands.joystickDrive( drive, - () -> -driverController.getLeftY(), - () -> -driverController.getLeftX(), - () -> -driverController.getRightX())); + () -> driverController.getLeftY() * 0.8, + () -> driverController.getLeftX() * 0.8, + () -> driverController.getRightX() * -0.75)); + + driverController.y().onTrue(Commands.runOnce(drive::zeroHeading, drive)); operatorController - .povUp() + .rightBumper() .whileTrue( Commands.startEnd(() -> climb.setVoltage(ClimbConstants.ForwardV), climb::stop, climb)); operatorController - .povDown() + .leftBumper() .whileTrue( Commands.startEnd(() -> climb.setVoltage(ClimbConstants.ReverseV), climb::stop, climb)); - operatorController - .x() - .onTrue( - Commands.sequence( - Commands.startEnd( - () -> climb.setVoltage(ClimbConstants.ReverseV), climb::stop, climb) - .withTimeout(4.0), - Commands.startEnd( - () -> climb.setVoltage(ClimbConstants.ForwardV), climb::stop, climb) - .withTimeout(4.0))); // Lock to 0° when A button is held driverController @@ -193,24 +185,28 @@ private void configureButtonBindings() { operatorController .b() .whileTrue( - Commands.startEnd(() -> intake.setIntakeVoltage(6), () -> intake.setIntakeVoltage(0))); + Commands.startEnd(() -> intake.setIntakeVoltage(3), () -> intake.setIntakeVoltage(0))); operatorController .a() .whileTrue( - Commands.startEnd(() -> intake.setIntakeVoltage(-6), () -> intake.setIntakeVoltage(0))); + Commands.startEnd(() -> intake.setIntakeVoltage(-3), () -> intake.setIntakeVoltage(0))); operatorController .rightTrigger() .whileTrue( - Commands.run( - () -> pivot.setVoltage(operatorController.getRightTriggerAxis() * 4), pivot)); + Commands.startEnd( + () -> pivot.setVoltage(operatorController.getRightTriggerAxis() * 2), + () -> pivot.setVoltage(0), + pivot)); operatorController .leftTrigger() .whileTrue( - Commands.run( - () -> pivot.setVoltage(operatorController.getLeftTriggerAxis() * 4), pivot)); + Commands.startEnd( + () -> pivot.setVoltage(operatorController.getLeftTriggerAxis() * -2), + () -> pivot.setVoltage(0), + pivot)); } /** diff --git a/src/main/java/frc/robot/subsystems/climb/Climb.java b/src/main/java/frc/robot/subsystems/climb/Climb.java index 4599f8f..bbb6770 100644 --- a/src/main/java/frc/robot/subsystems/climb/Climb.java +++ b/src/main/java/frc/robot/subsystems/climb/Climb.java @@ -1,6 +1,5 @@ package frc.robot.subsystems.climb; -import edu.wpi.first.math.MathUtil; import edu.wpi.first.wpilibj2.command.SubsystemBase; import org.littletonrobotics.junction.Logger; @@ -8,7 +7,7 @@ public class Climb extends SubsystemBase { private ClimbIO io; private final ClimbIOInputsAutoLogged inputs = new ClimbIOInputsAutoLogged(); - private double velocity = 0.0; + private double appliedVolts = 0.0; private static Climb instance; private Climb(ClimbIO io) { @@ -18,10 +17,12 @@ private Climb(ClimbIO io) { public void setVoltage(double volts) { io.setClimbVoltage(volts); + appliedVolts = volts; } public void stop() { io.setClimbVoltage(0); + appliedVolts = 0; } public static Climb getInstance() { @@ -35,9 +36,9 @@ public static Climb init(ClimbIO io) { return instance; } - public void setVelocity(double v) { - this.velocity = v; - } + // public void setVelocity(double v) { + // this.velocity = v; + // } public double getVelocity() { return inputs.velocityRPM; @@ -55,6 +56,6 @@ public double getVoltage() { public void periodic() { io.updateInputs(inputs); Logger.processInputs("climb", inputs); - io.setClimbVoltage(MathUtil.clamp(velocity * 12, -12, 12)); + io.setClimbVoltage(appliedVolts); } } diff --git a/src/main/java/frc/robot/subsystems/climb/ClimbConstants.java b/src/main/java/frc/robot/subsystems/climb/ClimbConstants.java index 3bce1a3..a18aa18 100644 --- a/src/main/java/frc/robot/subsystems/climb/ClimbConstants.java +++ b/src/main/java/frc/robot/subsystems/climb/ClimbConstants.java @@ -5,10 +5,10 @@ public final class ClimbConstants { public static final double kP = 1; public static final double kI = 1; public static final double kD = 1; - public static final int currentLimit = 40; + public static final int currentLimit = 50; public static final double gearing = 10; public static final double moi = 10; - public static final double ForwardV = 6.0; - public static final double ReverseV = -6.0; + public static final double ForwardV = 12.0; + public static final double ReverseV = -12.0; } diff --git a/src/main/java/frc/robot/subsystems/drive/Drive.java b/src/main/java/frc/robot/subsystems/drive/Drive.java index 60b0076..25c6016 100644 --- a/src/main/java/frc/robot/subsystems/drive/Drive.java +++ b/src/main/java/frc/robot/subsystems/drive/Drive.java @@ -218,6 +218,10 @@ public void stop() { runVelocity(new ChassisSpeeds()); } + public void zeroHeading() { + gyroIO.zeroHeading(); + } + /** * Stops the drive and turns the modules to an X arrangement to resist movement. The modules will * return to their normal orientations the next time a nonzero velocity is requested. diff --git a/src/main/java/frc/robot/subsystems/drive/GyroIO.java b/src/main/java/frc/robot/subsystems/drive/GyroIO.java index 06d39db..ce3d584 100644 --- a/src/main/java/frc/robot/subsystems/drive/GyroIO.java +++ b/src/main/java/frc/robot/subsystems/drive/GyroIO.java @@ -26,5 +26,7 @@ public static class GyroIOInputs { public Rotation2d[] odometryYawPositions = new Rotation2d[] {}; } + public default void zeroHeading() {} + public default void updateInputs(GyroIOInputs inputs) {} } diff --git a/src/main/java/frc/robot/subsystems/drive/GyroIORedux.java b/src/main/java/frc/robot/subsystems/drive/GyroIORedux.java index c0cb898..90bd01f 100644 --- a/src/main/java/frc/robot/subsystems/drive/GyroIORedux.java +++ b/src/main/java/frc/robot/subsystems/drive/GyroIORedux.java @@ -30,17 +30,22 @@ public GyroIORedux() { yawPositionQueue = SparkOdometryThread.getInstance().registerSignal(navX::getYaw); } + @Override + public void zeroHeading() { + navX.setYaw(0); + } + @Override public void updateInputs(GyroIOInputs inputs) { inputs.connected = navX.isConnected(); - inputs.yawPosition = Rotation2d.fromDegrees(-navX.getYaw()); + inputs.yawPosition = Rotation2d.fromRadians(-navX.getYaw() * 2 * Math.PI); inputs.yawVelocityRadPerSec = Units.degreesToRadians(-navX.getAngularVelocityYaw()); inputs.odometryYawTimestamps = yawTimestampQueue.stream().mapToDouble((Double value) -> value).toArray(); inputs.odometryYawPositions = yawPositionQueue.stream() - .map((Double value) -> Rotation2d.fromDegrees(-value)) + .map((Double value) -> Rotation2d.fromRadians(value * 2 * Math.PI)) .toArray(Rotation2d[]::new); yawTimestampQueue.clear(); yawPositionQueue.clear();