From: moo Date: Sun, 22 Feb 2026 00:37:55 +0000 (-0800) Subject: negative progress X-Git-Url: https://git.taranathan.com/?a=commitdiff_plain;h=3d2fce2725f682ddb44144d2ac5099b834bbd0e3;p=FRC2026.git negative progress --- diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 377285c..aab87c8 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -59,7 +59,7 @@ public class RobotContainer { private Command auto = new DoNothing(); // Controllers are defined here - private BaseDriverConfig driver = null; + private PS5ControllerDriverConfig driver = null; private Operator operator = null; /** 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 8156f5c..5c17630 100644 --- a/src/main/java/frc/robot/commands/drive_comm/DefaultDriveCommand.java +++ b/src/main/java/frc/robot/commands/drive_comm/DefaultDriveCommand.java @@ -3,6 +3,7 @@ package frc.robot.commands.drive_comm; import org.littletonrobotics.junction.Logger; import edu.wpi.first.math.geometry.Rectangle2d; +import edu.wpi.first.math.geometry.Rotation2d; import edu.wpi.first.math.kinematics.ChassisSpeeds; import edu.wpi.first.math.util.Units; import edu.wpi.first.wpilibj.DriverStation.Alliance; @@ -11,21 +12,24 @@ 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.controls.PS5ControllerDriverConfig; import frc.robot.subsystems.drivetrain.Drivetrain; import frc.robot.util.TrenchAssist.TrenchAssist; +import frc.robot.util.TrenchAssist.TrenchAssist2; import frc.robot.util.TrenchAssist.TrenchAssistConstants; import frc.robot.util.Vision.DriverAssist; +import lib.controllers.PS5Controller.PS5Axis; /** * Default drive command. Drives robot using driver controls. */ public class DefaultDriveCommand extends Command { protected final Drivetrain swerve; - protected final BaseDriverConfig driver; + protected final PS5ControllerDriverConfig driver; public DefaultDriveCommand( Drivetrain swerve, - BaseDriverConfig driver) { + PS5ControllerDriverConfig driver) { this.swerve = swerve; this.driver = driver; @@ -45,6 +49,20 @@ public class DefaultDriveCommand extends Command { double forwardTranslation = driver.getForwardTranslation(); double sideTranslation = driver.getSideTranslation(); double rotation = -driver.getRotation(); + // if (swerve.getTrenchAssist()) { + // sideTranslation = Math + // .sqrt(driver.controller.get(PS5Axis.LEFT_X) * driver.controller.get(PS5Axis.LEFT_Y) + // * driver.controller.get(PS5Axis.LEFT_X) * driver.controller.get(PS5Axis.LEFT_Y)) + // * + // new Rotation2d(driver.controller.get(PS5Axis.LEFT_X), driver.controller.get(PS5Axis.LEFT_Y)) + // .rotateBy(swerve.getYaw()).getCos(); + // forwardTranslation = Math + // .sqrt(driver.controller.get(PS5Axis.LEFT_X) * driver.controller.get(PS5Axis.LEFT_Y) + // * driver.controller.get(PS5Axis.LEFT_X) * driver.controller.get(PS5Axis.LEFT_Y)) + // * + // new Rotation2d(driver.controller.get(PS5Axis.LEFT_X), driver.controller.get(PS5Axis.LEFT_Y)) + // .rotateBy(swerve.getYaw()).getSin(); + // } double slowFactor = driver.getIsSlowMode() ? DriveConstants.SLOW_DRIVE_FACTOR : 1; @@ -86,7 +104,7 @@ public class DefaultDriveCommand extends Command { Logger.recordOutput("TrenchAssist", swerve.getTrenchAssist()); if (swerve.getTrenchAssist()) { - drive(TrenchAssist.calculate(swerve, corrected)); + drive(TrenchAssist2.calculate(swerve, corrected)); } else { drive(corrected); } diff --git a/src/main/java/frc/robot/commands/gpm/SimpleAutoShoot.java b/src/main/java/frc/robot/commands/gpm/SimpleAutoShoot.java index 899a51e..0f0b8dd 100644 --- a/src/main/java/frc/robot/commands/gpm/SimpleAutoShoot.java +++ b/src/main/java/frc/robot/commands/gpm/SimpleAutoShoot.java @@ -23,12 +23,10 @@ import frc.robot.subsystems.turret.ShotInterpolation; import frc.robot.subsystems.turret.Turret; import frc.robot.util.FieldZone; import frc.robot.util.ShootingTarget; -import frc.robot.util.Vision.TurretVision; public class SimpleAutoShoot extends Command { private Turret turret; private Drivetrain drivetrain; - private TurretVision turretVision; private Shooter shooter; private double fieldAngleRad; @@ -93,19 +91,6 @@ public class SimpleAutoShoot extends Command { // System.out.println("Aligning the turn to degree angle: " + turretSetpoint); } - public void adjustWithTurretCam() { - if(turretVision.canSeeTag(26) || turretVision.canSeeTag(10)){ - // Adjust turret setpoint based on vision input - if(Robot.getAlliance() == Alliance.Blue){ - yawToTagCamera = -1 * Units.radiansToDegrees(turretVision.getYawToTagRad(26).get()); - } - else{ - yawToTagCamera = -1 * Units.radiansToDegrees(turretVision.getYawToTagRad(10).get()); - } - double error = yawToTagCamera - yawToTag; - adjustedSetpoint = turretSetpoint + error; - } - } public void updateYawToTag(){ // Calculate the yaw offset to the tag diff --git a/src/main/java/frc/robot/commands/vision/AimAtGamePiece.java b/src/main/java/frc/robot/commands/vision/AimAtGamePiece.java index f3c371c..ad8eef6 100644 --- a/src/main/java/frc/robot/commands/vision/AimAtGamePiece.java +++ b/src/main/java/frc/robot/commands/vision/AimAtGamePiece.java @@ -7,6 +7,7 @@ import edu.wpi.first.math.kinematics.ChassisSpeeds; import frc.robot.commands.drive_comm.DefaultDriveCommand; import frc.robot.constants.VisionConstants; import frc.robot.controls.BaseDriverConfig; +import frc.robot.controls.PS5ControllerDriverConfig; import frc.robot.subsystems.drivetrain.Drivetrain; import frc.robot.util.Vision.DetectedObject; @@ -16,7 +17,7 @@ public class AimAtGamePiece extends DefaultDriveCommand { private static DetectedObject cachedObject; - public AimAtGamePiece(Drivetrain drive, BaseDriverConfig driver, Supplier objectSupplier){ + public AimAtGamePiece(Drivetrain drive, PS5ControllerDriverConfig driver, Supplier objectSupplier){ super(drive, driver); this.objectSupplier = objectSupplier; } diff --git a/src/main/java/frc/robot/controls/PS5ControllerDriverConfig.java b/src/main/java/frc/robot/controls/PS5ControllerDriverConfig.java index 6df9538..0e035cc 100644 --- a/src/main/java/frc/robot/controls/PS5ControllerDriverConfig.java +++ b/src/main/java/frc/robot/controls/PS5ControllerDriverConfig.java @@ -21,7 +21,7 @@ import lib.controllers.PS5Controller.PS5Button; * Driver controls for the PS5 controller */ public class PS5ControllerDriverConfig extends BaseDriverConfig { - private final PS5Controller driver = new PS5Controller(Constants.DRIVER_JOY); + public final PS5Controller controller = new PS5Controller(Constants.DRIVER_JOY); private final BooleanSupplier slowModeSupplier = () -> false; private boolean intakeBoolean = true; private Command autoShoot = null; @@ -40,27 +40,27 @@ public class PS5ControllerDriverConfig extends BaseDriverConfig { public void configureControls() { // Reset the yaw. Mainly useful for testing/driver practice - driver.get(PS5Button.CREATE).onTrue(new InstantCommand(() -> getDrivetrain().setYaw( + controller.get(PS5Button.CREATE).onTrue(new InstantCommand(() -> getDrivetrain().setYaw( new Rotation2d(Robot.getAlliance() == Alliance.Blue ? 0 : Math.PI)))); // Cancel commands - driver.get(PS5Button.RIGHT_TRIGGER).onTrue(new InstantCommand(() -> { + controller.get(PS5Button.RIGHT_TRIGGER).onTrue(new InstantCommand(() -> { getDrivetrain().setIsAlign(false); getDrivetrain().setDesiredPose(() -> null); CommandScheduler.getInstance().cancelAll(); })); // Align wheels - driver.get(PS5Button.MUTE).onTrue(new FunctionalCommand( + controller.get(PS5Button.MUTE).onTrue(new FunctionalCommand( () -> getDrivetrain().setStateDeadband(false), getDrivetrain()::alignWheels, interrupted -> getDrivetrain().setStateDeadband(true), () -> false, getDrivetrain()).withTimeout(2)); - driver.get(PS5Button.CROSS).onTrue(new InstantCommand(() -> getDrivetrain().setTrenchAlign(true))) + controller.get(PS5Button.CROSS).onTrue(new InstantCommand(() -> getDrivetrain().setTrenchAlign(true))) .onFalse(new InstantCommand(() -> getDrivetrain().setTrenchAlign(false))); - driver.get(PS5Button.CIRCLE).onTrue(new InstantCommand(() -> getDrivetrain().setTrenchAssist(true))) + controller.get(PS5Button.CIRCLE).onTrue(new InstantCommand(() -> getDrivetrain().setTrenchAssist(true))) .onFalse(new InstantCommand(() -> getDrivetrain().setTrenchAssist(false))); @@ -125,27 +125,27 @@ public class PS5ControllerDriverConfig extends BaseDriverConfig { @Override public double getRawSideTranslation() { - return driver.get(PS5Axis.LEFT_X); + return controller.get(PS5Axis.LEFT_X); } @Override public double getRawForwardTranslation() { - return driver.get(PS5Axis.LEFT_Y); + return controller.get(PS5Axis.LEFT_Y); } @Override public double getRawRotation() { - return driver.get(PS5Axis.RIGHT_X); + return controller.get(PS5Axis.RIGHT_X); } @Override public double getRawHeadingAngle() { - return Math.atan2(driver.get(PS5Axis.RIGHT_X), -driver.get(PS5Axis.RIGHT_Y)) - Math.PI / 2; + return Math.atan2(controller.get(PS5Axis.RIGHT_X), -controller.get(PS5Axis.RIGHT_Y)) - Math.PI / 2; } @Override public double getRawHeadingMagnitude() { - return Math.hypot(driver.get(PS5Axis.RIGHT_X), driver.get(PS5Axis.RIGHT_Y)); + return Math.hypot(controller.get(PS5Axis.RIGHT_X), controller.get(PS5Axis.RIGHT_Y)); } @Override @@ -159,10 +159,10 @@ public class PS5ControllerDriverConfig extends BaseDriverConfig { } public void startRumble() { - driver.rumbleOn(); + controller.rumbleOn(); } public void endRumble() { - driver.rumbleOff(); + controller.rumbleOff(); } } diff --git a/src/main/java/frc/robot/util/TrenchAssist/TrenchAssist2.java b/src/main/java/frc/robot/util/TrenchAssist/TrenchAssist2.java index 183791d..af10768 100644 --- a/src/main/java/frc/robot/util/TrenchAssist/TrenchAssist2.java +++ b/src/main/java/frc/robot/util/TrenchAssist/TrenchAssist2.java @@ -110,68 +110,33 @@ public class TrenchAssist2 { return Optional.empty(); } - static Translation2d calculateCorrection(Rectangle2d[] rectangles, Drivetrain drive, ChassisSpeeds predictedSpeeds) { - Pose2d pose = drive.getPose(); - - Translation2d velocityRobotRelative = new Translation2d(predictedSpeeds.vxMetersPerSecond, - predictedSpeeds.vyMetersPerSecond); - - Translation2d velocityFieldRelative = velocityRobotRelative.rotateBy(drive.getYaw()); - - double halfRobotWidth = 0.5 * DriveConstants.ROBOT_WIDTH_WITH_BUMPERS; - - Translation2d[] robotCorners = new Translation2d[] { - pose.transformBy(new Transform2d(new Translation2d(halfRobotWidth, 0), new Rotation2d(0.0))) - .getTranslation(), - pose.transformBy(new Transform2d(new Translation2d(0, halfRobotWidth), new Rotation2d(0.0))) - .getTranslation(), - pose.transformBy(new Transform2d(new Translation2d(-halfRobotWidth, 0), new Rotation2d(0.0))) - .getTranslation(), - pose.transformBy(new Transform2d(new Translation2d(0, -halfRobotWidth), new Rotation2d(0.0))) - .getTranslation(), - }; - - for (Translation2d corner : robotCorners) { - for (Rectangle2d rectangle : rectangles) { - Optional distanceOptional = rayCast(rectangle, corner, velocityFieldRelative, 1.0); - if (distanceOptional.isPresent()) { - double timeToCollision = distanceOptional.get(); // seconds - double timeWindow = 1.0; //secs - double scale = Math.max(0.0, 1.0 - (timeToCollision / timeWindow)); - - - if (drive.getPose().getY() > rectangle.getCenter().getY() + (rectangle.getYWidth() / 2)) { - // above rectangle: push +90 deg - return velocityFieldRelative.rotateBy(new Rotation2d(Units.degreesToRadians(90))).times(scale); - } else if (drive.getPose().getY() <= rectangle.getCenter().getY() - (rectangle.getYWidth() / 2)) { - // below rectangle push -90 deg - return velocityFieldRelative.rotateBy(new Rotation2d(Units.degreesToRadians(-90))).times(scale); - } - - // emergency fallback - if (rayCast(rectangle, corner, velocityFieldRelative, 0.2).isPresent()) { - return velocityFieldRelative.unaryMinus(); - } - } - } - } - return new Translation2d(0, 0); - } + public static ChassisSpeeds funky(Drivetrain drive, ChassisSpeeds chassisSpeeds) { + ChassisSpeeds speedsFieldRelative = ChassisSpeeds.fromRobotRelativeSpeeds(chassisSpeeds, drive.getYaw()); + ChassisSpeeds horizontalSpeeds = new ChassisSpeeds(speedsFieldRelative.vxMetersPerSecond, 0.0, speedsFieldRelative.omegaRadiansPerSecond); + Logger.recordOutput("slideTranslation", new Translation2d(horizontalSpeeds.vxMetersPerSecond, horizontalSpeeds.vyMetersPerSecond)); - public static ChassisSpeeds calculate(Drivetrain drive, ChassisSpeeds chassisSpeeds) { - Translation2d correctionFieldRelative = calculateCorrection(TrenchAssistConstants.OBSTACLES, drive, chassisSpeeds); - - Logger.recordOutput("TrenchCorrectionFieldRelative", correctionFieldRelative); - - ChassisSpeeds correctionRobot = convertToChassisSpeedsRobotRelative(correctionFieldRelative, drive); - - double vx = chassisSpeeds.vxMetersPerSecond + correctionRobot.vxMetersPerSecond; - double vy = chassisSpeeds.vyMetersPerSecond + correctionRobot.vyMetersPerSecond; + var x = new ChassisSpeeds(horizontalSpeeds.vxMetersPerSecond, horizontalSpeeds.vyMetersPerSecond, chassisSpeeds.omegaRadiansPerSecond); + var y = new Translation2d(x.vxMetersPerSecond, x.vyMetersPerSecond).rotateBy(drive.getYaw().unaryMinus()); + return new ChassisSpeeds(y.getX(), y.getY(), x.omegaRadiansPerSecond); + } - return new ChassisSpeeds(vx, vy, chassisSpeeds.omegaRadiansPerSecond); + public static ChassisSpeeds calculate(Drivetrain drive, ChassisSpeeds chassisSpeeds) { + double yawRadians = drive.getYaw().unaryMinus().getRadians(); + + double fieldRelativeX = chassisSpeeds.vxMetersPerSecond * Math.cos(yawRadians) - chassisSpeeds.vyMetersPerSecond * Math.sin(yawRadians); + double fieldRelativeY = chassisSpeeds.vxMetersPerSecond * Math.sin(yawRadians) + chassisSpeeds.vyMetersPerSecond * Math.cos(yawRadians); + + fieldRelativeY = 0; + + double neutralizedRobotX = fieldRelativeX * Math.cos(yawRadians) + fieldRelativeY * Math.sin(yawRadians); + double neutralizedRobotY = -fieldRelativeX * Math.sin(yawRadians) + fieldRelativeY * Math.cos(yawRadians); + + var x = new Translation2d(neutralizedRobotX, neutralizedRobotY).rotateBy(drive.getYaw().unaryMinus()); + + return new ChassisSpeeds(x.getX(), x.getY(), chassisSpeeds.omegaRadiansPerSecond); } public static ChassisSpeeds convertToChassisSpeedsRobotRelative(Translation2d translation, Drivetrain drive){ @@ -184,7 +149,7 @@ public class TrenchAssist2 { 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 75e905f..29654bb 100644 --- a/src/main/java/frc/robot/util/TrenchAssist/TrenchAssistConstants.java +++ b/src/main/java/frc/robot/util/TrenchAssist/TrenchAssistConstants.java @@ -2,6 +2,7 @@ package frc.robot.util.TrenchAssist; import edu.wpi.first.math.geometry.Rectangle2d; import edu.wpi.first.math.geometry.Translation2d; +import edu.wpi.first.math.util.Units; public class TrenchAssistConstants { public static final Rectangle2d[] OBSTACLES = new Rectangle2d[]{ @@ -19,5 +20,9 @@ public class TrenchAssistConstants { // new Rectangle2d(new Translation2d(99.99, 99.9), new Translation2d(0.0, 0.0)), }; + public static final double[] SLIDE_ZONES = new double[]{ + 8.07 - Units.inchesToMeters(25.0), + Units.inchesToMeters(25.0), + }; } \ No newline at end of file diff --git a/src/main/java/frc/robot/util/Vision/TurretVision.java b/src/main/java/frc/robot/util/Vision/TurretVision.java deleted file mode 100644 index 09f69ee..0000000 --- a/src/main/java/frc/robot/util/Vision/TurretVision.java +++ /dev/null @@ -1,52 +0,0 @@ -package frc.robot.util.Vision; - -import java.util.Optional; - -import org.photonvision.PhotonCamera; -import org.photonvision.targeting.PhotonPipelineResult; -import org.photonvision.targeting.PhotonTrackedTarget; - -import edu.wpi.first.math.util.Units; - -public class TurretVision { - - private final PhotonCamera camera; - - public TurretVision(String cameraName) { - camera = new PhotonCamera(cameraName); - } - - /** - * @param tagId AprilTag ID to aim at - * @return Optional yaw error in radians (positive = target to the right, negative = target to the left) - */ - public Optional getYawToTagRad(int tagId) { - PhotonPipelineResult result = camera.getLatestResult(); - - if (!result.hasTargets()) { - return Optional.empty(); - } - - for (PhotonTrackedTarget target : result.getTargets()) { - if (target.getFiducialId() == tagId) { - // PhotonVision yaw is in DEGREES - double yawRad = Units.degreesToRadians(target.getYaw()); - return Optional.of(yawRad); - } - } - - return Optional.empty(); - } - - public boolean canSeeTag(int tagId) { - PhotonPipelineResult result = camera.getLatestResult(); - if (!result.hasTargets()) return false; - - for (PhotonTrackedTarget target : result.getTargets()) { - if (target.getFiducialId() == tagId) { - return true; - } - } - return false; - } -}