From: moo Date: Sat, 21 Feb 2026 22:11:46 +0000 (-0800) Subject: works X-Git-Url: https://git.taranathan.com/?a=commitdiff_plain;h=23f889f999e2c797e02e981d9a2c46f5ed43be3b;p=FRC2026.git works ignore trenchassist2 --- 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 c289b5d..8156f5c 100644 --- a/src/main/java/frc/robot/commands/drive_comm/DefaultDriveCommand.java +++ b/src/main/java/frc/robot/commands/drive_comm/DefaultDriveCommand.java @@ -1,21 +1,27 @@ package frc.robot.commands.drive_comm; +import org.littletonrobotics.junction.Logger; + +import edu.wpi.first.math.geometry.Rectangle2d; import edu.wpi.first.math.kinematics.ChassisSpeeds; +import edu.wpi.first.math.util.Units; 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.TrenchAssist.TrenchAssist; +import frc.robot.util.TrenchAssist.TrenchAssistConstants; import frc.robot.util.Vision.DriverAssist; - /** * Default drive command. Drives robot using driver controls. */ public class DefaultDriveCommand extends Command { protected final Drivetrain swerve; - private final BaseDriverConfig driver; + protected final BaseDriverConfig driver; public DefaultDriveCommand( Drivetrain swerve, @@ -31,6 +37,9 @@ public class DefaultDriveCommand extends Command { swerve.setStateDeadband(true); } + private boolean trenchAlign = false; + private boolean trenchAssist = true; + @Override public void execute() { double forwardTranslation = driver.getForwardTranslation(); @@ -50,29 +59,63 @@ public class DefaultDriveCommand extends Command { ChassisSpeeds driverInput = new ChassisSpeeds(forwardTranslation, sideTranslation, rotation); ChassisSpeeds corrected = DriverAssist.calculate(swerve, driverInput, swerve.getDesiredPose(), true); - drive(corrected); + Logger.recordOutput("TrenchAlign", swerve.getTrenchAlign()); + Logger.recordOutput("AlignZones", TrenchAssistConstants.ALIGN_ZONES); + if (swerve.getTrenchAlign()) { + boolean inZone = false; + for (Rectangle2d rectangle : TrenchAssistConstants.ALIGN_ZONES) { + if (rectangle.contains(swerve.getPose().getTranslation())) { + inZone = true; + } + } + + if (inZone) { + Logger.recordOutput("InAlignZone", true); + swerve.setIsAlign(true); + + double yawDegrees = swerve.getYaw().getDegrees(); + double snappedDeg = Math.round(yawDegrees / 90.0) * 90.0; + swerve.setAlignAngle(Units.degreesToRadians(snappedDeg)); + } else { + Logger.recordOutput("InAlignZone", false); + swerve.setIsAlign(false); + } + } else { + swerve.setIsAlign(false); + } + + Logger.recordOutput("TrenchAssist", swerve.getTrenchAssist()); + if (swerve.getTrenchAssist()) { + drive(TrenchAssist.calculate(swerve, corrected)); + } else { + drive(corrected); + } + + Logger.recordOutput("isAlign", swerve.getIsAlign()); + Logger.recordOutput("alignAngle", swerve.getAlignAngle()); } /** * Drives the robot + * * @param speeds The ChassisSpeeds to drive at */ - protected void drive(ChassisSpeeds speeds){ + protected void drive(ChassisSpeeds speeds) { // If the driver is pressing the align button or a command set the drivetrain to // align, then align to speaker if (driver.getIsAlign() || swerve.getIsAlign()) { swerve.driveHeading( - speeds.vxMetersPerSecond, - speeds.vyMetersPerSecond, - swerve.getAlignAngle(), - true); + speeds.vxMetersPerSecond, + speeds.vyMetersPerSecond, + swerve.getAlignAngle(), + true); } else { swerve.drive( - speeds.vxMetersPerSecond, - speeds.vyMetersPerSecond, - speeds.omegaRadiansPerSecond, - true, - false); + speeds.vxMetersPerSecond, + speeds.vyMetersPerSecond, + speeds.omegaRadiansPerSecond, + true, + false); } } -} +} \ No newline at end of file diff --git a/src/main/java/frc/robot/controls/PS5ControllerDriverConfig.java b/src/main/java/frc/robot/controls/PS5ControllerDriverConfig.java index 373aefc..6df9538 100644 --- a/src/main/java/frc/robot/controls/PS5ControllerDriverConfig.java +++ b/src/main/java/frc/robot/controls/PS5ControllerDriverConfig.java @@ -2,26 +2,16 @@ package frc.robot.controls; import java.util.function.BooleanSupplier; -import edu.wpi.first.math.geometry.Pose2d; import edu.wpi.first.math.geometry.Rotation2d; -import edu.wpi.first.math.geometry.Translation2d; -import edu.wpi.first.math.util.Units; import edu.wpi.first.wpilibj.DriverStation.Alliance; import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.CommandScheduler; import edu.wpi.first.wpilibj2.command.FunctionalCommand; import edu.wpi.first.wpilibj2.command.InstantCommand; -import edu.wpi.first.wpilibj2.command.SequentialCommandGroup; -import edu.wpi.first.wpilibj2.command.WaitCommand; import frc.robot.Robot; -import frc.robot.commands.gpm.SimpleAutoShoot; -import frc.robot.commands.gpm.TurretAutoShoot; -import frc.robot.commands.gpm.TurretJoyStickAim; import frc.robot.constants.Constants; -import frc.robot.constants.FieldConstants; import frc.robot.subsystems.drivetrain.Drivetrain; import frc.robot.subsystems.shooter.Shooter; -import frc.robot.subsystems.shooter.ShooterConstants; import frc.robot.subsystems.turret.Turret; import lib.controllers.PS5Controller; import lib.controllers.PS5Controller.PS5Axis; @@ -32,119 +22,105 @@ import lib.controllers.PS5Controller.PS5Button; */ public class PS5ControllerDriverConfig extends BaseDriverConfig { private final PS5Controller driver = new PS5Controller(Constants.DRIVER_JOY); - private final BooleanSupplier slowModeSupplier = ()->false; + private final BooleanSupplier slowModeSupplier = () -> false; + private boolean intakeBoolean = true; + private Command autoShoot = null; private Shooter shooter; private Turret turret; - private Pose2d alignmentPose = null; - private Command turretAutoShoot; - private Command simpleTurretAutoShoot; - private TurretJoyStickAim turretJoyStickAim; - - public PS5ControllerDriverConfig(Drivetrain drive, Shooter shooter, Turret turret) { + public PS5ControllerDriverConfig( + Drivetrain drive, + Shooter shooter, + Turret turret + ){ super(drive); this.shooter = shooter; this.turret = turret; } - public void configureControls() { + public void configureControls() { // Reset the yaw. Mainly useful for testing/driver practice driver.get(PS5Button.CREATE).onTrue(new InstantCommand(() -> getDrivetrain().setYaw( - new Rotation2d(Robot.getAlliance() == Alliance.Blue ? 0 : Math.PI) - ))); + new Rotation2d(Robot.getAlliance() == Alliance.Blue ? 0 : Math.PI)))); // Cancel commands - driver.get(PS5Button.RIGHT_TRIGGER).onTrue(new InstantCommand(()->{ + driver.get(PS5Button.RIGHT_TRIGGER).onTrue(new InstantCommand(() -> { getDrivetrain().setIsAlign(false); - getDrivetrain().setDesiredPose(()->null); + getDrivetrain().setDesiredPose(() -> null); CommandScheduler.getInstance().cancelAll(); })); // Align wheels driver.get(PS5Button.MUTE).onTrue(new FunctionalCommand( - ()->getDrivetrain().setStateDeadband(false), - getDrivetrain()::alignWheels, - interrupted->getDrivetrain().setStateDeadband(true), - ()->false, getDrivetrain()).withTimeout(2)); - - // driver.get(PS5Button.LB).onTrue( - // new SequentialCommandGroup( - // new InstantCommand(()-> shooter.setShooter(-ShooterConstants.SHOOTER_VELOCITY)), - // new WaitCommand(0.8), - // new InstantCommand(()-> shooter.setFeeder(ShooterConstants.FEEDER_RUN_POWER)) - // ) - // ).onFalse( - // new InstantCommand(() -> { - // shooter.setFeeder(0); - // shooter.setShooter(0); - // })); - //driver.get(PS5Button.TRIANGLE).onTrue(new InstantCommand(() -> shooter.setShooter(ShooterConstants.SHOOTER_VELOCITY))).onFalse(new InstantCommand(() -> shooter.setShooter(0))); - - driver.get(PS5Button.SQUARE).onTrue( - new InstantCommand(()->{ - if (simpleTurretAutoShoot != null && simpleTurretAutoShoot.isScheduled()){ - simpleTurretAutoShoot.cancel(); - } else{ - simpleTurretAutoShoot = new SimpleAutoShoot(turret, getDrivetrain(), shooter); - CommandScheduler.getInstance().schedule(simpleTurretAutoShoot); - } - }) - ); - - driver.get(PS5Button.TRIANGLE).onTrue( - new InstantCommand(()->{ - if (turretAutoShoot != null && turretAutoShoot.isScheduled()){ - turretAutoShoot.cancel(); - } else{ - turretAutoShoot = new TurretAutoShoot(turret, getDrivetrain()); - CommandScheduler.getInstance().schedule(turretAutoShoot); - } - }) - ); - - driver.get(PS5Button.LB).onTrue(new InstantCommand(() -> shooter.setFeeder(1))).onFalse( - new InstantCommand(()->{ - shooter.setFeeder(0); - }) - ); - - driver.get(PS5Button.CIRCLE).onTrue( - new InstantCommand(()->{ - turret.setFieldRelativeTarget(new Rotation2d(Units.degreesToRadians(180)), 0); - }) - ).onFalse( - new InstantCommand(()->{ - turret.setFieldRelativeTarget(new Rotation2d(Units.degreesToRadians(-170)), 0); - }) - ); - - // driver.get(PS5Button.CROSS).onTrue( - // new InstantCommand(()->{ - // if(turretJoyStickAim == null || !turretJoyStickAim.isScheduled()){ - // turretJoyStickAim = new TurretJoyStickAim(turret, this); - // turretJoyStickAim.schedule(); - // } - // }) - // ).onFalse( - // new InstantCommand(()->{ - // if(turretJoyStickAim.isScheduled()){ - // turretJoyStickAim.cancel(); + () -> getDrivetrain().setStateDeadband(false), + getDrivetrain()::alignWheels, + interrupted -> getDrivetrain().setStateDeadband(true), + () -> false, getDrivetrain()).withTimeout(2)); + + driver.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))) + .onFalse(new InstantCommand(() -> getDrivetrain().setTrenchAssist(false))); + + + + + // // Intake + // if (intake != null) { + // driver.get(PS5Button.CROSS).onTrue(new InstantCommand(() -> { + // if (intakeBoolean) { + // intake.extend(); + // intake.spinStart(); + // intakeBoolean = false; + // } else { + // intake.intermediateExtend(); + // intake.spinStop(); + // intakeBoolean = true; // } - // }) - // ); - - } - - public void setAlignmentPose(){ - Translation2d drivepose = getDrivetrain().getPose().getTranslation(); - // uses tag #?? - int tagNumber = 17; - Translation2d tagpose = FieldConstants.field.getTagPose(tagNumber).get().toPose2d().getTranslation(); - double YDifference = tagpose.getY() - drivepose.getY(); - double XDifference = tagpose.getX() - drivepose.getX(); - double angle = Math.atan(YDifference/XDifference); - alignmentPose = new Pose2d(drivepose.getX(), drivepose.getY(), new Rotation2d(angle)); - System.out.println("Alignment angle: " + Units.radiansToDegrees(angle)); + // })); + + // // Retract if hold for 3 seconds + // driver.get(PS5Button.CROSS).debounce(3.0).onTrue(new InstantCommand(()->{ + // intake.retract(); + // intakeBoolean = true; + // })); + + // // Calibration + // driver.get(PS5Button.OPTIONS).onTrue(new InstantCommand(()->{ + // intake.calibrate(); + // })).onFalse(new InstantCommand(()->{ + // intake.stopCalibrating(); + // })); + // } + + // // Spindexer + // if (spindexer != null){ + // // Will only run if we are not calling default shoot command + // driver.get(PS5Button.LB).onTrue(new InstantCommand(()-> spindexer.maxSpindexer())) + // .onFalse(new InstantCommand(()-> spindexer.stopSpindexer())); + // } + + // // Auto shoot + // if (turret != null) { + // driver.get(PS5Button.SQUARE).onTrue( + // new InstantCommand(() -> { + // if (autoShoot != null && autoShoot.isScheduled()) { + // autoShoot.cancel(); + // } else { + // autoShoot = new AutoShootCommand(turret, getDrivetrain(), hood, shooter, spindexer); + // CommandScheduler.getInstance().schedule(autoShoot); + // } + // })); + // } + + // if (climb != null) { + // driver.get(PS5Button.CIRCLE).onTrue(new InstantCommand(() -> { + // climb.hardstopCalibration(); + // })).onFalse(new InstantCommand(() -> { + // climb.stopCalibrating(); + // })); + // } } @Override @@ -182,11 +158,11 @@ public class PS5ControllerDriverConfig extends BaseDriverConfig { return false; } - public void startRumble(){ + public void startRumble() { driver.rumbleOn(); } - public void endRumble(){ + public void endRumble() { driver.rumbleOff(); } } diff --git a/src/main/java/frc/robot/subsystems/drivetrain/Drivetrain.java b/src/main/java/frc/robot/subsystems/drivetrain/Drivetrain.java index 492b2fe..2c36dee 100644 --- a/src/main/java/frc/robot/subsystems/drivetrain/Drivetrain.java +++ b/src/main/java/frc/robot/subsystems/drivetrain/Drivetrain.java @@ -344,6 +344,25 @@ public class Drivetrain extends SubsystemBase { // GETTERS AND SETTERS + private boolean trenchAssist = false; + private boolean trenchAlign = false; + + public boolean getTrenchAssist() { + return trenchAssist; + } + + public boolean getTrenchAlign() { + return trenchAlign; + } + + public void setTrenchAssist(boolean target){ + trenchAssist = target; + } + + public void setTrenchAlign(boolean target){ + trenchAlign = target; + } + /** * Sets the desired states for all swerve modules. * diff --git a/src/main/java/frc/robot/util/TrenchAssist/TrenchAssist.java b/src/main/java/frc/robot/util/TrenchAssist/TrenchAssist.java new file mode 100644 index 0000000..e35d0b9 --- /dev/null +++ b/src/main/java/frc/robot/util/TrenchAssist/TrenchAssist.java @@ -0,0 +1,190 @@ +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.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 TrenchAssist { + + // public void execute() { + // trenchAlign = SmartDashboard.getBoolean("trench aligning", trenchAlign); + // trenchAssist = SmartDashboard.getBoolean("trench aligning", trenchAssist); + + // SmartDashboard.putBoolean("trench aligning", trenchAlign); + // SmartDashboard.putBoolean("trench assisting", trenchAssist); + + // double forwardTranslation = driver.getForwardTranslation(); + // double sideTranslation = driver.getSideTranslation(); + // double rotation = -driver.getRotation(); + + // double slowFactor = driver.getIsSlowMode() ? DriveConstants.SLOW_DRIVE_FACTOR : 1; + + // forwardTranslation *= slowFactor; + // sideTranslation *= slowFactor; + // rotation *= driver.getIsSlowMode() ? DriveConstants.SLOW_ROT_FACTOR : 1; + + // int allianceReversal = Robot.getAlliance() == Alliance.Red ? 1 : -1; + // forwardTranslation *= allianceReversal; + // sideTranslation *= allianceReversal; + + // ChassisSpeeds driverInput = new ChassisSpeeds(forwardTranslation, sideTranslation, rotation); + // ChassisSpeeds corrected = DriverAssist.calculate(drive, driverInput, drive.getDesiredPose(), true); + + // if (trenchAlign) { + // for (Rectangle2d rectangle : TrenchAssistConstants.ALIGN_ZONES) { + // if (rectangle.contains(drive.getPose().getTranslation())) { + // drive.setIsAlign(true); + + // if (drive.getPose().getRotation().getDegrees() > 90 + // && drive.getPose().getRotation().getDegrees() < 270) { + // drive.setAlignAngle(0.0); + // } else if (drive.getPose().getRotation().getDegrees() > 270 + // && drive.getPose().getRotation().getDegrees() < 90) { + // drive.setAlignAngle(Units.degreesToRadians(180)); + // } + // } else { + // drive.setIsAlign(false); + // } + // } + // } + + // if (trenchAssist) { + // Translation2d calculated = calculateCorrection(TrenchAssistConstants.OBSTACLES); + // ChassisSpeeds assisted = new ChassisSpeeds(corrected.vxMetersPerSecond + calculated.getX(), + // corrected.vyMetersPerSecond + calculated.getY(), corrected.omegaRadiansPerSecond); + + // drive(assisted); + // } else { + // drive(corrected); + // } + + // } + + /** + * + * @param rectangle the rectangle that the ray should check against + * @param point the origin point of the ray + * @param velocity vector of the ray, magnitude is speed + * @param time how far into the future to check + * @return returns Optional full if intersects rectangle within time param into + * future, time/100 + * second resolution for raymarching, returns distance that it hits it + * at + */ + public static Optional rayCast(Rectangle2d rectangle, Translation2d point, Translation2d velocity, + double time) { + // double distance = velocity.getNorm(); + // Translation2d normalized = new Translation2d(velocity.getX() / distance, + // velocity.getY() / distance); + + for (int i = 0; i <= 100; i++) { + double t = (i * time) / 100.0; // seconds into the future + Translation2d ray = velocity.times(t).plus(point); + if (rectangle.contains(ray)) { + return Optional.of(t); + } + } + + 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 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; + + return new ChassisSpeeds(vx, vy, chassisSpeeds.omegaRadiansPerSecond); + } + + 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){ + Rotation2d yaw = drive.getYaw(); + Translation2d robotTranslation = new Translation2d(speeds.vxMetersPerSecond, speeds.vyMetersPerSecond); + return robotTranslation.rotateBy(yaw.times(-1)); + + } +} \ No newline at end of file diff --git a/src/main/java/frc/robot/util/TrenchAssist/TrenchAssist2.java b/src/main/java/frc/robot/util/TrenchAssist/TrenchAssist2.java new file mode 100644 index 0000000..183791d --- /dev/null +++ b/src/main/java/frc/robot/util/TrenchAssist/TrenchAssist2.java @@ -0,0 +1,190 @@ +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.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 { + + // public void execute() { + // trenchAlign = SmartDashboard.getBoolean("trench aligning", trenchAlign); + // trenchAssist = SmartDashboard.getBoolean("trench aligning", trenchAssist); + + // SmartDashboard.putBoolean("trench aligning", trenchAlign); + // SmartDashboard.putBoolean("trench assisting", trenchAssist); + + // double forwardTranslation = driver.getForwardTranslation(); + // double sideTranslation = driver.getSideTranslation(); + // double rotation = -driver.getRotation(); + + // double slowFactor = driver.getIsSlowMode() ? DriveConstants.SLOW_DRIVE_FACTOR : 1; + + // forwardTranslation *= slowFactor; + // sideTranslation *= slowFactor; + // rotation *= driver.getIsSlowMode() ? DriveConstants.SLOW_ROT_FACTOR : 1; + + // int allianceReversal = Robot.getAlliance() == Alliance.Red ? 1 : -1; + // forwardTranslation *= allianceReversal; + // sideTranslation *= allianceReversal; + + // ChassisSpeeds driverInput = new ChassisSpeeds(forwardTranslation, sideTranslation, rotation); + // ChassisSpeeds corrected = DriverAssist.calculate(drive, driverInput, drive.getDesiredPose(), true); + + // if (trenchAlign) { + // for (Rectangle2d rectangle : TrenchAssistConstants.ALIGN_ZONES) { + // if (rectangle.contains(drive.getPose().getTranslation())) { + // drive.setIsAlign(true); + + // if (drive.getPose().getRotation().getDegrees() > 90 + // && drive.getPose().getRotation().getDegrees() < 270) { + // drive.setAlignAngle(0.0); + // } else if (drive.getPose().getRotation().getDegrees() > 270 + // && drive.getPose().getRotation().getDegrees() < 90) { + // drive.setAlignAngle(Units.degreesToRadians(180)); + // } + // } else { + // drive.setIsAlign(false); + // } + // } + // } + + // if (trenchAssist) { + // Translation2d calculated = calculateCorrection(TrenchAssistConstants.OBSTACLES); + // ChassisSpeeds assisted = new ChassisSpeeds(corrected.vxMetersPerSecond + calculated.getX(), + // corrected.vyMetersPerSecond + calculated.getY(), corrected.omegaRadiansPerSecond); + + // drive(assisted); + // } else { + // drive(corrected); + // } + + // } + + /** + * + * @param rectangle the rectangle that the ray should check against + * @param point the origin point of the ray + * @param velocity vector of the ray, magnitude is speed + * @param time how far into the future to check + * @return returns Optional full if intersects rectangle within time param into + * future, time/100 + * second resolution for raymarching, returns distance that it hits it + * at + */ + public static Optional rayCast(Rectangle2d rectangle, Translation2d point, Translation2d velocity, + double time) { + // double distance = velocity.getNorm(); + // Translation2d normalized = new Translation2d(velocity.getX() / distance, + // velocity.getY() / distance); + + for (int i = 0; i <= 100; i++) { + double t = (i * time) / 100.0; // seconds into the future + Translation2d ray = velocity.times(t).plus(point); + if (rectangle.contains(ray)) { + return Optional.of(t); + } + } + + 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 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; + + return new ChassisSpeeds(vx, vy, chassisSpeeds.omegaRadiansPerSecond); + } + + 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){ + Rotation2d yaw = drive.getYaw(); + Translation2d robotTranslation = new Translation2d(speeds.vxMetersPerSecond, speeds.vyMetersPerSecond); + 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 new file mode 100644 index 0000000..75e905f --- /dev/null +++ b/src/main/java/frc/robot/util/TrenchAssist/TrenchAssistConstants.java @@ -0,0 +1,23 @@ +package frc.robot.util.TrenchAssist; + +import edu.wpi.first.math.geometry.Rectangle2d; +import edu.wpi.first.math.geometry.Translation2d; + +public class TrenchAssistConstants { + public static final Rectangle2d[] OBSTACLES = new Rectangle2d[]{ + new Rectangle2d(new Translation2d(4.03, 1.28), new Translation2d(5.22, 1.58)), + new Rectangle2d(new Translation2d(4.03, 8.07 - 1.28), new Translation2d(5.22, 8.07 - 1.58)), + new Rectangle2d(new Translation2d(11.32, 1.28), new Translation2d(12.51, 1.58)), + new Rectangle2d(new Translation2d(11.32, 8.07 - 1.28), new Translation2d(12.51, 8.07 - 1.58)), + }; //8.07m + + public static final Rectangle2d[] ALIGN_ZONES = new Rectangle2d[]{ + new Rectangle2d(new Translation2d(4.03 - .5, 0), new Translation2d(5.22 + .5, 3)), + new Rectangle2d(new Translation2d(4.03 - .5, 8.07), new Translation2d(5.22 + .5, 8.07 - 3)), + new Rectangle2d(new Translation2d(11.32 - .5, 0), new Translation2d(12.51 + .5, 3)), + new Rectangle2d(new Translation2d(11.32 - .5, 8.07), new Translation2d(12.51 + .5, 8.07 - 3)), + // new Rectangle2d(new Translation2d(99.99, 99.9), new Translation2d(0.0, 0.0)), + }; + + +} \ No newline at end of file