Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Add demo autos #266

Merged
merged 4 commits into from
Jun 13, 2024
Merged

Add demo autos #266

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
4 commits
Select commit Hold shift + click to select a range
33c49e2
Add demo autos
suryatho Jun 11, 2024
5f7b240
formatting :(
suryatho Jun 11, 2024
b28c0d3
Fix
jwbonner Jun 13, 2024
126a9a4
Merge branch 'main' into demo-autos
jwbonner Jun 13, 2024
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
8 changes: 8 additions & 0 deletions src/main/java/org/littletonrobotics/frc2024/Robot.java
View file
Open in desktop
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
Open in desktop
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",
jwbonner marked this conversation as resolved.
Show resolved Hide resolved
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
Open in desktop
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
Open in desktop
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),
jwbonner marked this conversation as resolved.
Show resolved Hide resolved
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
Open in desktop
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
Open in desktop
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()) {
jwbonner marked this conversation as resolved.
Show resolved Hide resolved
if (DriverStation.isEnabled()) {
currentDriveMode = DriveMode.AUTO_ALIGN;
autoAlignController = new AutoAlignController(poseSupplier, feedforwardSupplier, slowMode);
}
Expand Down
Loading
Loading