Skip to content

Commit

Permalink
Add AprilTag demo autos (#266)
Browse files Browse the repository at this point in the history 
* Add demo autos

* formatting :(

* Fix

---------

Co-authored-by: Jonah <[email protected]>
  • Loading branch information
@suryatho @jwbonner
suryatho and jwbonner authored Jun 13, 2024
1 parent 7bbcb66 commit fc52720
Show file tree
Hide file tree
Showing 8 changed files with 261 additions and 39 deletions.
8 changes: 8 additions & 0 deletions src/main/java/org/littletonrobotics/frc2024/Robot.java
View file
Original file line number Diff line number Diff line change
Expand Up @@ -227,6 +227,14 @@ public void robotPeriodic() {
Logger.recordOutput(
"RobotState/SuperPoopParameters/FlywheelsRightSpeed",
superPoopParameters.flywheelSpeeds().rightSpeed());
var demoParameters = RobotState.getInstance().getDemoTagParameters();
demoParameters.ifPresent(
parameters -> {
Logger.recordOutput("RobotState/DemoParameters/TargetPose", parameters.targetPose());
Logger.recordOutput(
"RobotState/DemoParameters/TargetHeading", parameters.targetHeading());
Logger.recordOutput("RobotState/DemoParameters/ArmAngle", parameters.armAngle());
});

// Robot container periodic methods
robotContainer.updateDemoControls();
Expand Down
12 changes: 8 additions & 4 deletions src/main/java/org/littletonrobotics/frc2024/RobotContainer.java
View file
Original file line number Diff line number Diff line change
Expand Up @@ -33,10 +33,7 @@
import org.littletonrobotics.frc2024.AutoSelector.AutoQuestion;
import org.littletonrobotics.frc2024.AutoSelector.AutoQuestionResponse;
import org.littletonrobotics.frc2024.FieldConstants.AprilTagLayoutType;
import org.littletonrobotics.frc2024.commands.ClimbingCommands;
import org.littletonrobotics.frc2024.commands.FeedForwardCharacterization;
import org.littletonrobotics.frc2024.commands.StaticCharacterization;
import org.littletonrobotics.frc2024.commands.WheelRadiusCharacterization;
import org.littletonrobotics.frc2024.commands.*;
import org.littletonrobotics.frc2024.commands.auto.AutoBuilder;
import org.littletonrobotics.frc2024.subsystems.apriltagvision.AprilTagVision;
import org.littletonrobotics.frc2024.subsystems.apriltagvision.AprilTagVisionIO;
Expand Down Expand Up @@ -415,6 +412,13 @@ private void configureAutos() {
AutoQuestionResponse.SIX_SECONDS,
AutoQuestionResponse.LAST_SECOND))),
autoBuilder.davisInspirationalAuto());
DemoAutos demoAutos = new DemoAutos(drive, superstructure, autoSelector::getResponses);
autoSelector.addRoutine(
"Davis Demo Auto",
List.of(
new AutoQuestion(
"Follow Tag?", List.of(AutoQuestionResponse.YES, AutoQuestionResponse.NO))),
demoAutos.davisDemoAuto());

// Set up feedforward characterization
autoSelector.addRoutine(
Expand Down
49 changes: 49 additions & 0 deletions src/main/java/org/littletonrobotics/frc2024/RobotState.java
View file
Original file line number Diff line number Diff line change
Expand Up @@ -18,11 +18,13 @@
import edu.wpi.first.math.util.Units;
import edu.wpi.first.wpilibj.DriverStation;
import java.util.NoSuchElementException;
import java.util.Optional;
import java.util.function.BooleanSupplier;
import lombok.Getter;
import lombok.Setter;
import lombok.experimental.ExtensionMethod;
import org.littletonrobotics.frc2024.subsystems.drive.DriveConstants;
import org.littletonrobotics.frc2024.subsystems.superstructure.arm.ArmConstants;
import org.littletonrobotics.frc2024.util.AllianceFlipUtil;
import org.littletonrobotics.frc2024.util.GeomUtil;
import org.littletonrobotics.frc2024.util.LoggedTunableNumber;
Expand Down Expand Up @@ -52,6 +54,9 @@ public record AimingParameters(
double effectiveDistance,
FlywheelSpeeds flywheelSpeeds) {}

public record DemoFollowParameters(
Pose2d targetPose, Rotation2d targetHeading, Rotation2d armAngle) {}

public record DemoShotParameters(Rotation2d armAngle, FlywheelSpeeds flywheelSpeeds) {}

private static final LoggedTunableNumber autoLookahead =
Expand All @@ -62,6 +67,7 @@ public record DemoShotParameters(Rotation2d armAngle, FlywheelSpeeds flywheelSpe
new LoggedTunableNumber("RobotState/SuperPoopLookahead", 0.1);
private static final LoggedTunableNumber closeShootingZoneFeet =
new LoggedTunableNumber("RobotState/CloseShootingZoneFeet", 10.0);

private static final double poseBufferSizeSeconds = 2.0;

private static final double armAngleCoefficient = 57.254371165197;
Expand Down Expand Up @@ -130,6 +136,9 @@ public static RobotState getInstance() {
private AimingParameters latestParameters = null;

private AimingParameters latestSuperPoopParameters = null;
// Demo parameters
private Pose3d demoTagPose = null;
private DemoFollowParameters latestDemoParamters = null;

@Setter @Getter
private DemoShotParameters demoShotParameters =
Expand Down Expand Up @@ -326,6 +335,46 @@ public AimingParameters getSuperPoopAimingParameters() {
return latestSuperPoopParameters;
}

public void setDemoTagPose(Pose3d demoTagPose) {
this.demoTagPose = demoTagPose;
latestDemoParamters = null;
}

private static final LoggedTunableNumber demoTargetDistance =
new LoggedTunableNumber("RobotState/DemoTargetDistance", 2.0);

public Optional<DemoFollowParameters> getDemoTagParameters() {
if (latestDemoParamters != null) {
// Use cached demo parameters.
return Optional.of(latestDemoParamters);
}
// Return empty optional if no demo tag pose.
if (demoTagPose == null) return Optional.empty();

// Calculate target pose.
Pose2d targetPose =
demoTagPose
.toPose2d()
.transformBy(
new Transform2d(
new Translation2d(demoTargetDistance.get(), 0.0), new Rotation2d(Math.PI)));

// Calculate heading without movement.
Translation2d demoTagFixed = demoTagPose.getTranslation().toTranslation2d();
Translation2d robotToDemoTagFixed = demoTagFixed.minus(getEstimatedPose().getTranslation());
Rotation2d targetHeading = robotToDemoTagFixed.getAngle();

// Calculate arm angle.
double z = demoTagPose.getZ();
Rotation2d armAngle =
new Rotation2d(
robotToDemoTagFixed.getNorm() - ArmConstants.armOrigin.getX(),
z - ArmConstants.armOrigin.getY());

latestDemoParamters = new DemoFollowParameters(targetPose, targetHeading, armAngle);
return Optional.of(latestDemoParamters);
}

public ModuleLimits getModuleLimits() {
return flywheelAccelerating && !DriverStation.isAutonomousEnabled()
? DriveConstants.moduleLimitsFlywheelSpinup
Expand Down
69 changes: 69 additions & 0 deletions src/main/java/org/littletonrobotics/frc2024/commands/DemoAutos.java
View file
Original file line number Diff line number Diff line change
@@ -0,0 +1,69 @@
// Copyright (c) 2024 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 org.littletonrobotics.frc2024.commands;

import edu.wpi.first.math.geometry.Translation2d;
import edu.wpi.first.wpilibj2.command.Command;
import edu.wpi.first.wpilibj2.command.Commands;
import java.util.List;
import java.util.function.Supplier;
import lombok.RequiredArgsConstructor;
import org.littletonrobotics.frc2024.AutoSelector;
import org.littletonrobotics.frc2024.RobotState;
import org.littletonrobotics.frc2024.subsystems.drive.Drive;
import org.littletonrobotics.frc2024.subsystems.superstructure.Superstructure;
import org.littletonrobotics.frc2024.subsystems.superstructure.arm.Arm;

@RequiredArgsConstructor
public class DemoAutos {
private final Drive drive;
private final Superstructure superstructure;
private final Supplier<List<AutoSelector.AutoQuestionResponse>> responses;

public Command davisDemoAuto() {
return Commands.either(
followTag(),
lookAtTag(),
() -> responses.get().get(0).equals(AutoSelector.AutoQuestionResponse.YES));
}

private Command followTag() {
return drive
.startEnd(
() ->
drive.setAutoAlignGoal(
() ->
RobotState.getInstance()
.getDemoTagParameters()
.map(RobotState.DemoFollowParameters::targetPose)
.orElseGet(() -> RobotState.getInstance().getEstimatedPose()),
Translation2d::new,
false),
drive::clearAutoAlignGoal)
.alongWith(
superstructure.setGoalWithConstraintsCommand(
Superstructure.Goal.AIM_AT_DEMO_TAG, Arm.smoothProfileConstraints.get()));
}

private Command lookAtTag() {
return drive
.startEnd(
() ->
drive.setHeadingGoal(
() ->
RobotState.getInstance()
.getDemoTagParameters()
.map(RobotState.DemoFollowParameters::targetHeading)
.orElseGet(
() -> RobotState.getInstance().getEstimatedPose().getRotation())),
drive::clearHeadingGoal)
.alongWith(
superstructure.setGoalWithConstraintsCommand(
Superstructure.Goal.AIM_AT_DEMO_TAG, Arm.smoothProfileConstraints.get()));
}
}
148 changes: 114 additions & 34 deletions src/main/java/org/littletonrobotics/frc2024/subsystems/apriltagvision/AprilTagVision.java
View file
Original file line number Diff line number Diff line change
Expand Up @@ -12,11 +12,7 @@
import static org.littletonrobotics.frc2024.subsystems.apriltagvision.AprilTagVisionIO.AprilTagVisionIOInputs;

import edu.wpi.first.math.VecBuilder;
import edu.wpi.first.math.geometry.Pose2d;
import edu.wpi.first.math.geometry.Pose3d;
import edu.wpi.first.math.geometry.Quaternion;
import edu.wpi.first.math.geometry.Rotation2d;
import edu.wpi.first.math.geometry.Rotation3d;
import edu.wpi.first.math.geometry.*;
import edu.wpi.first.wpilibj.Timer;
import java.util.*;
import java.util.function.Supplier;
Expand All @@ -34,6 +30,7 @@
public class AprilTagVision extends VirtualSubsystem {
private static final LoggedTunableNumber timestampOffset =
new LoggedTunableNumber("AprilTagVision/TimestampOffset", -(1.0 / 50.0));
private static final double demoTagPosePersistenceSecs = 0.5;

private final Supplier<AprilTagLayoutType> aprilTagTypeSupplier;
private final AprilTagVisionIO[] io;
Expand All @@ -42,6 +39,9 @@ public class AprilTagVision extends VirtualSubsystem {
private final Map<Integer, Double> lastFrameTimes = new HashMap<>();
private final Map<Integer, Double> lastTagDetectionTimes = new HashMap<>();

private Pose3d demoTagPose = null;
private double lastDemoTagPoseTimestamp = 0.0;

public AprilTagVision(Supplier<AprilTagLayoutType> aprilTagTypeSupplier, AprilTagVisionIO... io) {
this.aprilTagTypeSupplier = aprilTagTypeSupplier;
this.io = io;
Expand Down Expand Up @@ -161,7 +161,7 @@ public void periodic() {
aprilTagTypeSupplier.get().getLayout().getTagPose((int) values[i]);
tagPose.ifPresent(tagPoses::add);
}
if (tagPoses.size() == 0) continue;
if (tagPoses.isEmpty()) continue;

// Calculate average distance to tag
double totalDistance = 0.0;
Expand Down Expand Up @@ -199,39 +199,119 @@ public void periodic() {
"AprilTagVision/Inst" + instanceIndex + "/TagPoses", tagPoses.toArray(Pose3d[]::new));
}

// If no frames from instances, clear robot pose
if (inputs[instanceIndex].timestamps.length == 0) {
Logger.recordOutput("AprilTagVision/Inst" + instanceIndex + "/RobotPose", new Pose2d());
Logger.recordOutput("AprilTagVision/Inst" + instanceIndex + "/RobotPose3d", new Pose3d());
// Record demo tag pose
if (inputs[instanceIndex].demoFrame.length > 0) {
var values = inputs[instanceIndex].demoFrame;
double error0 = values[0];
double error1 = values[8];
Pose3d fieldToCameraPose =
new Pose3d(RobotState.getInstance().getEstimatedPose())
.transformBy(cameraPoses[instanceIndex].toTransform3d());
Pose3d fieldToTagPose0 =
fieldToCameraPose.transformBy(
new Transform3d(
new Translation3d(values[1], values[2], values[3]),
new Rotation3d(new Quaternion(values[4], values[5], values[6], values[7]))));
Pose3d fieldToTagPose1 =
fieldToCameraPose.transformBy(
new Transform3d(
new Translation3d(values[9], values[10], values[11]),
new Rotation3d(
new Quaternion(values[12], values[13], values[14], values[15]))));
Pose3d fieldToTagPose;

// Find best pose
if (demoTagPose == null && error0 < error1) {
fieldToTagPose = fieldToTagPose0;
} else if (demoTagPose == null && error0 >= error1) {
fieldToTagPose = fieldToTagPose1;
} else if (error0 < error1 * ambiguityThreshold) {
fieldToTagPose = fieldToTagPose0;
} else if (error1 < error0 * ambiguityThreshold) {
fieldToTagPose = fieldToTagPose1;
} else {
var pose0Quaternion = fieldToTagPose0.getRotation().getQuaternion();
var pose1Quaternion = fieldToTagPose1.getRotation().getQuaternion();
var referenceQuaternion = demoTagPose.getRotation().getQuaternion();
double pose0Distance =
Math.acos(
pose0Quaternion.getW() * referenceQuaternion.getW()
+ pose0Quaternion.getX() * referenceQuaternion.getX()
+ pose0Quaternion.getY() * referenceQuaternion.getY()
+ pose0Quaternion.getZ() * referenceQuaternion.getZ());
double pose1Distance =
Math.acos(
pose1Quaternion.getW() * referenceQuaternion.getW()
+ pose1Quaternion.getX() * referenceQuaternion.getX()
+ pose1Quaternion.getY() * referenceQuaternion.getY()
+ pose1Quaternion.getZ() * referenceQuaternion.getZ());
if (pose0Distance > Math.PI / 2) {
pose0Distance = Math.PI - pose0Distance;
}
if (pose1Distance > Math.PI / 2) {
pose1Distance = Math.PI - pose1Distance;
}
if (pose0Distance < pose1Distance) {
fieldToTagPose = fieldToTagPose0;
} else {
fieldToTagPose = fieldToTagPose1;
}
}

// Save pose
if (fieldToTagPose != null) {
demoTagPose = fieldToTagPose;
lastDemoTagPoseTimestamp = Timer.getFPGATimestamp();
}

// If no frames from instances, clear robot pose
if (inputs[instanceIndex].timestamps.length == 0) {
Logger.recordOutput("AprilTagVision/Inst" + instanceIndex + "/RobotPose", new Pose2d());
Logger.recordOutput("AprilTagVision/Inst" + instanceIndex + "/RobotPose3d", new Pose3d());
}

// If no recent frames from instance, clear tag poses
if (Timer.getFPGATimestamp() - lastFrameTimes.get(instanceIndex) > targetLogTimeSecs) {
//noinspection RedundantArrayCreation
Logger.recordOutput("AprilTagVision/Inst" + instanceIndex + "/TagPoses", new Pose3d[] {});
}
}

// Clear demo tag pose
if (Timer.getFPGATimestamp() - lastDemoTagPoseTimestamp > demoTagPosePersistenceSecs) {
demoTagPose = null;
}

// If no recent frames from instance, clear tag poses
if (Timer.getFPGATimestamp() - lastFrameTimes.get(instanceIndex) > targetLogTimeSecs) {
//noinspection RedundantArrayCreation
Logger.recordOutput("AprilTagVision/Inst" + instanceIndex + "/TagPoses", new Pose3d[] {});
// Log robot poses
Logger.recordOutput("AprilTagVision/RobotPoses", allRobotPoses.toArray(Pose2d[]::new));
Logger.recordOutput("AprilTagVision/RobotPoses3d", allRobotPoses3d.toArray(Pose3d[]::new));

// Log tag poses
List<Pose3d> allTagPoses = new ArrayList<>();
for (Map.Entry<Integer, Double> detectionEntry : lastTagDetectionTimes.entrySet()) {
if (Timer.getFPGATimestamp() - detectionEntry.getValue() < targetLogTimeSecs) {
aprilTagTypeSupplier
.get()
.getLayout()
.getTagPose(detectionEntry.getKey())
.ifPresent(allTagPoses::add);
}
}
}
Logger.recordOutput("AprilTagVision/TagPoses", allTagPoses.toArray(Pose3d[]::new));

// Log robot poses
Logger.recordOutput("AprilTagVision/RobotPoses", allRobotPoses.toArray(Pose2d[]::new));
Logger.recordOutput("AprilTagVision/RobotPoses3d", allRobotPoses3d.toArray(Pose3d[]::new));

// Log tag poses
List<Pose3d> allTagPoses = new ArrayList<>();
for (Map.Entry<Integer, Double> detectionEntry : lastTagDetectionTimes.entrySet()) {
if (Timer.getFPGATimestamp() - detectionEntry.getValue() < targetLogTimeSecs) {
aprilTagTypeSupplier
.get()
.getLayout()
.getTagPose(detectionEntry.getKey())
.ifPresent(allTagPoses::add);
// Log demo tag pose
if (demoTagPose == null) {
Logger.recordOutput("AprilTagVision/DemoTagPose", new Pose3d[] {});
} else {
Logger.recordOutput("AprilTagVision/DemoTagPose", demoTagPose);
}
}
Logger.recordOutput("AprilTagVision/TagPoses", allTagPoses.toArray(Pose3d[]::new));
Logger.recordOutput("AprilTagVision/DemoTagPoseId", new long[] {29});

// Send results to robot state
allVisionObservations.stream()
.sorted(Comparator.comparingDouble(VisionObservation::timestamp))
.forEach(RobotState.getInstance()::addVisionObservation);
// Send results to robot state
allVisionObservations.stream()
.sorted(Comparator.comparingDouble(VisionObservation::timestamp))
.forEach(RobotState.getInstance()::addVisionObservation);
}
RobotState.getInstance().setDemoTagPose(demoTagPose);
}
}
2 changes: 1 addition & 1 deletion src/main/java/org/littletonrobotics/frc2024/subsystems/drive/Drive.java
View file
Original file line number Diff line number Diff line change
Expand Up @@ -384,7 +384,7 @@ public void setAutoAlignGoal(
Supplier<Pose2d> poseSupplier,
Supplier<Translation2d> feedforwardSupplier,
boolean slowMode) {
if (DriverStation.isTeleopEnabled()) {
if (DriverStation.isEnabled()) {
currentDriveMode = DriveMode.AUTO_ALIGN;
autoAlignController = new AutoAlignController(poseSupplier, feedforwardSupplier, slowMode);
}
Expand Down
Loading

0 comments on commit fc52720

Please sign in to comment.