From f2946c11a94e33e93af21bb083922019cf77bd65 Mon Sep 17 00:00:00 2001 From: Zach Wilke <83995467+C4llSqin@users.noreply.github.com> Date: Thu, 30 Oct 2025 17:19:46 -0600 Subject: [PATCH] Update --- .vscode/settings.json | 3 +- src/main/deploy/pathplanner/navgrid.json | 1 + .../java/frc4388/robot/RobotContainer.java | 14 -- src/main/java/frc4388/robot/RobotMap.java | 24 --- .../commands/Autos/neoPlaybackChooser.java | 2 +- .../robot/commands/Swerve/RotateToAngle.java | 2 +- .../robot/commands/alignment/DriveToReef.java | 188 ------------------ .../commands/alignment/DriveUntilLiDAR.java | 48 ----- .../robot/commands/alignment/LidarAlign.java | 107 ---------- .../robot/constants/BuildConstants.java | 14 +- .../frc4388/robot/constants/Constants.java | 1 + .../utility/compute/ReefPositionHelper.java | 106 ---------- 12 files changed, 13 insertions(+), 497 deletions(-) create mode 100644 src/main/deploy/pathplanner/navgrid.json delete mode 100644 src/main/java/frc4388/robot/commands/alignment/DriveToReef.java delete mode 100644 src/main/java/frc4388/robot/commands/alignment/DriveUntilLiDAR.java delete mode 100644 src/main/java/frc4388/robot/commands/alignment/LidarAlign.java delete mode 100644 src/main/java/frc4388/utility/compute/ReefPositionHelper.java diff --git a/.vscode/settings.json b/.vscode/settings.json index ef24bd2..2b80291 100644 --- a/.vscode/settings.json +++ b/.vscode/settings.json @@ -58,5 +58,6 @@ "edu.wpi.first.math.**.struct.*", ], "java.jdt.ls.vmargs": "-XX:+UseParallelGC -XX:GCTimeRatio=4 -XX:AdaptiveSizePolicyWeight=90 -Dsun.zip.disableMemoryMapping=true -Xmx2G -Xms100m -Xlog:disable", - "wpilib.autoStartRioLog": false + "wpilib.autoStartRioLog": false, + "java.debug.settings.onBuildFailureProceed": true } diff --git a/src/main/deploy/pathplanner/navgrid.json b/src/main/deploy/pathplanner/navgrid.json new file mode 100644 index 0000000..23e0db9 --- /dev/null +++ b/src/main/deploy/pathplanner/navgrid.json @@ -0,0 +1 @@ +{"field_size":{"x":17.548,"y":8.052},"nodeSizeMeters":0.3,"grid":[[true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true],[true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true],[true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true],[true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true],[true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true],[true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true],[true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true],[true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true],[true,true,false,false,false,false,false,false,false,false,false,false,false,false,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,true,true],[true,true,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,true,true],[true,true,false,false,false,false,false,false,false,false,false,true,true,true,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,true,true],[true,true,false,false,false,false,false,false,false,false,true,true,true,true,true,true,true,true,true,true,false,false,false,false,false,false,false,false,true,true,true,false,false,false,false,false,false,false,false,true,true,true,true,true,true,true,true,true,false,false,false,false,false,false,false,false,false,true,true],[true,true,false,false,false,false,false,false,false,false,true,true,true,true,true,true,true,true,true,true,false,false,false,false,false,false,false,true,true,true,true,true,false,false,false,false,false,false,false,true,true,true,true,true,true,true,true,true,false,false,false,false,false,false,false,false,false,true,true],[true,true,false,false,false,false,false,false,false,false,true,true,true,true,true,true,true,true,true,true,false,false,false,false,false,false,false,true,true,true,true,true,false,false,false,false,false,false,false,true,true,true,true,true,true,true,true,true,false,false,false,false,false,false,false,false,false,true,true],[true,true,false,false,false,false,false,false,false,false,true,true,true,true,true,true,true,true,true,true,false,false,false,false,false,false,false,true,true,true,true,true,false,false,false,false,false,false,false,true,true,true,true,true,true,true,true,true,false,false,false,false,false,false,false,false,false,true,true],[true,true,false,false,false,false,false,false,false,false,true,true,true,true,true,true,true,true,true,true,false,false,false,false,false,false,false,false,true,true,true,false,false,false,false,false,false,false,false,true,true,true,true,true,true,true,true,true,false,false,false,false,false,false,false,false,false,true,true],[true,true,false,false,false,false,false,false,false,false,false,true,true,true,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,true,true],[true,true,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,true,true],[true,true,false,false,false,false,false,false,false,false,false,false,false,false,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,true,true],[true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true],[true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true],[true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true],[true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true],[true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true],[true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true],[true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true],[true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true]]} \ No newline at end of file diff --git a/src/main/java/frc4388/robot/RobotContainer.java b/src/main/java/frc4388/robot/RobotContainer.java index 4d81a20..dea29a0 100644 --- a/src/main/java/frc4388/robot/RobotContainer.java +++ b/src/main/java/frc4388/robot/RobotContainer.java @@ -37,12 +37,6 @@ import edu.wpi.first.wpilibj2.command.ConditionalCommand; import frc4388.utility.controller.VirtualController; import frc4388.robot.commands.MoveForTimeCommand; import frc4388.robot.commands.MoveUntilSuply; -import frc4388.robot.commands.alignment.DriveToReef; -import frc4388.robot.commands.alignment.DriveUntilLiDAR; -import frc4388.robot.commands.alignment.LidarAlign; -import frc4388.robot.commands.wait.waitElevatorRefrence; -import frc4388.robot.commands.wait.waitEndefectorRefrence; -import frc4388.robot.commands.wait.waitFeedCoral; import frc4388.robot.commands.wait.waitSupplier; import frc4388.robot.constants.Constants; import frc4388.robot.constants.Constants.AutoConstants; @@ -55,15 +49,12 @@ import com.pathplanner.lib.commands.PathPlannerAuto; // Subsystems import frc4388.robot.subsystems.LED; -import frc4388.robot.subsystems.elevator.Elevator; -import frc4388.robot.subsystems.elevator.Elevator.CoordinationState; import frc4388.robot.subsystems.lidar.LiDAR; import frc4388.robot.subsystems.swerve.SwerveDrive; import frc4388.robot.subsystems.vision.Vision; // Utilites import frc4388.utility.DeferredBlock; import frc4388.utility.compute.TimesNegativeOne; -import frc4388.utility.compute.ReefPositionHelper.Side; /** * This class is where the bulk of the robot should be declared. Since @@ -80,14 +71,9 @@ public class RobotContainer { /* Subsystems */ public final LED m_robotLED = new LED(); public final Vision m_vision = new Vision(m_robotMap.leftCamera, m_robotMap.rightCamera); - public final Elevator m_robotElevator = new Elevator(m_robotMap.elevatorIO, m_robotLED); public final SwerveDrive m_robotSwerveDrive = new SwerveDrive(m_robotMap.swerveDrivetrain, m_vision); // public final SwerveDrive m_robotSwerveDrive = new SwerveDrive(m_robotMap.swerveDrivetrain); - public final LiDAR reefLidar = new LiDAR(m_robotMap.reefLidar, "Reef"); - public final LiDAR reverseLidar = new LiDAR(m_robotMap.reverseLidar, "Reverse"); - - /* Controllers */ private final DeadbandedXboxController m_driverXbox = new DeadbandedXboxController(OIConstants.XBOX_DRIVER_ID); private final DeadbandedXboxController m_operatorXbox = new DeadbandedXboxController(OIConstants.XBOX_OPERATOR_ID); diff --git a/src/main/java/frc4388/robot/RobotMap.java b/src/main/java/frc4388/robot/RobotMap.java index 01cdb68..6d1c2b6 100644 --- a/src/main/java/frc4388/robot/RobotMap.java +++ b/src/main/java/frc4388/robot/RobotMap.java @@ -17,12 +17,9 @@ import com.ctre.phoenix6.hardware.CANcoder; import com.ctre.phoenix6.swerve.SwerveDrivetrain; import edu.wpi.first.math.geometry.Rotation2d; import edu.wpi.first.wpilibj.DigitalInput; -import frc4388.robot.constants.Constants.ElevatorConstants; import frc4388.robot.constants.Constants.LiDARConstants; import frc4388.robot.constants.Constants.SimConstants; import frc4388.robot.constants.Constants.VisionConstants; -import frc4388.robot.subsystems.elevator.ElevatorIO; -import frc4388.robot.subsystems.elevator.ElevatorReal; import frc4388.robot.subsystems.lidar.LiDAR; import frc4388.robot.subsystems.lidar.LidarIO; import frc4388.robot.subsystems.lidar.LidarReal; @@ -59,9 +56,6 @@ public class RobotMap { /* Swreve Drive Subsystem */ public final SwerveIO swerveDrivetrain; - /* Elevator Subsystem */ - public final ElevatorIO elevatorIO; - public RobotMap(SimConstants.Mode mode) { switch (mode) { case REAL: @@ -88,25 +82,8 @@ public class RobotMap { swerveDrivetrain = new SwerveReal(swerveDrivetrainReal); - // Configure elevator - - TalonFX elevator = new TalonFX(ElevatorConstants.ELEVATOR_ID.id); - TalonFX endeffector = new TalonFX(ElevatorConstants.ENDEFFECTOR_ID.id); - - - DigitalInput basinLimitSwitch = new DigitalInput(ElevatorConstants.BASIN_LIMIT_SWITCH); - DigitalInput endeffectorLimitSwitch = new DigitalInput(ElevatorConstants.ENDEFFECTOR_LIMIT_SWITCH); - DigitalInput IRIntakeBeam = new DigitalInput(ElevatorConstants.INTAKE_LIMIT_SWITCH); - - elevatorIO = new ElevatorReal(elevator, endeffector, basinLimitSwitch, endeffectorLimitSwitch, IRIntakeBeam); - - - // Fault FaultPidgeon2.addDevice(swerveDrivetrainReal.getPigeon2(), "Gyro"); - - FaultTalonFX.addDevice(elevator, "Elevator"); - FaultTalonFX.addDevice(endeffector, "Endeffector"); FaultTalonFX.addDevice(swerveDrivetrainReal.getModule(0).getDriveMotor(), "Module 0 Drive"); FaultTalonFX.addDevice(swerveDrivetrainReal.getModule(0).getSteerMotor(), "Module 0 Steer"); @@ -130,7 +107,6 @@ public class RobotMap { reefLidar = new LidarIO() {}; reverseLidar = new LidarIO() {}; swerveDrivetrain = new SwerveIO() {}; - elevatorIO = new ElevatorIO() {}; break; } } diff --git a/src/main/java/frc4388/robot/commands/Autos/neoPlaybackChooser.java b/src/main/java/frc4388/robot/commands/Autos/neoPlaybackChooser.java index a069786..f3d2987 100644 --- a/src/main/java/frc4388/robot/commands/Autos/neoPlaybackChooser.java +++ b/src/main/java/frc4388/robot/commands/Autos/neoPlaybackChooser.java @@ -1,4 +1,4 @@ -package frc4388.robot.commands.autos; +package frc4388.robot.commands.Autos; // package frc4388.robot.commands.Autos; // import java.io.File; diff --git a/src/main/java/frc4388/robot/commands/Swerve/RotateToAngle.java b/src/main/java/frc4388/robot/commands/Swerve/RotateToAngle.java index 50e616c..a3610d1 100644 --- a/src/main/java/frc4388/robot/commands/Swerve/RotateToAngle.java +++ b/src/main/java/frc4388/robot/commands/Swerve/RotateToAngle.java @@ -2,7 +2,7 @@ // Open Source Software; you can modify and/or share it under the terms of // the WPILib BSD license file in the root directory of this project. -package frc4388.robot.commands.swerve; +package frc4388.robot.commands.Swerve; import edu.wpi.first.math.geometry.Translation2d; import frc4388.robot.commands.PID; diff --git a/src/main/java/frc4388/robot/commands/alignment/DriveToReef.java b/src/main/java/frc4388/robot/commands/alignment/DriveToReef.java deleted file mode 100644 index 6a11e85..0000000 --- a/src/main/java/frc4388/robot/commands/alignment/DriveToReef.java +++ /dev/null @@ -1,188 +0,0 @@ -package frc4388.robot.commands.alignment; - -import edu.wpi.first.math.geometry.Pose2d; -import edu.wpi.first.math.geometry.Translation2d; -import edu.wpi.first.math.util.Units; -import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; -import edu.wpi.first.wpilibj2.command.Command; -import frc4388.robot.constants.Constants.AutoConstants; -import frc4388.robot.subsystems.swerve.SwerveDrive; -import frc4388.robot.subsystems.vision.Vision; -import frc4388.utility.compute.ReefPositionHelper; -import frc4388.utility.compute.TimesNegativeOne; -import frc4388.utility.compute.ReefPositionHelper.Side; -import frc4388.utility.structs.Gains; - -public class DriveToReef extends Command { - - - // private Translation2d translation2d= new Translation2d(14.579471999999997,0.24587199999999998); - // private Translation2d translation2d= new Translation2d(16.579342-0.15,5.547867999999999); - - private PID xPID = new PID(AutoConstants.XY_GAINS, 0); - private PID yPID = new PID(AutoConstants.XY_GAINS, 0); - // private PID rotPID = new PID(AutoConstants.ROT_GAINS, 0); - private Pose2d targetpos; - - SwerveDrive swerveDrive; - Vision vision; - double distance; - Side side; - boolean waitVelocityZero; - - /** - * Command to drive robot to position. - * @param SwerveDrive m_robotSwerveDrive - */ - - public DriveToReef(SwerveDrive swerveDrive, Vision vision, double distance, Side side, boolean waitVelocityZero) { - this.swerveDrive = swerveDrive; - this.vision = vision; - this.distance = distance; - this.side = side; - this.waitVelocityZero = waitVelocityZero && false; - addRequirements(swerveDrive); - } - - - public static double tagRelativeXError = -1; - private static void setTagRelativeXError(double val){ - tagRelativeXError = val; - } - - @Override - public void initialize() { - xPID.initialize(); - yPID.initialize(); - this.targetpos = ReefPositionHelper.getNearestPosition( - this.vision.getPose2d(), - side, - Units.inchesToMeters(AutoConstants.X_OFFSET_TRIM.get()), - distance + Units.inchesToMeters(AutoConstants.Y_OFFSET_TRIM.get()) - ); - } - - double xerr; - double yerr; - double roterr; - - double xoutput; - double youtput; - double rotoutput; - - @Override - public void execute() { - xerr = TimesNegativeOne.invert(targetpos.getX() - vision.getPose2d().getX(), TimesNegativeOne.XAxis); - yerr = TimesNegativeOne.invert(targetpos.getY() - vision.getPose2d().getY(), !TimesNegativeOne.YAxis); - // xerr = targetpos.getX() - vision.getPose2d().getX(); - // yerr = targetpos.getX() - vision.getPose2d().getY(); - - // roterr = TimesNegativeOne.invert(targetpos.getRotation().getDegrees() - vision.getPose2d().getRotation().getDegrees(), TimesNegativeOne.isRed); - - roterr = ((targetpos.getRotation().getDegrees() - vision.getPose2d().getRotation().getDegrees())); - - if(roterr > 180){ - roterr -= 360; - }else if(roterr < -180){ - roterr += 360; - } - - // SmartDashboard.putNumber("Rotational PID target", targetpos.getRotation().getDegrees()); - // SmartDashboard.putNumber("Rotational PID position", vision.getPose2d().getRotation().getDegrees()); - // SmartDashboard.putNumber("Rotational PID error", roterr); - - SmartDashboard.putNumber("PID X Error", xerr); - SmartDashboard.putNumber("PID Y Error", yerr); - SmartDashboard.putNumber("PID Rot Error", roterr); - - xoutput = xPID.update(xerr); - youtput = yPID.update(yerr); - // rotoutput = rotPID.update(roterr); - - xoutput *= Math.abs(xerr) < AutoConstants.XY_TOLERANCE ? 0 : 1; - youtput *= Math.abs(yerr) < AutoConstants.XY_TOLERANCE ? 0 : 1; - // rotoutput *= Math.abs(roterr) < AutoConstants.ROT_TOLERANCE ? 0 : 1; - - - - Translation2d leftStick = new Translation2d( - Math.max(Math.min(-youtput, 1), -1), - Math.max(Math.min(-xoutput, 1), -1) - // 0,0 - ); - - // Translation2d rightStick = new Translation2d( - // Math.max(Math.min(rotoutput, 1), -1), - // 0 - // ); - - setTagRelativeXError( - new Translation2d(xerr, yerr).getAngle() - .rotateBy(targetpos.getRotation()) - .getCos()); - - swerveDrive.driveRelativeAngle(leftStick, targetpos.getRotation()); - // swerveDrive.driveWithInputOrientation(leftStick, rightStick, true); - } - - @Override - public final boolean isFinished() { - boolean finished = ( - (Math.abs(xerr) < AutoConstants.XY_TOLERANCE || Math.abs(xoutput) <= AutoConstants.MIN_XY_PID_OUTPUT) && - (Math.abs(yerr) < AutoConstants.XY_TOLERANCE || Math.abs(youtput) <= AutoConstants.MIN_XY_PID_OUTPUT) && - (Math.abs(roterr) < AutoConstants.ROT_TOLERANCE) && - (!waitVelocityZero || swerveDrive.lastOdomSpeed < AutoConstants.VELOCITY_THRESHHOLD) - ); - // System.out.println(finished); - - if(finished) - swerveDrive.softStop(); - - return finished; - // this statement is a boolean in and of itself - } - - - - - - - - - - - - - - - private class PID { - protected Gains gains; - private double output = 0; - - - /** Creates a new PelvicInflammatoryDisease. */ - public PID(Gains gains, double tolerance) { - this.gains = gains; - } - - // Called when the command is initially scheduled. - public final void initialize() { - output = 0; - } - - private double prevError, cumError = 0; - - // Called every time the scheduler runs while the command is scheduled. - public double update(double error) { - cumError += error * .02; // 20 ms - double delta = error - prevError; - - output = error * gains.kP; - output += cumError * gains.kI; - output += delta * gains.kD; - output += gains.kF; - - return output; - } - } -} diff --git a/src/main/java/frc4388/robot/commands/alignment/DriveUntilLiDAR.java b/src/main/java/frc4388/robot/commands/alignment/DriveUntilLiDAR.java deleted file mode 100644 index 648a7d8..0000000 --- a/src/main/java/frc4388/robot/commands/alignment/DriveUntilLiDAR.java +++ /dev/null @@ -1,48 +0,0 @@ -package frc4388.robot.commands.alignment; - -import edu.wpi.first.math.geometry.Translation2d; -import edu.wpi.first.wpilibj2.command.Command; -import frc4388.robot.subsystems.lidar.LiDAR; -import frc4388.robot.subsystems.swerve.SwerveDrive; - -// Command to repeat a joystick movement for a specific time. -public class DriveUntilLiDAR extends Command { - private final SwerveDrive swerveDrive; - private final Translation2d leftStick; - private final Translation2d rightStick; - private final LiDAR m_lidar; - private final double mindistance; - - public DriveUntilLiDAR( - SwerveDrive swerveDrive, - Translation2d leftStick, - Translation2d rightStick, - LiDAR lidar, - double mindistance) { - addRequirements(swerveDrive); - - this.swerveDrive = swerveDrive; - this.leftStick = leftStick; - this.rightStick = rightStick; - this.m_lidar = lidar; - this.mindistance = mindistance; - } - - @Override - public void initialize() { - } - - @Override - public void execute() { - swerveDrive.driveFine(leftStick, rightStick, 0.3); - } - - @Override - public boolean isFinished() { - if (Math.abs(m_lidar.getDistance()) < mindistance) { - swerveDrive.softStop(); - return true; - } - return false; - } -} \ No newline at end of file diff --git a/src/main/java/frc4388/robot/commands/alignment/LidarAlign.java b/src/main/java/frc4388/robot/commands/alignment/LidarAlign.java deleted file mode 100644 index 19efd85..0000000 --- a/src/main/java/frc4388/robot/commands/alignment/LidarAlign.java +++ /dev/null @@ -1,107 +0,0 @@ -// Copyright (c) FIRST and other WPILib contributors. -// Open Source Software; you can modify and/or share it under the terms of -// the WPILib BSD license file in the root directory of this project. - -package frc4388.robot.commands.alignment; - -import edu.wpi.first.math.geometry.Rotation2d; -import edu.wpi.first.math.geometry.Translation2d; -import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; -import edu.wpi.first.wpilibj2.command.Command; -import frc4388.robot.subsystems.lidar.LiDAR; -import frc4388.robot.subsystems.swerve.SwerveDrive; - -/* You should consider using the more terse Command factories API instead https://docs.wpilib.org/en/stable/docs/software/commandbased/organizing-command-based.html#defining-commands */ -public class LidarAlign extends Command { - private SwerveDrive swerveDrive; - private LiDAR lidar; - - private int currentFinderTick; - // private int tickFoundPipe; - private boolean foundReef; - private boolean headedRight; - private double speed; - private int bounces; - private double additionalDistance = 0; - // private final boolean constructedHeadedRight; - - /** Creates a new LidarAlign. */ - public LidarAlign(SwerveDrive swerveDrive, LiDAR lidar) {//, boolean headedRight) { - // Use addRequirements() here to declare subsystem dependencies. - - this.swerveDrive = swerveDrive; - this.lidar = lidar; - - addRequirements(swerveDrive, lidar); - } - - // Called when the command is initially scheduled. - @Override - public void initialize() { - this.currentFinderTick = 0; - this.speed = 0.4; // TODO: find good speed for this - this.foundReef = false; - this.headedRight = (DriveToReef.tagRelativeXError < 0); - this.additionalDistance = 0; - this.bounces = 0; - } - - - // Called every time the scheduler runs while the command is scheduled. - @Override - public void execute() { - if (lidar.withinDistance()) { - swerveDrive.softStop(); - foundReef = true; - return; - } - - if (currentFinderTick > (15 + additionalDistance)) { //arbutrary threshhold for now. - headedRight = !headedRight; - currentFinderTick *= -1; - bounces++; - additionalDistance += 5; - if (bounces == 5) return; - } - double currentHeading = (swerveDrive.getGyroAngle() * 180) / Math.PI; - double relAngle = (Math.round(currentHeading / 60.d) * 60); // Relative driving to the side of the reef - SmartDashboard.putNumber("Rel Angle", relAngle); - SmartDashboard.putNumber("heading", currentHeading); - if (!headedRight) { - swerveDrive.driveRelativeLockedAngle(new Translation2d(0, -speed), Rotation2d.fromDegrees(relAngle)); - } else { - swerveDrive.driveRelativeLockedAngle(new Translation2d(0, speed), Rotation2d.fromDegrees(relAngle)); - } - - currentFinderTick++; - } - - // Called once the command ends or is interrupted. - @Override - public void end(boolean interrupted) { - - } - - // Returns true when the command should end. - @Override - public boolean isFinished() { - if (lidar.getDistance() < 4) { - swerveDrive.stopModules(); - return true; - } else if (foundReef && lidar.withinDistance()) { // spot on - swerveDrive.stopModules(); - return true; - } else if (foundReef && !lidar.withinDistance()) { // over shot - speed = speed / 2; - headedRight = !headedRight; - currentFinderTick = 0; - foundReef = false; - return false; - } else if (bounces >= 3) { - swerveDrive.stopModules(); - return true; - } else { - return false; - } - } -} diff --git a/src/main/java/frc4388/robot/constants/BuildConstants.java b/src/main/java/frc4388/robot/constants/BuildConstants.java index 5aeda6b..9456c7c 100644 --- a/src/main/java/frc4388/robot/constants/BuildConstants.java +++ b/src/main/java/frc4388/robot/constants/BuildConstants.java @@ -5,14 +5,14 @@ package frc4388.robot.constants; */ public final class BuildConstants { public static final String MAVEN_GROUP = ""; - public static final String MAVEN_NAME = "2025RidgeScape"; + public static final String MAVEN_NAME = "Robot-Essentials"; public static final String VERSION = "unspecified"; - public static final int GIT_REVISION = 173; - public static final String GIT_SHA = "2b7bb1224195b9e001e60b895ee04d63abdfc513"; - public static final String GIT_DATE = "2025-07-17 09:15:19 MDT"; - public static final String GIT_BRANCH = "advantagekit"; - public static final String BUILD_DATE = "2025-07-17 11:55:31 MDT"; - public static final long BUILD_UNIX_TIME = 1752774931371L; + public static final int GIT_REVISION = 123; + public static final String GIT_SHA = "56841636f984f1a691134b4f1ae8862353846b54"; + public static final String GIT_DATE = "2025-10-30 16:40:04 MDT"; + public static final String GIT_BRANCH = "calibration-system"; + public static final String BUILD_DATE = "2025-10-30 17:18:25 MDT"; + public static final long BUILD_UNIX_TIME = 1761866305954L; public static final int DIRTY = 1; private BuildConstants(){} diff --git a/src/main/java/frc4388/robot/constants/Constants.java b/src/main/java/frc4388/robot/constants/Constants.java index 4bc9d59..9510475 100644 --- a/src/main/java/frc4388/robot/constants/Constants.java +++ b/src/main/java/frc4388/robot/constants/Constants.java @@ -69,6 +69,7 @@ public final class Constants { public static final double MIN_ROT_PID_OUTPUT = 0.0; public static final double VELOCITY_THRESHHOLD = 0.01; + public static final double STOP_VELOCITY = 2; } diff --git a/src/main/java/frc4388/utility/compute/ReefPositionHelper.java b/src/main/java/frc4388/utility/compute/ReefPositionHelper.java deleted file mode 100644 index 73dc6a5..0000000 --- a/src/main/java/frc4388/utility/compute/ReefPositionHelper.java +++ /dev/null @@ -1,106 +0,0 @@ -package frc4388.utility.compute; - -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 frc4388.robot.constants.Constants.AutoConstants; -import frc4388.robot.constants.Constants.FieldConstants; - -public class ReefPositionHelper { - public enum Side { - LEFT, - RIGHT, - CENTER, - FAR_LEFT - } - - public static final Pose2d[] RED_TAGS = { - FieldConstants.kTagLayout.getTagPose(6).get().toPose2d(), - FieldConstants.kTagLayout.getTagPose(7).get().toPose2d(), - FieldConstants.kTagLayout.getTagPose(8).get().toPose2d(), - FieldConstants.kTagLayout.getTagPose(9).get().toPose2d(), - FieldConstants.kTagLayout.getTagPose(10).get().toPose2d(), - FieldConstants.kTagLayout.getTagPose(11).get().toPose2d() - }; - - public static final Pose2d[] BLUE_TAGS = { - FieldConstants.kTagLayout.getTagPose(17).get().toPose2d(), - FieldConstants.kTagLayout.getTagPose(18).get().toPose2d(), - FieldConstants.kTagLayout.getTagPose(19).get().toPose2d(), - FieldConstants.kTagLayout.getTagPose(20).get().toPose2d(), - FieldConstants.kTagLayout.getTagPose(21).get().toPose2d(), - FieldConstants.kTagLayout.getTagPose(22).get().toPose2d() - }; - - public static double distanceTo(Pose2d first, Pose2d second){ - return Math.sqrt(Math.pow(first.getX() - second.getX(),2) + Math.pow(first.getY() - second.getY(),2)); - } - - - /* - * Function to loop through a list of tag locations to figure out closest one - */ - public static Pose2d getNearestTag(Pose2d[] locations, Pose2d position){ - if(locations.length <= 0) return new Pose2d(); - - Pose2d minPos = locations[0]; - double minDistance = distanceTo(position,minPos); - - for(int i = 1; i < locations.length; i++){ - double dist = distanceTo(locations[i],position); - if(dist < minDistance){ - minPos = locations[i]; - minDistance = dist; - } - } - - System.out.println(minPos.getRotation().getDegrees()); - - return minPos; - } - - /* - * Function to find closest tag location based on side - */ - public static Pose2d getNearestTag(Pose2d position) { - - if(TimesNegativeOne.isRed) - return getNearestTag(RED_TAGS, position); - else - return getNearestTag(BLUE_TAGS, position); - } - - public static Pose2d getNearestPosition(Pose2d position, Side side, double xtrim, double ydistance) { - return offset(getNearestTag(position), - getSide(side) + xtrim, - ydistance); - } - - public static double getSide(Side side){ - switch(side) { - case LEFT: - return -(AutoConstants.X_SCORING_POSITION_OFFSET); - case FAR_LEFT: - return -(AutoConstants.X_SCORING_POSITION_OFFSET+Units.inchesToMeters(8)); - case RIGHT: - return (AutoConstants.X_SCORING_POSITION_OFFSET); - case CENTER: - return 0; - } - assert false; - return 0; - } - - - public static Pose2d offset(Pose2d oldPose, double xoffset, double yoffset){ - Translation2d oldTranslation = oldPose.getTranslation(); - - double rot = oldPose.getRotation().getRadians(); - - return new Pose2d(new Translation2d( - oldTranslation.getX() + Math.cos(rot + Math.PI/2) * xoffset + Math.cos(rot) * yoffset, - oldTranslation.getY() + Math.sin(rot + Math.PI/2) * xoffset + Math.sin(rot) * yoffset - ), oldPose.getRotation().rotateBy(Rotation2d.k180deg)); - } -}