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,
swerve.setStateDeadband(true);
}
+ private boolean trenchAlign = false;
+ private boolean trenchAssist = true;
+
@Override
public void execute() {
double forwardTranslation = driver.getForwardTranslation();
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
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;
*/
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
return false;
}
- public void startRumble(){
+ public void startRumble() {
driver.rumbleOn();
}
- public void endRumble(){
+ public void endRumble() {
driver.rumbleOff();
}
}
// 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.
*
--- /dev/null
+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<Double> 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<Double> 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
--- /dev/null
+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<Double> 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<Double> 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
--- /dev/null
+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