diff --git a/src/main/java/frc/robot/Robot.java b/src/main/java/frc/robot/Robot.java index 22c65330..171664be 100644 --- a/src/main/java/frc/robot/Robot.java +++ b/src/main/java/frc/robot/Robot.java @@ -60,6 +60,8 @@ import frc.robot.subsystems.vision.VisionIOSim; import frc.robot.subsystems.wrist.*; import frc.robot.utils.CommandXboxControllerSubsystem; +import frc.robot.utils.LoggedTunableBoolean; +import frc.robot.utils.LoggedTunableNumber; import frc.robot.utils.Tracer; import frc.robot.utils.autoaim.AutoAim; import frc.robot.utils.autoaim.AutoAimTargets; @@ -332,6 +334,16 @@ public static enum AlgaeScoreTarget { // Main benefit to that is reducing startup time, which idt we care about too much private final LoggedDashboardChooser autoChooser = new LoggedDashboardChooser<>("Autos"); + public static final boolean TUNING_MODE = true; + private final LoggedTunableNumber elevatorManualTarget = + new LoggedTunableNumber("Manual Target/Elevator", 0.0); + private final LoggedTunableNumber shoulderManualTarget = + new LoggedTunableNumber("Manual Target/Shoulder", 0.0); + private final LoggedTunableNumber wristManualTarget = + new LoggedTunableNumber("Manual Target/Wrist", 0.0); + private final LoggedTunableBoolean manualTarget = + new LoggedTunableBoolean("Manual Target/Enable", false); + // Mechanisms private final LoggedMechanism2d elevatorMech2d = new LoggedMechanism2d(3.0, Units.feetToMeters(4.0)); @@ -546,6 +558,16 @@ public Robot() { .rightTrigger() .onTrue(Commands.runOnce(() -> algaeScoreTarget = AlgaeScoreTarget.PROCESSOR)); + new Trigger(() -> manualTarget.get()) + // .and(() -> superstructure.getState() == SuperState.IDLE) + .onTrue(Commands.print("Manual Control")) + .whileTrue( + Commands.parallel( + Commands.print("Manual Control Activated"), + elevator.setExtension(elevatorManualTarget::get), + shoulder.setTargetAngle(() -> Rotation2d.fromDegrees(shoulderManualTarget.get())), + wrist.setTargetAngle(() -> Rotation2d.fromDegrees(wristManualTarget.get())))); + // Log locations of all autoaim targets Logger.recordOutput( "AutoAim/Targets", @@ -593,6 +615,7 @@ public void robotPeriodic() { "MapleSim/Pose", swerveDriveSimulation.get().getSimulatedDriveTrainPose()); } + Logger.recordOutput("Manual Override", manualTarget.get()); Logger.recordOutput("Targets/Reef Target", currentTarget); Logger.recordOutput("Targets/Algae Intake Target", algaeIntakeTarget); Logger.recordOutput("Targets/Algae Score Target", algaeScoreTarget); diff --git a/src/main/java/frc/robot/subsystems/Superstructure.java b/src/main/java/frc/robot/subsystems/Superstructure.java index 637d9318..ab848acf 100644 --- a/src/main/java/frc/robot/subsystems/Superstructure.java +++ b/src/main/java/frc/robot/subsystems/Superstructure.java @@ -1,8 +1,12 @@ package frc.robot.subsystems; +import com.google.common.collect.Streams; 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.Translation2d; import edu.wpi.first.math.kinematics.ChassisSpeeds; +import edu.wpi.first.math.util.Units; import edu.wpi.first.wpilibj.Timer; import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.Command.InterruptionBehavior; @@ -20,6 +24,7 @@ import java.util.HashMap; import java.util.Map; import java.util.function.Supplier; +import java.util.stream.Stream; import org.littletonrobotics.junction.AutoLogOutput; import org.littletonrobotics.junction.Logger; @@ -152,6 +157,11 @@ public Superstructure( /** This file is not a subsystem, so this MUST be called manually. */ public void periodic() { Logger.recordOutput("Superstructure/Superstructure State", state); + if (Robot.isSimulation()) { + Logger.recordOutput("Superstructure/Check Collisions", checkCollisions()); + Logger.recordOutput( + "Superstructure/Collision Statics", getStaticColliders(elevator.getExtensionMeters())); + } } private void configureStateTransitionCommands() { @@ -579,4 +589,110 @@ private Command forceState(SuperState nextState) { this.state = nextState; }); } + + public boolean checkCollisions() { + return checkCollisions(elevator.getExtensionMeters(), shoulder.getAngle(), wrist.getAngle()); + } + + public static boolean checkCollisions( + double elevatorExtension, Rotation2d shoulderAngle, Rotation2d wristAngle) { + // Check if arm collides with hp, cross bar, bumper + final var armRect = + new Rectangle2d( + new Pose2d( // arm + new Translation2d( + ShoulderSubsystem.X_OFFSET_METERS + + shoulderAngle.getCos() * ShoulderSubsystem.ARM_LENGTH_METERS / 2.0, + ShoulderSubsystem.Z_OFFSET_METERS + + elevatorExtension + + shoulderAngle.getSin() * ShoulderSubsystem.ARM_LENGTH_METERS / 2.0), + new Rotation2d( + (Math.PI / 2) + Units.degreesToRadians(2.794042) + shoulderAngle.getRadians())), + Units.inchesToMeters(2.787574), + ShoulderSubsystem.ARM_LENGTH_METERS); + // Check if back of manip collides w .. + final var manipRect = + new Rectangle2d( + new Pose2d( // Manipulator + new Translation2d( + ShoulderSubsystem.X_OFFSET_METERS + + shoulderAngle.getCos() * ShoulderSubsystem.ARM_LENGTH_METERS, + elevatorExtension + + ShoulderSubsystem.Z_OFFSET_METERS + + shoulderAngle.getSin() * ShoulderSubsystem.ARM_LENGTH_METERS) + .plus( + new Translation2d(0.0, Units.inchesToMeters(8.0 / 2)).rotateBy(wristAngle)), + new Rotation2d(wristAngle.getRadians())), + Units.inchesToMeters(1.5), + Units.inchesToMeters(8.0)); + // Check if centering of manip collides w .. + final var centeringRect = + new Rectangle2d( + manipRect.getCenter(), Units.inchesToMeters(3.0 * 2), Units.inchesToMeters(1.0)); + // Check if tusk collides w .. + final var tuskRect = + new Rectangle2d( + new Pose2d( // Manipulator + new Translation2d( + ShoulderSubsystem.X_OFFSET_METERS + + shoulderAngle.getCos() * ShoulderSubsystem.ARM_LENGTH_METERS, + elevatorExtension + + ShoulderSubsystem.Z_OFFSET_METERS + + shoulderAngle.getSin() * ShoulderSubsystem.ARM_LENGTH_METERS) + .plus( + new Translation2d(Units.inchesToMeters(15.0), Units.inchesToMeters(-3.5)) + .rotateBy(wristAngle)), + new Rotation2d(wristAngle.getRadians())), + Units.inchesToMeters(3.0), + Units.inchesToMeters(3.0)); + Logger.recordOutput( + "Collision Boxes", + Streams.concat( + Stream.of(getPoints(armRect)), + Stream.of(getPoints(manipRect)), + Stream.of(getPoints(centeringRect)), + Stream.of(getPoints(tuskRect))) + .toArray(Translation2d[]::new)); + return checkCollision(armRect, elevatorExtension) + || checkCollision(manipRect, elevatorExtension) + || checkCollision(centeringRect, elevatorExtension) + || checkCollision(tuskRect, elevatorExtension); + } + + private static Translation2d[] getPoints(Rectangle2d rect) { + return new Translation2d[] { + new Translation2d(rect.getXWidth() / 2.0, rect.getYWidth() / 2.0) + .rotateBy(rect.getRotation()) + .plus(rect.getCenter().getTranslation()), + new Translation2d(-rect.getXWidth() / 2.0, rect.getYWidth() / 2.0) + .rotateBy(rect.getRotation()) + .plus(rect.getCenter().getTranslation()), + new Translation2d(rect.getXWidth() / 2.0, -rect.getYWidth() / 2.0) + .rotateBy(rect.getRotation()) + .plus(rect.getCenter().getTranslation()), + new Translation2d(-rect.getXWidth() / 2.0, -rect.getYWidth() / 2.0) + .rotateBy(rect.getRotation()) + .plus(rect.getCenter().getTranslation()) + }; + } + + private static Translation2d[] getStaticColliders(double elevatorExtension) { + return new Translation2d[] { + // funnel + new Translation2d(Units.inchesToMeters(2.173), Units.inchesToMeters(23.368176)), + // crossbar + new Translation2d(Units.inchesToMeters(0.7), Units.inchesToMeters(37.683)), + new Translation2d( + Units.inchesToMeters(3.000), Units.inchesToMeters(43.557883) + elevatorExtension / 2.0), + // bumpers + new Translation2d(Units.inchesToMeters(14.850000), Units.inchesToMeters(5.698368)), + new Translation2d(Units.inchesToMeters(14.85 + 3.0), Units.inchesToMeters(5.698368)) + }; + } + + /** Checks if a rectangle is colliding with the funnel, crossbar, or bumpers */ + private static boolean checkCollision(Rectangle2d rect, double elevatorExtension) { + return Stream.of(getStaticColliders(elevatorExtension)) + .anyMatch((c) -> rect.getDistance(c) <= Units.inchesToMeters(0.0)); + } } diff --git a/src/main/java/frc/robot/subsystems/elevator/ElevatorSubsystem.java b/src/main/java/frc/robot/subsystems/elevator/ElevatorSubsystem.java index d161282f..0abf849e 100644 --- a/src/main/java/frc/robot/subsystems/elevator/ElevatorSubsystem.java +++ b/src/main/java/frc/robot/subsystems/elevator/ElevatorSubsystem.java @@ -36,7 +36,7 @@ public class ElevatorSubsystem extends SubsystemBase { public static final double INTAKE_ALGAE_LOW_EXTENSION = Units.inchesToMeters(19.75); public static final double INTAKE_ALGAE_HIGH_EXTENSION = Units.inchesToMeters(35.25); - public static final double ALGAE_NET_EXTENSION = Units.inchesToMeters(50.0); + public static final double ALGAE_NET_EXTENSION = Units.inchesToMeters(63.5); public static final double ALGAE_PROCESSOR_EXTENSION = Units.inchesToMeters(10.0); public static final double HP_EXTENSION_METERS = Units.inchesToMeters(0.0); diff --git a/src/main/java/frc/robot/subsystems/shoulder/ShoulderSubsystem.java b/src/main/java/frc/robot/subsystems/shoulder/ShoulderSubsystem.java index 92f2af31..8770cbdd 100644 --- a/src/main/java/frc/robot/subsystems/shoulder/ShoulderSubsystem.java +++ b/src/main/java/frc/robot/subsystems/shoulder/ShoulderSubsystem.java @@ -27,7 +27,7 @@ public class ShoulderSubsystem extends SubsystemBase { public static final Rotation2d SHOULDER_INTAKE_ALGAE_STACK_POS = Rotation2d.fromDegrees(35); public static final Rotation2d SHOULDER_INTAKE_ALGAE_REEF_POS = Rotation2d.fromDegrees(68.5); public static final Rotation2d SHOULDER_SCORE_POS = Rotation2d.fromDegrees(75); - public static final Rotation2d SHOULDER_SHOOT_NET_POS = Rotation2d.fromDegrees(90); + public static final Rotation2d SHOULDER_SHOOT_NET_POS = Rotation2d.fromDegrees(85); // TODO: SET TO CORRECT POS public static final Rotation2d SHOULDER_SCORE_PROCESSOR_POS = Rotation2d.fromDegrees(0.0); diff --git a/src/main/java/frc/robot/subsystems/wrist/WristSubsystem.java b/src/main/java/frc/robot/subsystems/wrist/WristSubsystem.java index 7a9d1855..d5638633 100644 --- a/src/main/java/frc/robot/subsystems/wrist/WristSubsystem.java +++ b/src/main/java/frc/robot/subsystems/wrist/WristSubsystem.java @@ -26,7 +26,7 @@ public class WristSubsystem extends SubsystemBase { public static final Rotation2d WRIST_SCORE_L3_POS = Rotation2d.fromDegrees(-35); public static final Rotation2d WRIST_SCORE_L4_POS = Rotation2d.fromDegrees(-40); public static final Rotation2d WRIST_INTAKE_ALGAE_REEF_POS = Rotation2d.fromDegrees(-33.5); - public static final Rotation2d WRIST_SHOOT_NET_POS = Rotation2d.fromDegrees(75); + public static final Rotation2d WRIST_SHOOT_NET_POS = Rotation2d.fromDegrees(90); public static final Rotation2d WRIST_SCORE_PROCESSOR_POS = Rotation2d.fromDegrees(-45.0); private final WristIO io; diff --git a/src/main/java/frc/robot/utils/LoggedTunableBoolean.java b/src/main/java/frc/robot/utils/LoggedTunableBoolean.java new file mode 100644 index 00000000..876b0720 --- /dev/null +++ b/src/main/java/frc/robot/utils/LoggedTunableBoolean.java @@ -0,0 +1,124 @@ +// Copyright (c) 2025 FRC 6328 +// http://github.com/Mechanical-Advantage +// +// Use of this source code is governed by an MIT-style +// license that can be found in the LICENSE file at +// the root directory of this project. + +package frc.robot.utils; + +import frc.robot.Robot; +import java.util.Arrays; +import java.util.HashMap; +import java.util.Map; +import java.util.function.BooleanSupplier; +import java.util.function.Consumer; +import org.littletonrobotics.junction.networktables.LoggedNetworkBoolean; + +/** + * Class for a tunable number. Gets value from dashboard in tuning mode, returns default if not or + * value not in dashboard. + */ +public class LoggedTunableBoolean implements BooleanSupplier { + private static final String tableKey = "/Tuning"; + + private final String key; + private boolean hasDefault = false; + private boolean defaultValue; + private LoggedNetworkBoolean dashboardBoolean; + private Map lastHasChangedValues = new HashMap<>(); + + /** + * Create a new LoggedTunableNumber + * + * @param dashboardKey Key on dashboard + */ + public LoggedTunableBoolean(String dashboardKey) { + this.key = tableKey + "/" + dashboardKey; + } + + /** + * Create a new LoggedTunableNumber with the default value + * + * @param dashboardKey Key on dashboard + * @param defaultValue Default value + */ + public LoggedTunableBoolean(String dashboardKey, boolean defaultValue) { + this(dashboardKey); + initDefault(defaultValue); + } + + /** + * Set the default value of the number. The default value can only be set once. + * + * @param defaultValue The default value + */ + public void initDefault(boolean defaultValue) { + if (!hasDefault) { + hasDefault = true; + this.defaultValue = defaultValue; + if (Robot.TUNING_MODE) { + dashboardBoolean = new LoggedNetworkBoolean(key, defaultValue); + } + } + } + + /** + * Get the current value, from dashboard if available and in tuning mode. + * + * @return The current value + */ + public boolean get() { + if (!hasDefault) { + return false; + } else { + return Robot.TUNING_MODE ? dashboardBoolean.get() : defaultValue; + } + } + + /** + * Checks whether the number has changed since our last check + * + * @param id Unique identifier for the caller to avoid conflicts when shared between multiple + * objects. Recommended approach is to pass the result of "hashCode()" + * @return True if the number has changed since the last time this method was called, false + * otherwise. + */ + public boolean hasChanged(int id) { + boolean currentValue = get(); + Boolean lastValue = lastHasChangedValues.get(id); + if (lastValue == null || currentValue != lastValue) { + lastHasChangedValues.put(id, currentValue); + return true; + } + + return false; + } + + /** + * Runs action if any of the tunableNumbers have changed + * + * @param id Unique identifier for the caller to avoid conflicts when shared between multiple * + * objects. Recommended approach is to pass the result of "hashCode()" + * @param action Callback to run when any of the tunable numbers have changed. Access tunable + * numbers in order inputted in method + * @param tunableBooleans All tunable numbers to check + */ + public static void ifChanged( + int id, Consumer action, LoggedTunableBoolean... tunableBooleans) { + if (Arrays.stream(tunableBooleans).anyMatch(tunableNumber -> tunableNumber.hasChanged(id))) { + action.accept( + Arrays.stream(tunableBooleans).map(LoggedTunableBoolean::get).toArray(Boolean[]::new)); + } + } + + /** Runs action if any of the tunableNumbers have changed */ + public static void ifChanged(int id, Runnable action, LoggedTunableBoolean... tunableNumbers) { + ifChanged(id, values -> action.run(), tunableNumbers); + } + + @Override + public boolean getAsBoolean() { + return get(); + } +} diff --git a/src/main/java/frc/robot/utils/LoggedTunableNumber.java b/src/main/java/frc/robot/utils/LoggedTunableNumber.java new file mode 100644 index 00000000..3409454d --- /dev/null +++ b/src/main/java/frc/robot/utils/LoggedTunableNumber.java @@ -0,0 +1,123 @@ +// Copyright (c) 2025 FRC 6328 +// http://github.com/Mechanical-Advantage +// +// Use of this source code is governed by an MIT-style +// license that can be found in the LICENSE file at +// the root directory of this project. + +package frc.robot.utils; + +import frc.robot.Robot; +import java.util.Arrays; +import java.util.HashMap; +import java.util.Map; +import java.util.function.Consumer; +import java.util.function.DoubleSupplier; +import org.littletonrobotics.junction.networktables.LoggedNetworkNumber; + +/** + * Class for a tunable number. Gets value from dashboard in tuning mode, returns default if not or + * value not in dashboard. + */ +public class LoggedTunableNumber implements DoubleSupplier { + private static final String tableKey = "/Tuning"; + + private final String key; + private boolean hasDefault = false; + private double defaultValue; + private LoggedNetworkNumber dashboardNumber; + private Map lastHasChangedValues = new HashMap<>(); + + /** + * Create a new LoggedTunableNumber + * + * @param dashboardKey Key on dashboard + */ + public LoggedTunableNumber(String dashboardKey) { + this.key = tableKey + "/" + dashboardKey; + } + + /** + * Create a new LoggedTunableNumber with the default value + * + * @param dashboardKey Key on dashboard + * @param defaultValue Default value + */ + public LoggedTunableNumber(String dashboardKey, double defaultValue) { + this(dashboardKey); + initDefault(defaultValue); + } + + /** + * Set the default value of the number. The default value can only be set once. + * + * @param defaultValue The default value + */ + public void initDefault(double defaultValue) { + if (!hasDefault) { + hasDefault = true; + this.defaultValue = defaultValue; + if (Robot.TUNING_MODE) { + dashboardNumber = new LoggedNetworkNumber(key, defaultValue); + } + } + } + + /** + * Get the current value, from dashboard if available and in tuning mode. + * + * @return The current value + */ + public double get() { + if (!hasDefault) { + return 0.0; + } else { + return Robot.TUNING_MODE ? dashboardNumber.get() : defaultValue; + } + } + + /** + * Checks whether the number has changed since our last check + * + * @param id Unique identifier for the caller to avoid conflicts when shared between multiple + * objects. Recommended approach is to pass the result of "hashCode()" + * @return True if the number has changed since the last time this method was called, false + * otherwise. + */ + public boolean hasChanged(int id) { + double currentValue = get(); + Double lastValue = lastHasChangedValues.get(id); + if (lastValue == null || currentValue != lastValue) { + lastHasChangedValues.put(id, currentValue); + return true; + } + + return false; + } + + /** + * Runs action if any of the tunableNumbers have changed + * + * @param id Unique identifier for the caller to avoid conflicts when shared between multiple * + * objects. Recommended approach is to pass the result of "hashCode()" + * @param action Callback to run when any of the tunable numbers have changed. Access tunable + * numbers in order inputted in method + * @param tunableNumbers All tunable numbers to check + */ + public static void ifChanged( + int id, Consumer action, LoggedTunableNumber... tunableNumbers) { + if (Arrays.stream(tunableNumbers).anyMatch(tunableNumber -> tunableNumber.hasChanged(id))) { + action.accept(Arrays.stream(tunableNumbers).mapToDouble(LoggedTunableNumber::get).toArray()); + } + } + + /** Runs action if any of the tunableNumbers have changed */ + public static void ifChanged(int id, Runnable action, LoggedTunableNumber... tunableNumbers) { + ifChanged(id, values -> action.run(), tunableNumbers); + } + + @Override + public double getAsDouble() { + return get(); + } +}