From 523954bf69ff0ba8ef59a26619980e1d2adf606f Mon Sep 17 00:00:00 2001 From: moo Date: Tue, 24 Feb 2026 17:06:44 -0800 Subject: [PATCH] yayy finished kousha happy yayy --- .../drive_comm/DefaultDriveCommand.java | 12 +++-- .../controls/PS5ControllerDriverConfig.java | 9 +++- .../util/TrenchAssist/TrenchAssist2.java | 54 +++++++++---------- .../TrenchAssist/TrenchAssistConstants.java | 8 +-- 4 files changed, 45 insertions(+), 38 deletions(-) diff --git a/src/main/java/frc/robot/commands/drive_comm/DefaultDriveCommand.java b/src/main/java/frc/robot/commands/drive_comm/DefaultDriveCommand.java index 28775d4..90f0c5e 100644 --- a/src/main/java/frc/robot/commands/drive_comm/DefaultDriveCommand.java +++ b/src/main/java/frc/robot/commands/drive_comm/DefaultDriveCommand.java @@ -2,6 +2,7 @@ package frc.robot.commands.drive_comm; import org.littletonrobotics.junction.Logger; +import edu.wpi.first.math.controller.PIDController; import edu.wpi.first.math.geometry.Rectangle2d; import edu.wpi.first.math.geometry.Rotation2d; import edu.wpi.first.math.kinematics.ChassisSpeeds; @@ -26,6 +27,7 @@ import lib.controllers.PS5Controller.PS5Axis; public class DefaultDriveCommand extends Command { protected final Drivetrain swerve; protected final BaseDriverConfig driver; + private PIDController trenchAssistPid = new PIDController(9, 0.0, 3); public DefaultDriveCommand( Drivetrain swerve, @@ -39,6 +41,10 @@ public class DefaultDriveCommand extends Command { @Override public void initialize() { swerve.setStateDeadband(true); + + trenchAssistPid.setIZone(2); + trenchAssistPid.setIntegratorRange(-1, 1); + } private boolean trenchAlign = false; @@ -90,13 +96,11 @@ public class DefaultDriveCommand extends Command { Logger.recordOutput("TrenchAssist", swerve.getTrenchAssist()); if (swerve.getTrenchAssist()) { - drive(TrenchAssist2.calculate(swerve, corrected)); + drive(TrenchAssist2.calculate(swerve, corrected, trenchAssistPid)); } else { + trenchAssistPid.reset(); drive(corrected); } - - Logger.recordOutput("isAlign", swerve.getIsAlign()); - Logger.recordOutput("alignAngle", swerve.getAlignAngle()); } /** diff --git a/src/main/java/frc/robot/controls/PS5ControllerDriverConfig.java b/src/main/java/frc/robot/controls/PS5ControllerDriverConfig.java index 0e035cc..97e9b95 100644 --- a/src/main/java/frc/robot/controls/PS5ControllerDriverConfig.java +++ b/src/main/java/frc/robot/controls/PS5ControllerDriverConfig.java @@ -4,6 +4,7 @@ import java.util.function.BooleanSupplier; import edu.wpi.first.math.geometry.Rotation2d; import edu.wpi.first.wpilibj.DriverStation.Alliance; +import edu.wpi.first.wpilibj.PS5Controller.Button; import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.CommandScheduler; import edu.wpi.first.wpilibj2.command.FunctionalCommand; @@ -14,6 +15,7 @@ import frc.robot.subsystems.drivetrain.Drivetrain; import frc.robot.subsystems.shooter.Shooter; import frc.robot.subsystems.turret.Turret; import lib.controllers.PS5Controller; +import lib.controllers.PS5Controller.DPad; import lib.controllers.PS5Controller.PS5Axis; import lib.controllers.PS5Controller.PS5Button; @@ -60,8 +62,11 @@ public class PS5ControllerDriverConfig extends BaseDriverConfig { controller.get(PS5Button.CROSS).onTrue(new InstantCommand(() -> getDrivetrain().setTrenchAlign(true))) .onFalse(new InstantCommand(() -> getDrivetrain().setTrenchAlign(false))); - controller.get(PS5Button.CIRCLE).onTrue(new InstantCommand(() -> getDrivetrain().setTrenchAssist(true))) - .onFalse(new InstantCommand(() -> getDrivetrain().setTrenchAssist(false))); + controller.get(PS5Button.CIRCLE).onTrue(new InstantCommand(() -> {getDrivetrain().setTrenchAssist(true); getDrivetrain().setTrenchAlign(true);})) + .onFalse(new InstantCommand(() -> {getDrivetrain().setTrenchAssist(false); getDrivetrain().setTrenchAlign(false);})); + + controller.get(DPad.LEFT).onTrue(new InstantCommand(() -> {getDrivetrain().setTrenchAssist(true); getDrivetrain().setTrenchAlign(true);})) + .onFalse(new InstantCommand(() -> {getDrivetrain().setTrenchAssist(false); getDrivetrain().setTrenchAlign(false);})); diff --git a/src/main/java/frc/robot/util/TrenchAssist/TrenchAssist2.java b/src/main/java/frc/robot/util/TrenchAssist/TrenchAssist2.java index 0a2f142..18972a5 100644 --- a/src/main/java/frc/robot/util/TrenchAssist/TrenchAssist2.java +++ b/src/main/java/frc/robot/util/TrenchAssist/TrenchAssist2.java @@ -1,70 +1,68 @@ package frc.robot.util.TrenchAssist; -import static edu.wpi.first.units.Units.Rotation; - -import java.util.Optional; - import org.littletonrobotics.junction.Logger; -import org.opencv.core.Point; +import edu.wpi.first.math.controller.PIDController; import edu.wpi.first.math.geometry.Pose2d; -import edu.wpi.first.math.geometry.Rectangle2d; import edu.wpi.first.math.geometry.Rotation2d; -import edu.wpi.first.math.geometry.Transform2d; import edu.wpi.first.math.geometry.Translation2d; import edu.wpi.first.math.kinematics.ChassisSpeeds; import edu.wpi.first.math.util.Units; -import edu.wpi.first.units.measure.Distance; -import edu.wpi.first.wpilibj.RobotBase; -import edu.wpi.first.wpilibj.DriverStation.Alliance; -import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; -import edu.wpi.first.wpilibj2.command.Command; -import frc.robot.Robot; -import frc.robot.constants.swerve.DriveConstants; -import frc.robot.controls.BaseDriverConfig; import frc.robot.subsystems.drivetrain.Drivetrain; -import frc.robot.util.Vision.DriverAssist; -import frc.robot.util.TrenchAssist.TrenchAssistConstants; public class TrenchAssist2 { - private static final double SCALE = 1.0; + // private static final double SCALE = 1.0; - public static ChassisSpeeds calculate(Drivetrain drive, ChassisSpeeds chassisSpeeds) { - //ChassisSpeeds speedsFieldRelative = ChassisSpeeds.fromRobotRelativeSpeeds(chassisSpeeds, drive.getYaw().unaryMinus()); //this singular unary minus is best we can get + public static ChassisSpeeds calculate(Drivetrain drive, ChassisSpeeds chassisSpeeds, PIDController pid) { + // ChassisSpeeds speedsFieldRelative = + // ChassisSpeeds.fromRobotRelativeSpeeds(chassisSpeeds, + // drive.getYaw().unaryMinus()); double distanceFromSlideLatitude; - if (drive.getPose().getY() > (8.07 / 2.0)){ + if (drive.getPose().getY() > (8.07 / 2.0)) { distanceFromSlideLatitude = (drive.getPose().getY() - TrenchAssistConstants.SLIDE_LATITUDES[0]); } else { distanceFromSlideLatitude = (drive.getPose().getY() - TrenchAssistConstants.SLIDE_LATITUDES[1]); } - var impulse = -distanceFromSlideLatitude * SCALE; + Logger.recordOutput("slides", + new Pose2d[] { new Pose2d(2.0, TrenchAssistConstants.SLIDE_LATITUDES[0], Rotation2d.kZero), + new Pose2d(2.0, TrenchAssistConstants.SLIDE_LATITUDES[1], Rotation2d.kZero) }); + + double impulse = pid.calculate(distanceFromSlideLatitude, 0); + + if (distanceFromSlideLatitude < Units.inchesToMeters(3)){ + impulse = 0.0; + } + - ChassisSpeeds horizontalSpeeds = new ChassisSpeeds(chassisSpeeds.vxMetersPerSecond, impulse, chassisSpeeds.omegaRadiansPerSecond); + ChassisSpeeds horizontalSpeeds = new ChassisSpeeds(chassisSpeeds.vxMetersPerSecond, impulse, + chassisSpeeds.omegaRadiansPerSecond); - var x = new ChassisSpeeds(horizontalSpeeds.vxMetersPerSecond, horizontalSpeeds.vyMetersPerSecond, chassisSpeeds.omegaRadiansPerSecond); + var x = new ChassisSpeeds(horizontalSpeeds.vxMetersPerSecond, horizontalSpeeds.vyMetersPerSecond, + chassisSpeeds.omegaRadiansPerSecond); var y = new Translation2d(x.vxMetersPerSecond, x.vyMetersPerSecond).rotateBy(drive.getYaw()); - var z = ChassisSpeeds.fromFieldRelativeSpeeds(new ChassisSpeeds(y.getX(), y.getY(), x.omegaRadiansPerSecond), drive.getYaw()); + var z = ChassisSpeeds.fromFieldRelativeSpeeds(new ChassisSpeeds(y.getX(), y.getY(), x.omegaRadiansPerSecond), + drive.getYaw()); return z; } - public static ChassisSpeeds convertToChassisSpeedsRobotRelative(Translation2d translation, Drivetrain drive){ + public static ChassisSpeeds convertToChassisSpeedsRobotRelative(Translation2d translation, Drivetrain drive) { Rotation2d yaw = drive.getYaw(); Translation2d robotRelative = translation.rotateBy(yaw); return new ChassisSpeeds(robotRelative.getX(), robotRelative.getY(), 0.0); } - public static Translation2d convertToTranslation2dFieldRelative(ChassisSpeeds speeds, Drivetrain drive){ + public static Translation2d convertToTranslation2dFieldRelative(ChassisSpeeds speeds, Drivetrain drive) { Rotation2d yaw = drive.getYaw(); Translation2d robotTranslation = new Translation2d(speeds.vxMetersPerSecond, speeds.vyMetersPerSecond); - return robotTranslation.rotateBy(yaw.times(-1)); + return robotTranslation.rotateBy(yaw.times(-1)); } } \ No newline at end of file diff --git a/src/main/java/frc/robot/util/TrenchAssist/TrenchAssistConstants.java b/src/main/java/frc/robot/util/TrenchAssist/TrenchAssistConstants.java index 1c9c5ca..48b9265 100644 --- a/src/main/java/frc/robot/util/TrenchAssist/TrenchAssistConstants.java +++ b/src/main/java/frc/robot/util/TrenchAssist/TrenchAssistConstants.java @@ -21,10 +21,10 @@ public class TrenchAssistConstants { }; public static final double[] SLIDE_LATITUDES = new double[]{ - // 8.07 - Units.inchesToMeters(30.0), - // Units.inchesToMeters(30.0), should be accurate, i think our field is slightly too small - 6.550, - 0.668, + 8.07 - Units.inchesToMeters(30.0), + Units.inchesToMeters(30.0), // should be accurate, i think our field is slightly too small + // 6.550, + // 0.668, }; -- 2.39.5