From b546502e3b92d6668b9de247ade3818093538af1 Mon Sep 17 00:00:00 2001 From: sohnshaik Date: Thu, 21 Aug 2025 18:30:47 -0400 Subject: [PATCH 1/3] main framework for idea is here, some thinking about certain things is required --- src/main/java/frc/robot/Robot.java | 35 +++++++++++++++++++++++++++++- 1 file changed, 34 insertions(+), 1 deletion(-) diff --git a/src/main/java/frc/robot/Robot.java b/src/main/java/frc/robot/Robot.java index 90855e8..d48a2f9 100644 --- a/src/main/java/frc/robot/Robot.java +++ b/src/main/java/frc/robot/Robot.java @@ -10,6 +10,8 @@ import java.util.ArrayList; import java.util.List; import java.util.Optional; +import java.util.function.Supplier; + import org.photonvision.EstimatedRobotPose; import com.ctre.phoenix6.swerve.SwerveDrivetrain.SwerveDriveState; @@ -69,6 +71,7 @@ public class Robot extends TimedRobot { private final double kMaxTranslationSpeed = TunerConstants.kSpeedAt12Volts.in(MetersPerSecond); // kSpeedAt12Volts desired top speed private final double kMaxAngularRate = RotationsPerSecond.of(0.75).in(RadiansPerSecond); // 3/4 of a rotation per second max angular velocity private final double kMaxHighAngularRate = RotationsPerSecond.of(1.5).in(RadiansPerSecond); + private final double kSlowSpeed = kMaxTranslationSpeed * .5; private final Telemetry logger = new Telemetry(kMaxTranslationSpeed); /* Setting up bindings for necessary control of the swerve drive platform */ @@ -440,6 +443,30 @@ private void configureBindings() { } + private Supplier getTeleSwerveReq() { + return () -> { + double leftY = -driver.getLeftY(); + double leftX = -driver.getLeftX(); + return drive + .withVelocityX(leftY * kMaxTranslationSpeed) + .withVelocityY(leftX * kMaxTranslationSpeed) + .withRotationalRate(-driver.getRightX() * kMaxAngularRate) + .withRotationalDeadband(kMaxAngularRate * 0.1); + }; + } + + private Supplier getSlowerTeleSwerveReq() { + return () -> { + double leftY = -driver.getLeftY(); + double leftX = -driver.getLeftX(); + return drive + .withVelocityX(leftY * kSlowSpeed) + .withVelocityY(leftX * kSlowSpeed) + .withRotationalRate(-driver.getRightX() * kMaxAngularRate) + .withRotationalDeadband(kMaxAngularRate * 0.1); + }; + } + private void driverRumble(double intensity) { if (!DriverStation.isAutonomous()) { driver.getHID().setRumble(RumbleType.kBothRumble, intensity); @@ -619,7 +646,13 @@ public void teleopInit() { } @Override - public void teleopPeriodic() {} + public void teleopPeriodic() { + if (elevator.nearSetpoint() && trg_toL4.getAsBoolean() && DriverStation.isTeleop()) { + drivetrain.applyRequest(getSlowerTeleSwerveReq()) + .until(trg_teleopScoreReq) + .andThen(drivetrain.applyRequest(getTeleSwerveReq())); + } + } @Override public void teleopExit() {} From 53ae4e1acdbb865ab296145db05f318c17a986be Mon Sep 17 00:00:00 2001 From: sohnshaik Date: Thu, 28 Aug 2025 19:47:15 -0400 Subject: [PATCH 2/3] made the slowerize more fitting to ideals banks made comments on how we needed to fix, made fix and waiting for review. --- src/main/java/frc/robot/Robot.java | 24 ++++++++++++------- .../java/frc/robot/subsystems/Elevator.java | 2 +- 2 files changed, 16 insertions(+), 10 deletions(-) diff --git a/src/main/java/frc/robot/Robot.java b/src/main/java/frc/robot/Robot.java index d48a2f9..1bca47e 100644 --- a/src/main/java/frc/robot/Robot.java +++ b/src/main/java/frc/robot/Robot.java @@ -353,7 +353,7 @@ private void configureTestBindings() { // driver.start().whileTrue(drivetrain.wheelRadiusCharacterization(1)); } - private Command driveCommand() { + private Command driveCommand(boolean slow) { // Note that X is defined as forward according to WPILib convention, // and Y is defined as to the left according to WPILib convention. // Drivetrain will execute this command periodically @@ -368,8 +368,18 @@ private Command driveCommand() { log_stickDesiredFieldX.accept(driverXVelo); log_stickDesiredFieldY.accept(driverYVelo); log_stickDesiredFieldZRot.accept(driverYawRate); - - return drive + if (elevator.nearSetpoint() && trg_toL4.getAsBoolean() && DriverStation.isTeleop()) { + drivetrain.applyRequest(getSlowerTeleSwerveReq()) + .until(trg_teleopScoreReq) + .andThen(drivetrain.applyRequest(getTeleSwerveReq())); + } + if(slow) { + return drive + .withVelocityX(driverXVelo * kSlowSpeed ) // Drive forward with Y (forward) + .withVelocityY(driverYVelo * kSlowSpeed) // Drive left with X (left) + .withRotationalRate(driverYawRate); // Drive counterclockwise with negative X (left) + } + return drive .withVelocityX(driverXVelo) // Drive forward with Y (forward) .withVelocityY(driverYVelo) // Drive left with X (left) .withRotationalRate(driverYawRate); // Drive counterclockwise with negative X (left) @@ -377,7 +387,7 @@ private Command driveCommand() { } private void configureBindings() { - drivetrain.setDefaultCommand(driveCommand()); + drivetrain.setDefaultCommand(driveCommand(elevator.getPulleyRotations() >= (8.451660 + (0.169 / 2)))); trg_driverDanger.and(driver.leftBumper()).whileTrue( Commands.parallel( @@ -647,11 +657,7 @@ public void teleopInit() { @Override public void teleopPeriodic() { - if (elevator.nearSetpoint() && trg_toL4.getAsBoolean() && DriverStation.isTeleop()) { - drivetrain.applyRequest(getSlowerTeleSwerveReq()) - .until(trg_teleopScoreReq) - .andThen(drivetrain.applyRequest(getTeleSwerveReq())); - } + } @Override diff --git a/src/main/java/frc/robot/subsystems/Elevator.java b/src/main/java/frc/robot/subsystems/Elevator.java index 6814b1e..d474965 100644 --- a/src/main/java/frc/robot/subsystems/Elevator.java +++ b/src/main/java/frc/robot/subsystems/Elevator.java @@ -196,7 +196,7 @@ public boolean nearSetpoint(double tolerancePulleyRotations, double heightRots) return Math.abs(diff) <= tolerancePulleyRotations; } - private double getPulleyRotations() { + public double getPulleyRotations() { return m_rotationsSignal.refresh().getValueAsDouble(); } From 044886cccf3214130865d92e5511c2ce10bd8361 Mon Sep 17 00:00:00 2001 From: sohnshaik Date: Mon, 1 Sep 2025 18:55:13 -0400 Subject: [PATCH 3/3] slowerization works; need slew rate limiter TESTED: everything works as intended, just need to implement a slew rate limiter that way robot wont tip when going to L3/4 fast. --- src/main/java/frc/robot/Robot.java | 53 +++++++----------------------- 1 file changed, 12 insertions(+), 41 deletions(-) diff --git a/src/main/java/frc/robot/Robot.java b/src/main/java/frc/robot/Robot.java index 1bca47e..d5191ea 100644 --- a/src/main/java/frc/robot/Robot.java +++ b/src/main/java/frc/robot/Robot.java @@ -71,7 +71,7 @@ public class Robot extends TimedRobot { private final double kMaxTranslationSpeed = TunerConstants.kSpeedAt12Volts.in(MetersPerSecond); // kSpeedAt12Volts desired top speed private final double kMaxAngularRate = RotationsPerSecond.of(0.75).in(RadiansPerSecond); // 3/4 of a rotation per second max angular velocity private final double kMaxHighAngularRate = RotationsPerSecond.of(1.5).in(RadiansPerSecond); - private final double kSlowSpeed = kMaxTranslationSpeed * .5; + private final double kSlowSpeed = kMaxTranslationSpeed * .1; private final Telemetry logger = new Telemetry(kMaxTranslationSpeed); /* Setting up bindings for necessary control of the swerve drive platform */ @@ -353,33 +353,27 @@ private void configureTestBindings() { // driver.start().whileTrue(drivetrain.wheelRadiusCharacterization(1)); } - private Command driveCommand(boolean slow) { + private Command driveCommand() { // Note that X is defined as forward according to WPILib convention, // and Y is defined as to the left according to WPILib convention. // Drivetrain will execute this command periodically + + //define slewrate limiter here return drivetrain.applyRequest(() -> { var angularRate = driver.leftTrigger().getAsBoolean() ? kMaxHighAngularRate : kMaxAngularRate; - var driverXVelo = -driver.getLeftY() * kMaxTranslationSpeed; - var driverYVelo = -driver.getLeftX() * kMaxTranslationSpeed; + boolean slow = elevator.getPulleyRotations() >= (8.451660 + (0.169 / 2)); + double actualMaxTrSpeed = slow ? kSlowSpeed : kMaxTranslationSpeed; + //reset slewratelimiter here + var driverXVelo = -driver.getLeftY() * actualMaxTrSpeed; + var driverYVelo = -driver.getLeftX() * actualMaxTrSpeed; var driverYawRate = -driver.getRightX() * angularRate; log_stickDesiredFieldX.accept(driverXVelo); log_stickDesiredFieldY.accept(driverYVelo); log_stickDesiredFieldZRot.accept(driverYawRate); - if (elevator.nearSetpoint() && trg_toL4.getAsBoolean() && DriverStation.isTeleop()) { - drivetrain.applyRequest(getSlowerTeleSwerveReq()) - .until(trg_teleopScoreReq) - .andThen(drivetrain.applyRequest(getTeleSwerveReq())); - } - if(slow) { - return drive - .withVelocityX(driverXVelo * kSlowSpeed ) // Drive forward with Y (forward) - .withVelocityY(driverYVelo * kSlowSpeed) // Drive left with X (left) - .withRotationalRate(driverYawRate); // Drive counterclockwise with negative X (left) - } - return drive + return drive .withVelocityX(driverXVelo) // Drive forward with Y (forward) .withVelocityY(driverYVelo) // Drive left with X (left) .withRotationalRate(driverYawRate); // Drive counterclockwise with negative X (left) @@ -387,7 +381,7 @@ private Command driveCommand(boolean slow) { } private void configureBindings() { - drivetrain.setDefaultCommand(driveCommand(elevator.getPulleyRotations() >= (8.451660 + (0.169 / 2)))); + drivetrain.setDefaultCommand(driveCommand()); trg_driverDanger.and(driver.leftBumper()).whileTrue( Commands.parallel( @@ -453,30 +447,7 @@ private void configureBindings() { } - private Supplier getTeleSwerveReq() { - return () -> { - double leftY = -driver.getLeftY(); - double leftX = -driver.getLeftX(); - return drive - .withVelocityX(leftY * kMaxTranslationSpeed) - .withVelocityY(leftX * kMaxTranslationSpeed) - .withRotationalRate(-driver.getRightX() * kMaxAngularRate) - .withRotationalDeadband(kMaxAngularRate * 0.1); - }; - } - - private Supplier getSlowerTeleSwerveReq() { - return () -> { - double leftY = -driver.getLeftY(); - double leftX = -driver.getLeftX(); - return drive - .withVelocityX(leftY * kSlowSpeed) - .withVelocityY(leftX * kSlowSpeed) - .withRotationalRate(-driver.getRightX() * kMaxAngularRate) - .withRotationalDeadband(kMaxAngularRate * 0.1); - }; - } - + private void driverRumble(double intensity) { if (!DriverStation.isAutonomous()) { driver.getHID().setRumble(RumbleType.kBothRumble, intensity);