Skip to content

Commit

Permalink
Updates to camera handling and swerve
Browse files Browse the repository at this point in the history
  • Loading branch information
superpenguin612 committed Oct 24, 2023
1 parent 04cef6c commit 2ecca04
Show file tree
Hide file tree
Showing 9 changed files with 182 additions and 38 deletions.
12 changes: 0 additions & 12 deletions src/main/java/com/techhounds/houndutil/houndauto/AutoManager.java
Original file line number Diff line number Diff line change
Expand Up @@ -45,8 +45,6 @@ public class AutoManager {
new AutoSettingChooser()
};

private Runnable updatePoseEstimatorCallback = null;

private Timer timer = new Timer();

/**
Expand Down Expand Up @@ -247,14 +245,4 @@ public HashMap<String, Command> getEventMap() {
}
return eventMap;
}

public void setPoseEstimatorCallback(Runnable callback) {
this.updatePoseEstimatorCallback = callback;
}

public void updatePoseEstimator() {
if (updatePoseEstimatorCallback != null) {
this.updatePoseEstimatorCallback.run();
}
}
}
Original file line number Diff line number Diff line change
Expand Up @@ -23,14 +23,27 @@ public static double[] serializeAprilTags(List<AprilTag> 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<Pose3d> 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();
Expand All @@ -39,6 +52,14 @@ public static double[] serializePose3ds(List<Pose3d> 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<Pose2d> poses) {
double[] poseData = new double[poses.size() * 3];
for (int i = 0; i < poses.size() * 3; i += 3) {
Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -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;
Expand All @@ -32,14 +30,12 @@ public class AprilTagPhotonCamera {

private Pose3d detectedRobotPose = new Pose3d();
private List<Pose3d> detectedAprilTags = new ArrayList<Pose3d>();
@Log(name = "Has Pose")
private boolean hasPose = false;
@Log(name = "Target Count")
private int targetCount = 0;

@Log(name = "Detected Robot Pose3d")
private Supplier<double[]> detectedRobotPoseASSupp = () -> AdvantageScopeSerializer
.serializePose3ds(getCurrentlyDetectedAprilTags());
@Log(name = "Detected AprilTags")
private Supplier<double[]> detectedAprilTagsASSupp = () -> AdvantageScopeSerializer
.serializePose3ds(getCurrentlyDetectedAprilTags());
private List<PhotonTrackedTarget> targets;

public AprilTagPhotonCamera(String name, Transform3d robotToCam) {
try {
Expand Down Expand Up @@ -69,13 +65,17 @@ public Optional<EstimatedRobotPose> 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> 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 {
Expand All @@ -87,6 +87,20 @@ public Optional<EstimatedRobotPose> getEstimatedGlobalPose(
return estimatedRobotPose;
}

@Log(name = "Detected AprilTags")
public double[] getAllDetectedAprilTags() {
List<Pose3d> poses = new ArrayList<Pose3d>();
poses.addAll(detectedAprilTags);
return AdvantageScopeSerializer.serializePose3ds(poses);
}

@Log(name = "Detected Robot Pose")
public double[] getAllDetectedRobotPoses() {
List<Pose3d> poses = new ArrayList<Pose3d>();
poses.add(detectedRobotPose);
return AdvantageScopeSerializer.serializePose3ds(poses);
}

private List<Pose3d> getPosesFromTargets(List<PhotonTrackedTarget> targets, Pose2d robotPose,
Transform3d robotToCam) {
List<Pose3d> poses = new ArrayList<Pose3d>();
Expand All @@ -110,6 +124,22 @@ public Pose3d getCurrentlyDetectedRobotPose() {
return detectedRobotPose;
}

// public Optional<Pose2d> 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;
}
Expand Down
17 changes: 17 additions & 0 deletions src/main/java/com/techhounds/houndutil/houndlib/TriConsumer.java
Original file line number Diff line number Diff line change
@@ -0,0 +1,17 @@
package com.techhounds.houndutil.houndlib;

import java.util.Objects;

@FunctionalInterface
public interface TriConsumer<A, B, C> {
void accept(A a, B b, C c);

default TriConsumer<A, B, C> andThen(TriConsumer<? super A, ? super B, ? super C> after) {
Objects.requireNonNull(after);

return (a, b, c) -> {
accept(a, b, c);
after.accept(a, b, c);
};
}
}
Original file line number Diff line number Diff line change
@@ -0,0 +1,9 @@
package com.techhounds.houndutil.houndlib;

public class ValueContainer {
public int value;

public ValueContainer(int value) {
this.value = value;
}
}
71 changes: 71 additions & 0 deletions src/main/java/com/techhounds/houndutil/houndlib/leds/Patterns.java
Original file line number Diff line number Diff line change
@@ -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;
}
}
Original file line number Diff line number Diff line change
@@ -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<Object> robotContainerSupplier) {
if (robotContainerSupplier != null)
robotContainerSupplier.get(); // calls `new RobotContainer()` basically.
public HoundRobot(Consumer<TriConsumer<Runnable, Double, Double>> robotContainerCtor) {
if (robotContainerCtor != null)
robotContainerCtor.accept((callback, periodSeconds, offsetSeconds) -> {
addPeriodic(callback, periodSeconds, offsetSeconds);
});
}

@Override
Expand All @@ -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();
}

Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -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;
Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -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;
Expand All @@ -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;
Expand Down Expand Up @@ -254,6 +257,14 @@ public static Optional<Logger> getLoggerForValue(Supplier<Object> 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()))),
Expand Down

0 comments on commit 2ecca04

Please sign in to comment.