From 2ecca04d7de0e1c3ece66574706f21102003cc24 Mon Sep 17 00:00:00 2001 From: superpenguin612 <74030080+superpenguin612@users.noreply.github.com> Date: Tue, 24 Oct 2023 16:55:48 -0400 Subject: [PATCH] Updates to camera handling and swerve --- .../houndutil/houndauto/AutoManager.java | 12 ---- .../houndlib/AdvantageScopeSerializer.java | 31 ++++++-- .../houndlib/AprilTagPhotonCamera.java | 52 +++++++++++--- .../houndutil/houndlib/TriConsumer.java | 17 +++++ .../houndutil/houndlib/ValueContainer.java | 9 +++ .../houndutil/houndlib/leds/Patterns.java | 71 +++++++++++++++++++ .../houndutil/houndlib/robots/HoundRobot.java | 16 ++--- .../swerve/NEOCoaxialSwerveModule.java | 1 - .../houndlog/LogAnnotationHandler.java | 11 +++ 9 files changed, 182 insertions(+), 38 deletions(-) create mode 100644 src/main/java/com/techhounds/houndutil/houndlib/TriConsumer.java create mode 100644 src/main/java/com/techhounds/houndutil/houndlib/ValueContainer.java create mode 100644 src/main/java/com/techhounds/houndutil/houndlib/leds/Patterns.java diff --git a/src/main/java/com/techhounds/houndutil/houndauto/AutoManager.java b/src/main/java/com/techhounds/houndutil/houndauto/AutoManager.java index 448bb40..97c7958 100644 --- a/src/main/java/com/techhounds/houndutil/houndauto/AutoManager.java +++ b/src/main/java/com/techhounds/houndutil/houndauto/AutoManager.java @@ -45,8 +45,6 @@ public class AutoManager { new AutoSettingChooser() }; - private Runnable updatePoseEstimatorCallback = null; - private Timer timer = new Timer(); /** @@ -247,14 +245,4 @@ public HashMap getEventMap() { } return eventMap; } - - public void setPoseEstimatorCallback(Runnable callback) { - this.updatePoseEstimatorCallback = callback; - } - - public void updatePoseEstimator() { - if (updatePoseEstimatorCallback != null) { - this.updatePoseEstimatorCallback.run(); - } - } } diff --git a/src/main/java/com/techhounds/houndutil/houndlib/AdvantageScopeSerializer.java b/src/main/java/com/techhounds/houndutil/houndlib/AdvantageScopeSerializer.java index 4ebed48..33b9ef6 100644 --- a/src/main/java/com/techhounds/houndutil/houndlib/AdvantageScopeSerializer.java +++ b/src/main/java/com/techhounds/houndutil/houndlib/AdvantageScopeSerializer.java @@ -23,14 +23,27 @@ public static double[] serializeAprilTags(List tags) { return serializePose3ds(tags.stream().map(tag -> tag.pose).collect(Collectors.toList())); } + public static double[] serializePose3d(Pose3d pose) { + double[] poseData = new double[7]; + poseData[0] = pose.getX(); + poseData[1] = pose.getY(); + poseData[2] = pose.getZ(); + Quaternion tagQuat = pose.getRotation().getQuaternion(); + poseData[3] = tagQuat.getW(); + poseData[4] = tagQuat.getX(); + poseData[5] = tagQuat.getY(); + poseData[6] = tagQuat.getZ(); + return poseData; + } + public static double[] serializePose3ds(List poses) { double[] poseData = new double[poses.size() * 7]; for (int i = 0; i < poses.size() * 7; i += 7) { - Pose3d tagPose = poses.get(i / 7); - poseData[i] = tagPose.getX(); - poseData[i + 1] = tagPose.getY(); - poseData[i + 2] = tagPose.getZ(); - Quaternion tagQuat = tagPose.getRotation().getQuaternion(); + Pose3d pose = poses.get(i / 7); + poseData[i] = pose.getX(); + poseData[i + 1] = pose.getY(); + poseData[i + 2] = pose.getZ(); + Quaternion tagQuat = pose.getRotation().getQuaternion(); poseData[i + 3] = tagQuat.getW(); poseData[i + 4] = tagQuat.getX(); poseData[i + 5] = tagQuat.getY(); @@ -39,6 +52,14 @@ public static double[] serializePose3ds(List poses) { return poseData; } + public static double[] serializePose2d(Pose2d pose) { + double[] poseData = new double[3]; + poseData[0] = pose.getX(); + poseData[1] = pose.getY(); + poseData[2] = pose.getRotation().getDegrees(); + return poseData; + } + public static double[] serializePose2ds(List poses) { double[] poseData = new double[poses.size() * 3]; for (int i = 0; i < poses.size() * 3; i += 3) { diff --git a/src/main/java/com/techhounds/houndutil/houndlib/AprilTagPhotonCamera.java b/src/main/java/com/techhounds/houndutil/houndlib/AprilTagPhotonCamera.java index 2b4e8ba..b025e44 100644 --- a/src/main/java/com/techhounds/houndutil/houndlib/AprilTagPhotonCamera.java +++ b/src/main/java/com/techhounds/houndutil/houndlib/AprilTagPhotonCamera.java @@ -4,8 +4,6 @@ import java.util.ArrayList; import java.util.List; import java.util.Optional; -import java.util.function.Supplier; - import org.photonvision.EstimatedRobotPose; import org.photonvision.PhotonCamera; import org.photonvision.PhotonPoseEstimator; @@ -32,14 +30,12 @@ public class AprilTagPhotonCamera { private Pose3d detectedRobotPose = new Pose3d(); private List detectedAprilTags = new ArrayList(); + @Log(name = "Has Pose") private boolean hasPose = false; + @Log(name = "Target Count") + private int targetCount = 0; - @Log(name = "Detected Robot Pose3d") - private Supplier detectedRobotPoseASSupp = () -> AdvantageScopeSerializer - .serializePose3ds(getCurrentlyDetectedAprilTags()); - @Log(name = "Detected AprilTags") - private Supplier detectedAprilTagsASSupp = () -> AdvantageScopeSerializer - .serializePose3ds(getCurrentlyDetectedAprilTags()); + private List targets; public AprilTagPhotonCamera(String name, Transform3d robotToCam) { try { @@ -69,13 +65,17 @@ public Optional getEstimatedGlobalPose( result.targets.removeIf((target) -> target.getPoseAmbiguity() > 0.2); result.targets.removeIf((target) -> target.getFiducialId() > 8); result.targets.removeIf((target) -> target.getBestCameraToTarget().getTranslation().getNorm() > 4); + targetCount = result.targets.size(); + targets = result.targets; - photonPoseEstimator.setReferencePose(prevEstimatedRobotPose); + // photonPoseEstimator.setReferencePose(prevEstimatedRobotPose); Optional estimatedRobotPose = photonPoseEstimator.update(result); - if (estimatedRobotPose.isPresent()) { - detectedAprilTags = getPosesFromTargets(estimatedRobotPose.get().targetsUsed, prevEstimatedRobotPose, + if (result.targets.size() > 0) { + detectedAprilTags = getPosesFromTargets(result.targets, prevEstimatedRobotPose, robotToCam); + } + if (estimatedRobotPose.isPresent()) { detectedRobotPose = estimatedRobotPose.get().estimatedPose; hasPose = true; } else { @@ -87,6 +87,20 @@ public Optional getEstimatedGlobalPose( return estimatedRobotPose; } + @Log(name = "Detected AprilTags") + public double[] getAllDetectedAprilTags() { + List poses = new ArrayList(); + poses.addAll(detectedAprilTags); + return AdvantageScopeSerializer.serializePose3ds(poses); + } + + @Log(name = "Detected Robot Pose") + public double[] getAllDetectedRobotPoses() { + List poses = new ArrayList(); + poses.add(detectedRobotPose); + return AdvantageScopeSerializer.serializePose3ds(poses); + } + private List getPosesFromTargets(List targets, Pose2d robotPose, Transform3d robotToCam) { List poses = new ArrayList(); @@ -110,6 +124,22 @@ public Pose3d getCurrentlyDetectedRobotPose() { return detectedRobotPose; } + // public Optional getAprilTagPoseRelativeToRobot(Pose2d robotPose) { + // PhotonPipelineResult result = photonCamera.getLatestResult(); + // if (result.getBestTarget() != null) { + // Transform3d camToTarget = result.getBestTarget().getBestCameraToTarget(); + // Pose2d pose = new Pose3d(robotPose).plus(camToTarget).toPose2d(); + + // return Optional.of( + // new Pose2d( + // xFilter.calculate(pose.getX()), + // yFilter.calculate(pose.getY()), + // new Rotation2d(thetaFilter.calculate(pose.getRotation().getRadians())))); + // } else { + // return Optional.empty(); + // } + // } + public boolean hasPose() { return hasPose; } diff --git a/src/main/java/com/techhounds/houndutil/houndlib/TriConsumer.java b/src/main/java/com/techhounds/houndutil/houndlib/TriConsumer.java new file mode 100644 index 0000000..04f2659 --- /dev/null +++ b/src/main/java/com/techhounds/houndutil/houndlib/TriConsumer.java @@ -0,0 +1,17 @@ +package com.techhounds.houndutil.houndlib; + +import java.util.Objects; + +@FunctionalInterface +public interface TriConsumer { + void accept(A a, B b, C c); + + default TriConsumer andThen(TriConsumer after) { + Objects.requireNonNull(after); + + return (a, b, c) -> { + accept(a, b, c); + after.accept(a, b, c); + }; + } +} diff --git a/src/main/java/com/techhounds/houndutil/houndlib/ValueContainer.java b/src/main/java/com/techhounds/houndutil/houndlib/ValueContainer.java new file mode 100644 index 0000000..40b0353 --- /dev/null +++ b/src/main/java/com/techhounds/houndutil/houndlib/ValueContainer.java @@ -0,0 +1,9 @@ +package com.techhounds.houndutil.houndlib; + +public class ValueContainer { + public int value; + + public ValueContainer(int value) { + this.value = value; + } +} diff --git a/src/main/java/com/techhounds/houndutil/houndlib/leds/Patterns.java b/src/main/java/com/techhounds/houndutil/houndlib/leds/Patterns.java new file mode 100644 index 0000000..d4e4b42 --- /dev/null +++ b/src/main/java/com/techhounds/houndutil/houndlib/leds/Patterns.java @@ -0,0 +1,71 @@ +package com.techhounds.houndutil.houndlib.leds; + +import com.techhounds.houndutil.houndlib.ValueContainer; + +import edu.wpi.first.wpilibj.AddressableLEDBuffer; +import edu.wpi.first.wpilibj.Timer; +import edu.wpi.first.wpilibj.util.Color; +import edu.wpi.first.wpilibj.util.Color8Bit; + +public class Patterns { + /** + * Changes the contents of the AddressableLEDBuffer to the rainbow state. + */ + + public static Runnable rainbow(double firstIndex, double lastIndex, AddressableLEDBuffer buffer) { + ValueContainer firstPixelHue = new ValueContainer(0); + + return () -> { + for (int i = 0; i < buffer.getLength(); i++) { + // Calculate the hue - hue is easier for rainbows because the color + // shape is a circle so only one value needs to change + final var hue = (firstPixelHue.value + (i * 180 / buffer.getLength())) % 180; + // Set the value + buffer.setHSV(i, hue, 255, 255); // max brightness until adjusted later + + } + // Increase by 3 to make the rainbow "move" + firstPixelHue.value += 3; + // Check bounds + firstPixelHue.value %= 180; + }; + } + + public static Runnable solid(Color color, double firstIndex, double lastIndex, AddressableLEDBuffer buffer) { + return () -> { + for (int i = 0; i < buffer.getLength(); i++) { + buffer.setLED(i, color); + } + }; + } + + public static Runnable flash(Color color, double onDuration, double firstIndex, double lastIndex, + AddressableLEDBuffer buffer) { + return () -> { + + for (int i = 0; i < buffer.getLength(); i++) { + if (Timer.getFPGATimestamp() % (onDuration * 2) > onDuration) { + buffer.setLED(i, color); + } else { + buffer.setLED(i, Color.kBlack); + } + } + }; + } + + /** + * Changes the contents of the AddressableLEDBuffer to the TechHOUNDS state. + */ + private static void techHounds(AddressableLEDBuffer buffer) { + ValueContainer timeStep = new ValueContainer(0); + + for (int i = 0; i < buffer.getLength(); i++) { + if (((i + timeStep.value) / 8) % 2 == 0) { // shifts back and forth every 8 pixels + buffer.setHSV(i, 15, 250, 255); // gold + } else { + buffer.setHSV(i, 109, 240, 255); // blue + } + } + timeStep.value += 1; + } +} diff --git a/src/main/java/com/techhounds/houndutil/houndlib/robots/HoundRobot.java b/src/main/java/com/techhounds/houndutil/houndlib/robots/HoundRobot.java index 7785053..ba8db49 100644 --- a/src/main/java/com/techhounds/houndutil/houndlib/robots/HoundRobot.java +++ b/src/main/java/com/techhounds/houndutil/houndlib/robots/HoundRobot.java @@ -1,22 +1,23 @@ package com.techhounds.houndutil.houndlib.robots; -import edu.wpi.first.wpilibj.RobotBase; import edu.wpi.first.wpilibj.TimedRobot; import edu.wpi.first.wpilibj.livewindow.LiveWindow; import edu.wpi.first.wpilibj2.command.CommandScheduler; -import java.util.function.Supplier; - +import java.util.function.Consumer; import com.techhounds.houndutil.houndauto.AutoManager; +import com.techhounds.houndutil.houndlib.TriConsumer; import com.techhounds.houndutil.houndlog.LoggingManager; public class HoundRobot extends TimedRobot { public HoundRobot() { } - public HoundRobot(Supplier robotContainerSupplier) { - if (robotContainerSupplier != null) - robotContainerSupplier.get(); // calls `new RobotContainer()` basically. + public HoundRobot(Consumer> robotContainerCtor) { + if (robotContainerCtor != null) + robotContainerCtor.accept((callback, periodSeconds, offsetSeconds) -> { + addPeriodic(callback, periodSeconds, offsetSeconds); + }); } @Override @@ -27,9 +28,6 @@ public void robotInit() { // main thread LoggingManager.getInstance().init(); addPeriodic(LoggingManager.getInstance()::run, 0.1, 0.010); - if (RobotBase.isReal()) - addPeriodic(AutoManager.getInstance()::updatePoseEstimator, 0.1, 0.010); - LiveWindow.disableAllTelemetry(); } diff --git a/src/main/java/com/techhounds/houndutil/houndlib/swerve/NEOCoaxialSwerveModule.java b/src/main/java/com/techhounds/houndutil/houndlib/swerve/NEOCoaxialSwerveModule.java index a981e7e..12213a6 100644 --- a/src/main/java/com/techhounds/houndutil/houndlib/swerve/NEOCoaxialSwerveModule.java +++ b/src/main/java/com/techhounds/houndutil/houndlib/swerve/NEOCoaxialSwerveModule.java @@ -310,7 +310,6 @@ public void setState(SwerveModuleState state, boolean openLoop, boolean optimize state.angle.getRadians()); turnMotor.setVoltage(turnPIDOutput); - } else { simDriveEncoderVelocity = state.speedMetersPerSecond; double distancePer20Ms = state.speedMetersPerSecond / 50.0; diff --git a/src/main/java/com/techhounds/houndutil/houndlog/LogAnnotationHandler.java b/src/main/java/com/techhounds/houndutil/houndlog/LogAnnotationHandler.java index a1ba16b..8beb91a 100644 --- a/src/main/java/com/techhounds/houndutil/houndlog/LogAnnotationHandler.java +++ b/src/main/java/com/techhounds/houndutil/houndlog/LogAnnotationHandler.java @@ -13,6 +13,7 @@ import com.ctre.phoenix.sensors.Pigeon2; import com.kauailabs.navx.frc.AHRS; import com.revrobotics.CANSparkMax; +import com.techhounds.houndutil.houndlib.AdvantageScopeSerializer; import com.techhounds.houndutil.houndlog.interfaces.Log; import com.techhounds.houndutil.houndlog.interfaces.LoggedObject; import com.techhounds.houndutil.houndlog.interfaces.SendableLog; @@ -34,6 +35,8 @@ import edu.wpi.first.math.controller.PIDController; import edu.wpi.first.math.controller.ProfiledPIDController; +import edu.wpi.first.math.geometry.Pose2d; +import edu.wpi.first.math.geometry.Pose3d; import edu.wpi.first.util.function.BooleanConsumer; import edu.wpi.first.util.sendable.Sendable; import edu.wpi.first.wpilibj.DigitalInput; @@ -254,6 +257,14 @@ public static Optional getLoggerForValue(Supplier valueSupplier, entry(String.class, () -> new StringLogItem(name, () -> (String) valueSupplier.get(), logAnnotation.logLevel())), + entry(Pose2d.class, + () -> new DoubleArrayLogItem(name, + () -> AdvantageScopeSerializer.serializePose2d((Pose2d) valueSupplier.get()), + logAnnotation.logLevel())), + entry(Pose3d.class, + () -> new DoubleArrayLogItem(name, + () -> AdvantageScopeSerializer.serializePose3d((Pose3d) valueSupplier.get()), + logAnnotation.logLevel())), entry(CANSparkMax.class, () -> new DeviceLogger(name, LogProfileBuilder.buildCANSparkMaxLogItems((CANSparkMax) valueSupplier.get()))),