Skip to content

Commit

Permalink
Add the thing
Browse files Browse the repository at this point in the history
  • Loading branch information
@bryceroethel
bryceroethel committed Oct 14, 2024
1 parent 0e5e2b3 commit eaaaa71
Show file tree
Hide file tree
Showing 9 changed files with 495 additions and 11 deletions.
3 changes: 3 additions & 0 deletions .vscode/settings.json
View file
Original file line number Diff line number Diff line change
Expand Up @@ -39,6 +39,8 @@
"Accl",
"Accum",
"ADIS",
"Backleft",
"Backright",
"Bezier",
"Botpose",
"Brushless",
Expand Down Expand Up @@ -69,6 +71,7 @@
"Gradlew",
"Grav",
"GRRDashboard",
"GTSAM",
"Holonomic",
"Integ",
"Interpolatable",
Expand Down
20 changes: 12 additions & 8 deletions src/main/java/org/team340/lib/swerve/SwerveAPI.java
View file
Original file line number Diff line number Diff line change
Expand Up @@ -11,6 +11,7 @@
import edu.wpi.first.math.geometry.Translation2d;
import edu.wpi.first.math.kinematics.ChassisSpeeds;
import edu.wpi.first.math.kinematics.SwerveDriveKinematics;
import edu.wpi.first.math.kinematics.SwerveDriveWheelPositions;
import edu.wpi.first.math.kinematics.SwerveModulePosition;
import edu.wpi.first.math.kinematics.SwerveModuleState;
import edu.wpi.first.math.numbers.N1;
Expand All @@ -31,6 +32,8 @@

public class SwerveAPI implements AutoCloseable {

public final SwerveState state;

final SwerveIMU imu;
final SwerveModule[] modules;
final SwerveConfig config;
Expand All @@ -39,8 +42,8 @@ public class SwerveAPI implements AutoCloseable {
private final double farthestModule;
private final Translation2d[] moduleLocations;
private final SwerveModuleState[] lockedStates;
private final SwerveDriveWheelPositions lastPositions;

private final SwerveState state;
private final SwerveDriveKinematics kinematics;
private final SwerveDrivePoseEstimator poseEstimator;

Expand All @@ -59,15 +62,18 @@ public SwerveAPI(SwerveConfig config) {
modules = new SwerveModule[moduleCount];
moduleLocations = new Translation2d[moduleCount];
lockedStates = new SwerveModuleState[moduleCount];
var lastModulePositions = new SwerveModulePosition[moduleCount];
double farthest = 0.0;
for (int i = 0; i < moduleCount; i++) {
var moduleConfig = config.modules[i];
modules[i] = new SwerveModule(config, moduleConfig);
moduleLocations[i] = moduleConfig.location;
lockedStates[i] = new SwerveModuleState(0.0, moduleLocations[i].getAngle());
lastModulePositions[i] = new SwerveModulePosition();
farthest = Math.max(farthest, moduleLocations[i].getNorm());
}

lastPositions = new SwerveDriveWheelPositions(lastModulePositions);
farthestModule = farthest;

state = new SwerveState(modules);
Expand All @@ -85,19 +91,16 @@ public SwerveAPI(SwerveConfig config) {
odometryThread = new SwerveOdometryThread();
}

/**
* Gets the current state of the robot's swerve drivetrain.
*/
public SwerveState getState() {
return state;
}

/**
* Refreshes inputs from all swerve hardware. This must be called periodically
* in order for the API to function. Typically, this method is called at the
* start of the swerve subsystem's {@code periodic()} method.
*/
public void refresh() {
for (int i = 0; i < moduleCount; i++) {
Math2.copyInto(state.modules.positions[i], lastPositions.positions[i]);
}

odometryMutex.lock();
try {
odometryThread.run(true);
Expand All @@ -120,6 +123,7 @@ public void refresh() {

state.pitch = imu.getPitch();
state.roll = imu.getRoll();
state.twist = kinematics.toTwist2d(lastPositions, new SwerveDriveWheelPositions(state.modules.positions));
state.speeds = kinematics.toChassisSpeeds(state.modules.states);
state.velocity = Math.hypot(state.speeds.vxMetersPerSecond, state.speeds.vyMetersPerSecond);

Expand Down
4 changes: 3 additions & 1 deletion src/main/java/org/team340/lib/swerve/SwerveAPILogger.java
View file
Original file line number Diff line number Diff line change
Expand Up @@ -7,6 +7,7 @@
import edu.wpi.first.epilogue.logging.errors.ErrorHandler;
import edu.wpi.first.math.geometry.Pose2d;
import edu.wpi.first.math.geometry.Rotation2d;
import edu.wpi.first.math.geometry.Twist2d;
import edu.wpi.first.math.kinematics.ChassisSpeeds;
import edu.wpi.first.math.kinematics.SwerveModulePosition;
import edu.wpi.first.math.kinematics.SwerveModuleState;
Expand All @@ -20,7 +21,7 @@ public SwerveAPILogger() {

@Override
public void update(DataLogger logger, SwerveAPI swerveAPI) {
logState(logger.getSubLogger("state"), swerveAPI.getState());
logState(logger.getSubLogger("state"), swerveAPI.state);
var hardwareLogger = logger.getSubLogger("hardware");

ErrorHandler errorHandler = Epilogue.getConfig().errorHandler;
Expand All @@ -37,6 +38,7 @@ private void logState(DataLogger logger, SwerveState state) {
logger.log("pitch", state.pitch, Rotation2d.struct);
logger.log("roll", state.roll, Rotation2d.struct);
logger.log("pose", state.pose, Pose2d.struct);
logger.log("twist", state.twist, Twist2d.struct);
logger.log("speeds", state.speeds, ChassisSpeeds.struct);
logger.log("targetSpeeds", state.targetSpeeds, ChassisSpeeds.struct);
logger.log("velocity", state.velocity);
Expand Down
4 changes: 4 additions & 0 deletions src/main/java/org/team340/lib/swerve/SwerveState.java
View file
Original file line number Diff line number Diff line change
Expand Up @@ -2,6 +2,7 @@

import edu.wpi.first.math.geometry.Pose2d;
import edu.wpi.first.math.geometry.Rotation2d;
import edu.wpi.first.math.geometry.Twist2d;
import edu.wpi.first.math.kinematics.ChassisSpeeds;
import edu.wpi.first.math.kinematics.SwerveModulePosition;
import edu.wpi.first.math.kinematics.SwerveModuleState;
Expand Down Expand Up @@ -65,6 +66,8 @@ private Odometry() {}
public Rotation2d roll;
/** The current blue origin relative pose of the robot. */
public Pose2d pose;
/** A {@link Twist2d} representing the robot's movement since the last loop. */
public Twist2d twist;
/** The current measured robot-relative speeds. */
public ChassisSpeeds speeds;
/** The target robot-relative speeds when using {@code applySpeeds()}. */
Expand All @@ -78,6 +81,7 @@ private Odometry() {}
pitch = Math2.kZeroRotation2d;
roll = Math2.kZeroRotation2d;
pose = Math2.kZeroPose2d;
twist = new Twist2d();
speeds = new ChassisSpeeds();
targetSpeeds = new ChassisSpeeds();
}
Expand Down
119 changes: 117 additions & 2 deletions src/main/java/org/team340/robot/subsystems/Swerve.java
View file
Original file line number Diff line number Diff line change
@@ -1,13 +1,33 @@
package org.team340.robot.subsystems;

import com.revrobotics.CANSparkLowLevel.MotorType;
import edu.wpi.first.apriltag.AprilTagFieldLayout;
import edu.wpi.first.apriltag.AprilTagFieldLayout.OriginPosition;
import edu.wpi.first.apriltag.AprilTagFields;
import edu.wpi.first.epilogue.Logged;
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.Rotation3d;
import edu.wpi.first.math.geometry.Transform3d;
import edu.wpi.first.math.geometry.Translation3d;
import edu.wpi.first.math.geometry.Twist2d;
import edu.wpi.first.math.geometry.Twist3d;
import edu.wpi.first.math.util.Units;
import edu.wpi.first.util.WPIUtilJNI;
import edu.wpi.first.wpilibj.ADIS16470_IMU.CalibrationTime;
import edu.wpi.first.wpilibj.ADIS16470_IMU.IMUAxis;
import edu.wpi.first.wpilibj.SPI.Port;
import edu.wpi.first.wpilibj2.command.Command;
import java.util.ArrayList;
import java.util.Arrays;
import java.util.List;
import java.util.Optional;
import java.util.function.Supplier;
import org.photonvision.PhotonCamera;
import org.photonvision.PhotonPoseEstimator;
import org.photonvision.PhotonPoseEstimator.PoseStrategy;
import org.photonvision.targeting.PhotonPipelineResult;
import org.team340.lib.swerve.SwerveAPI;
import org.team340.lib.swerve.SwerveAPI.ForwardPerspective;
import org.team340.lib.swerve.config.SwerveConfig;
Expand All @@ -18,6 +38,8 @@
import org.team340.lib.util.GRRSubsystem;
import org.team340.robot.Constants;
import org.team340.robot.Constants.RobotMap;
import org.team340.robot.tagslam.GtsamInterface;
import org.team340.robot.tagslam.TagDetection;

@Logged
public class Swerve extends GRRSubsystem {
Expand Down Expand Up @@ -50,7 +72,7 @@ public class Swerve extends GRRSubsystem {
.setTurnMotor(SwerveMotors.sparkFlex(RobotMap.BR_TURN, MotorType.kBrushless, true))
.setEncoder(SwerveEncoders.sparkFlexEncoder(0.6675, true));

public static final SwerveConfig CONFIG = new SwerveConfig()
private static final SwerveConfig CONFIG = new SwerveConfig()
.setTimings(Constants.PERIOD, Constants.PERIOD, 0.04)
.setMovePID(0.0015, 0.0, 0.0, 0.0)
.setMoveFF(0.05, 0.212)
Expand All @@ -60,20 +82,113 @@ public class Swerve extends GRRSubsystem {
.setDriverProfile(4.3, 1.0, 4.2, 2.0)
.setPowerProperties(Constants.VOLTAGE, 80.0, 60.0)
.setMechanicalProperties(6.75, 150.0 / 7.0, 0.0, Units.inchesToMeters(4.0))
.setOdometryStd(0.003, 0.003, 0.0012)
.setOdometryStd(0.1, 0.1, 0.05)
.setIMU(SwerveIMUs.adis16470(IMUAxis.kZ, IMUAxis.kX, IMUAxis.kY, Port.kOnboardCS0, CalibrationTime._4s))
.setModules(FRONT_LEFT, FRONT_RIGHT, BACK_LEFT, BACK_RIGHT);

private static final Transform3d BACK_LEFT_CAMERA = new Transform3d(
new Translation3d(-0.29153, 0.26629, 0.24511),
new Rotation3d(0.0, Math.toRadians(-20.0), Math.toRadians(-160.0))
);

private static final Transform3d BACK_RIGHT_CAMERA = new Transform3d(
new Translation3d(-0.29153, -0.26629, 0.24511),
new Rotation3d(0.0, Math.toRadians(-20.0), Math.toRadians(160.0))
);

private final SwerveAPI api;

private final AprilTagFieldLayout aprilTags;
private final GtsamInterface gtsamInterface;
private final PhotonCamera[] cameras;
private final PhotonPoseEstimator[] poseEstimators;
private final PhotonPipelineResult[] lastResults;

private final List<Pose2d> measurements = new ArrayList<>();
private final List<Pose3d> targets = new ArrayList<>();
private Pose2d gtsamPose = new Pose2d();

public Swerve() {
api = new SwerveAPI(CONFIG);
api.enableTunables("Swerve");

aprilTags = AprilTagFields.k2024Crescendo.loadAprilTagLayoutField();
aprilTags.setOrigin(OriginPosition.kBlueAllianceWallRightSide);
gtsamInterface = new GtsamInterface(List.of("backleft", "backright"));
cameras = new PhotonCamera[] { new PhotonCamera("backleft"), new PhotonCamera("backright") };
poseEstimators = new PhotonPoseEstimator[] {
new PhotonPoseEstimator(aprilTags, PoseStrategy.MULTI_TAG_PNP_ON_COPROCESSOR, cameras[0], BACK_LEFT_CAMERA),
new PhotonPoseEstimator(aprilTags, PoseStrategy.MULTI_TAG_PNP_ON_COPROCESSOR, cameras[1], BACK_RIGHT_CAMERA)
};
lastResults = new PhotonPipelineResult[cameras.length];
Arrays.fill(lastResults, new PhotonPipelineResult());
}

@Override
public void periodic() {
long loopStart = WPIUtilJNI.now();

api.refresh();

Twist2d twist = api.state.twist;
gtsamInterface.sendOdomUpdate(loopStart, new Twist3d(twist.dx, twist.dy, 0, 0, 0, twist.dtheta), null);

measurements.clear();
targets.clear();

for (int i = 0; i < cameras.length; i++) {
var camera = cameras[i];
var poseEstimator = poseEstimators[i];
var results = camera.getLatestResult();
var last = lastResults[i];

List<TagDetection> dets = new ArrayList<>();
long tagDetTime = 0;
if (results.getTimestampSeconds() != last.getTimestampSeconds()) {
lastResults[i] = results;
for (var result : results.getTargets()) {
dets.add(new TagDetection(result.getFiducialId(), result.getDetectedCorners()));
}

tagDetTime = loopStart - 10000; // YIPPEE

var estimate = poseEstimators[i].update(results, camera.getCameraMatrix(), camera.getDistCoeffs());
if (estimate.isPresent()) {
double sum = 0.0;
for (var target : estimate.get().targetsUsed) {
Optional<Pose3d> tagPose = aprilTags.getTagPose(target.getFiducialId());
if (tagPose.isEmpty()) continue;
targets.add(tagPose.get());
sum +=
api.state.pose.getTranslation().getDistance(tagPose.get().getTranslation().toTranslation2d());
}

int tagCount = estimate.get().targetsUsed.size();
double stdScale = Math.pow(sum / tagCount, 2.0) / tagCount;
double xyStd = 0.1 * stdScale;
double angStd = 0.15 * stdScale;

Pose2d estimatedPose = estimate.get().estimatedPose.toPose2d();
api.addVisionMeasurement(
estimatedPose,
estimate.get().timestampSeconds,
VecBuilder.fill(xyStd, xyStd, angStd)
);
measurements.add(estimatedPose);
continue;
}
}

gtsamInterface.setCamIntrinsics(camera.getName(), camera.getCameraMatrix(), camera.getDistCoeffs());
gtsamInterface.sendVisionUpdate(
camera.getName(),
tagDetTime,
dets,
poseEstimator.getRobotToCameraTransform()
);
}

gtsamPose = gtsamInterface.getLatencyCompensatedPoseEstimate().toPose2d();
}

/**
Expand Down
Loading

0 comments on commit eaaaa71

Please sign in to comment.