Skip to content

Commit

Permalink
Add Phoenix Pro features (#6)
Browse files Browse the repository at this point in the history 
* feat: use waitForAll to get timesynced signals

* fix: make swerve offset units consistent, swerve offsets happen in the same place

* feat: use fused CANCoder for swerve turn position

* bump: Phoenix 6 version

* fix: import CAN_BUS constant correctly

* fix: use Rotation2d factory methods

* feat: refresh all signals at the same time (scuffed implementation?)

* feat: use pigeon instead of navx

* fix: potentially less scuffed signal refresher

* fix+feat: signal refresher no longer static, better periodic updating, and error checking

* fix: don't print stack traces to declutter logs

* refactor: rename phoenixTimeSyncSignalRefresher to odometrySignalRefresher

* AprilTag pose estimation (#18)

* feat: use SwerveDrivePoseEstimator for odometry tracking
* stop using scuffed AdvantageKit Twist2d way

* feat: AprilTag vision IO and implementation for Limelight

* feat: AprilTagVision subsystem

* feat: fuse vision poses with drivetrain poses

* fix: null vision poses no longer causing issues

* fix: actually fix null vision poses

* feat: limelight positioning adjusts based on elevator height

* fix: properly update rear camera tag poses

* feat: block vision updates with moving elevator

* fix: correctly pass in suppliers
* im dumb lol

* fix: use optionals for getting vision pose

* feat: change how vision pose accepter is used
* we can now change the code to filter out input from a single camera if the elevator is moving, instead of all cameras

* feat: untested vision sim io with photonvision sim support

* fix: use vision pose in sim

* fix: sim values as constants

* refactor: remove unnecessary IO input

* refactor: april tag layout declared in sim io instead of constants

* fix: correctly check robot rotational velocity

* fix: no duplicate constants

---------

Co-authored-by: Zach Rutman <[email protected]>
  • Loading branch information
@mimizh2418 @rutmanz
mimizh2418 and rutmanz authored Feb 3, 2024
1 parent 3879f28 commit 552691f
Show file tree
Hide file tree
Showing 19 changed files with 1,487 additions and 113 deletions.
29 changes: 29 additions & 0 deletions src/main/java/org/team1540/robot2024/Constants.java
View file
Original file line number Diff line number Diff line change
@@ -1,6 +1,8 @@
package org.team1540.robot2024;

import edu.wpi.first.math.geometry.Pose3d;
import edu.wpi.first.math.geometry.Rotation2d;
import edu.wpi.first.math.geometry.Rotation3d;
import edu.wpi.first.math.util.Units;

/**
Expand Down Expand Up @@ -44,6 +46,9 @@ public static class SwerveConfig {
public static final int FRONT_RIGHT = IS_COMPETITION_ROBOT ? 4 : 0;
public static final int BACK_LEFT = IS_COMPETITION_ROBOT ? 7 : 0;
public static final int BACK_RIGHT = IS_COMPETITION_ROBOT ? 1 : 0;

// TODO: set this id
public static final int PIGEON_ID = 0;
}

public static class Drivetrain {
Expand All @@ -59,6 +64,30 @@ public static class Drivetrain {
public static final double MAX_ANGULAR_SPEED = MAX_LINEAR_SPEED / DRIVE_BASE_RADIUS;
}


public static class Vision {
public static final String FRONT_CAMERA_NAME = "limelight-front";
public static final String REAR_CAMERA_NAME = "limelight-rear";

// TODO: measure these offsets
public static final Pose3d FRONT_CAMERA_POSE = new Pose3d(0, 0, 0.5, new Rotation3d());
public static final Pose3d REAR_CAMERA_POSE = new Pose3d(0, 0, 0.5, new Rotation3d(0, 0, Math.PI));

// TODO: find these values
public static final double MAX_VISION_DELAY_SECS = 0.08;
public static final double MAX_ACCEPTED_ROT_SPEED_RAD_PER_SEC = 1.0;
public static final double MAX_ACCEPTED_LINEAR_SPEED_MPS = 4.0;
public static final double MIN_ACCEPTED_NUM_TAGS = 1;
public static final double MAX_ACCEPTED_AVG_TAG_DIST_METERS = 8.0;
public static final double MAX_ACCEPTED_ELEVATOR_SPEED_MPS = 0.05;

public static final int SIM_RES_WIDTH = 1280;
public static final int SIM_RES_HEIGHT = 960;
public static final Rotation2d SIM_DIAGONAL_FOV = Rotation2d.fromDegrees(100);
public static final double SIM_FPS = 14.5;
public static final double SIM_AVG_LATENCY_MS = 67.0;
}

public static class Shooter {
public static class Flywheels {
// TODO: determine ids
Expand Down
1 change: 1 addition & 0 deletions src/main/java/org/team1540/robot2024/Robot.java
View file
Original file line number Diff line number Diff line change
Expand Up @@ -90,6 +90,7 @@ public void robotPeriodic() {
// This must be called from the robot's periodic block in order for anything in
// the Command-based framework to work.
CommandScheduler.getInstance().run();
if (Constants.currentMode == Constants.Mode.REAL) robotContainer.odometrySignalRefresher.periodic();

// Update mechanism visualiser in sim
if (Robot.isSimulation()) MechanismVisualiser.periodic();
Expand Down
40 changes: 34 additions & 6 deletions src/main/java/org/team1540/robot2024/RobotContainer.java
View file
Original file line number Diff line number Diff line change
Expand Up @@ -18,8 +18,14 @@
import org.team1540.robot2024.subsystems.tramp.TrampIOSim;
import org.team1540.robot2024.subsystems.tramp.TrampIOSparkMax;
import org.team1540.robot2024.subsystems.shooter.*;
import org.team1540.robot2024.subsystems.vision.AprilTagVision;
import org.team1540.robot2024.subsystems.vision.AprilTagVisionIO;
import org.team1540.robot2024.subsystems.vision.AprilTagVisionIOLimelight;
import org.team1540.robot2024.subsystems.vision.AprilTagVisionIOSim;
import org.team1540.robot2024.util.PhoenixTimeSyncSignalRefresher;
import org.team1540.robot2024.util.swerve.SwerveFactory;
import org.littletonrobotics.junction.networktables.LoggedDashboardChooser;
import org.team1540.robot2024.util.vision.VisionPoseAcceptor;

import static org.team1540.robot2024.Constants.SwerveConfig;

Expand All @@ -34,6 +40,7 @@ public class RobotContainer {
public final Drivetrain drivetrain;
public final Tramp tramp;
public final Shooter shooter;
public final AprilTagVision aprilTagVision;

// Controller
public final CommandXboxController driver = new CommandXboxController(0);
Expand All @@ -42,6 +49,8 @@ public class RobotContainer {
// Dashboard inputs
public final LoggedDashboardChooser<Command> autoChooser;

public final PhoenixTimeSyncSignalRefresher odometrySignalRefresher = new PhoenixTimeSyncSignalRefresher(SwerveConfig.CAN_BUS);

// TODO: testing dashboard inputs, remove for comp
public final LoggedDashboardNumber leftFlywheelSetpoint = new LoggedDashboardNumber("Shooter/Flywheels/leftSetpoint", 6000);
public final LoggedDashboardNumber rightFlywheelSetpoint = new LoggedDashboardNumber("Shooter/Flywheels/rightSetpoint", 6000);
Expand All @@ -55,13 +64,19 @@ public RobotContainer() {
// Real robot, instantiate hardware IO implementations
drivetrain =
new Drivetrain(
new GyroIONavx(),
new ModuleIOTalonFX(SwerveFactory.getModuleMotors(SwerveConfig.FRONT_LEFT, SwerveFactory.SwerveCorner.FRONT_LEFT)),
new ModuleIOTalonFX(SwerveFactory.getModuleMotors(SwerveConfig.FRONT_RIGHT, SwerveFactory.SwerveCorner.FRONT_RIGHT)),
new ModuleIOTalonFX(SwerveFactory.getModuleMotors(SwerveConfig.BACK_LEFT, SwerveFactory.SwerveCorner.BACK_LEFT)),
new ModuleIOTalonFX(SwerveFactory.getModuleMotors(SwerveConfig.BACK_RIGHT, SwerveFactory.SwerveCorner.BACK_RIGHT)));
new GyroIOPigeon2(odometrySignalRefresher),
new ModuleIOTalonFX(SwerveFactory.getModuleMotors(SwerveConfig.FRONT_LEFT, SwerveFactory.SwerveCorner.FRONT_LEFT), odometrySignalRefresher),
new ModuleIOTalonFX(SwerveFactory.getModuleMotors(SwerveConfig.FRONT_RIGHT, SwerveFactory.SwerveCorner.FRONT_RIGHT), odometrySignalRefresher),
new ModuleIOTalonFX(SwerveFactory.getModuleMotors(SwerveConfig.BACK_LEFT, SwerveFactory.SwerveCorner.BACK_LEFT), odometrySignalRefresher),
new ModuleIOTalonFX(SwerveFactory.getModuleMotors(SwerveConfig.BACK_RIGHT, SwerveFactory.SwerveCorner.BACK_RIGHT), odometrySignalRefresher));
tramp = new Tramp(new TrampIOSparkMax());
shooter = new Shooter(new ShooterPivotIOTalonFX(), new FlywheelsIOTalonFX());
aprilTagVision = new AprilTagVision(
new AprilTagVisionIOLimelight(Constants.Vision.FRONT_CAMERA_NAME, Constants.Vision.FRONT_CAMERA_POSE),
new AprilTagVisionIOLimelight(Constants.Vision.REAR_CAMERA_NAME, Constants.Vision.REAR_CAMERA_POSE),
drivetrain::addVisionMeasurement,
() -> 0.0, // TODO: ACTUALLY GET ELEVATOR HEIGHT HERE
new VisionPoseAcceptor(drivetrain::getChassisSpeeds, () -> 0.0)); // TODO: ACTUALLY GET ELEVATOR VELOCITY HERE
break;
case SIM:
// Sim robot, instantiate physics sim IO implementations
Expand All @@ -74,8 +89,14 @@ public RobotContainer() {
new ModuleIOSim());
tramp = new Tramp(new TrampIOSim());
shooter = new Shooter(new ShooterPivotIOSim(), new FlywheelsIOSim());
aprilTagVision =
new AprilTagVision(
new AprilTagVisionIOSim(Constants.Vision.FRONT_CAMERA_NAME, Constants.Vision.FRONT_CAMERA_POSE, drivetrain::getPose),
new AprilTagVisionIOSim(Constants.Vision.REAR_CAMERA_NAME, Constants.Vision.REAR_CAMERA_POSE, drivetrain::getPose),
ignored -> {},
() -> 0.0, // TODO: ACTUALLY GET ELEVATOR HEIGHT HERE
new VisionPoseAcceptor(drivetrain::getChassisSpeeds, () -> 0.0)); // TODO: ACTUALLY GET ELEVATOR VELOCITY HERE
break;

default:
// Replayed robot, disable IO implementations
drivetrain =
Expand All @@ -86,6 +107,13 @@ public RobotContainer() {
new ModuleIO() {},
new ModuleIO() {});
shooter = new Shooter(new ShooterPivotIO() {}, new FlywheelsIO() {});
aprilTagVision =
new AprilTagVision(
new AprilTagVisionIO() {},
new AprilTagVisionIO() {},
(ignored) -> {},
() -> 0.0,
new VisionPoseAcceptor(drivetrain::getChassisSpeeds, () -> 0.0));
tramp = new Tramp(new TrampIO() {});
break;
}
Expand Down
76 changes: 48 additions & 28 deletions src/main/java/org/team1540/robot2024/subsystems/drive/Drivetrain.java
View file
Original file line number Diff line number Diff line change
Expand Up @@ -5,10 +5,11 @@
import com.pathplanner.lib.util.HolonomicPathFollowerConfig;
import com.pathplanner.lib.util.PathPlannerLogging;
import com.pathplanner.lib.util.ReplanningConfig;
import edu.wpi.first.math.VecBuilder;
import edu.wpi.first.math.estimator.SwerveDrivePoseEstimator;
import edu.wpi.first.math.geometry.Pose2d;
import edu.wpi.first.math.geometry.Rotation2d;
import edu.wpi.first.math.geometry.Translation2d;
import edu.wpi.first.math.geometry.Twist2d;
import edu.wpi.first.math.kinematics.ChassisSpeeds;
import edu.wpi.first.math.kinematics.SwerveDriveKinematics;
import edu.wpi.first.math.kinematics.SwerveModulePosition;
Expand All @@ -18,6 +19,7 @@
import org.team1540.robot2024.util.LocalADStarAK;
import org.littletonrobotics.junction.AutoLogOutput;
import org.littletonrobotics.junction.Logger;
import org.team1540.robot2024.util.vision.TimestampedVisionPose;

import static org.team1540.robot2024.Constants.Drivetrain.*;

Expand All @@ -27,10 +29,11 @@ public class Drivetrain extends SubsystemBase {
private final Module[] modules = new Module[4]; // FL, FR, BL, BR

private final SwerveDriveKinematics kinematics = new SwerveDriveKinematics(getModuleTranslations());
private Pose2d pose = new Pose2d();
private Rotation2d lastGyroRotation = new Rotation2d();
private Rotation2d rawGyroRotation = new Rotation2d();
private boolean forceModuleAngleChange = false;

private final SwerveDrivePoseEstimator poseEstimator;

public Drivetrain(
GyroIO gyroIO,
ModuleIO flModuleIO,
Expand All @@ -43,6 +46,15 @@ public Drivetrain(
modules[2] = new Module(blModuleIO, 2);
modules[3] = new Module(brModuleIO, 3);

poseEstimator = new SwerveDrivePoseEstimator(
kinematics,
rawGyroRotation,
getModulePositions(),
new Pose2d(),
// TODO: tune std devs (scale vision by distance?)
VecBuilder.fill(0.1, 0.1, 0.1),
VecBuilder.fill(0.5, 0.5, 5.0)); // Trust the gyro more than the AprilTags

// Configure AutoBuilder for PathPlanner
AutoBuilder.configureHolonomic(
this::getPose,
Expand All @@ -60,44 +72,35 @@ public Drivetrain(
(targetPose) -> Logger.recordOutput("Odometry/TrajectorySetpoint", targetPose));
}

@Override
public void periodic() {
gyroIO.updateInputs(gyroInputs);
Logger.processInputs("Drivetrain/Gyro", gyroInputs);
for (Module module : modules) {
module.periodic();
}

// Stop moving when disabled
if (DriverStation.isDisabled()) {
for (Module module : modules) {
module.stop();
}
}
// Log empty setpoint states when disabled
if (DriverStation.isDisabled()) {
// Stop moving when disabled
for (Module module : modules) module.stop();

// Log empty setpoint states when disabled
Logger.recordOutput("SwerveStates/Setpoints", new SwerveModuleState[]{});
Logger.recordOutput("SwerveStates/SetpointsOptimized", new SwerveModuleState[]{});
}

// Update odometry
// Calculate module deltas
SwerveModulePosition[] wheelDeltas = new SwerveModulePosition[4];
for (int i = 0; i < 4; i++) {
wheelDeltas[i] = modules[i].getPositionDelta();
}
// The twist represents the motion of the robot since the last
// loop cycle in x, y, and theta based only on the modules,
// without the gyro. The gyro is always disconnected in simulation.
Twist2d twist = kinematics.toTwist2d(wheelDeltas);
if (gyroInputs.connected) {
// If the gyro is connected, replace the theta component of the twist
// with the change in angle since the last loop cycle.
twist =
new Twist2d(
twist.dx, twist.dy, gyroInputs.yawPosition.minus(lastGyroRotation).getRadians());
lastGyroRotation = gyroInputs.yawPosition;
}
// Apply the twist (change since last loop cycle) to the current pose
pose = pose.exp(twist);
// Use gyro rotation if gyro is connected, otherwise use module deltas to calculate rotation delta
rawGyroRotation =
gyroInputs.connected ?
gyroInputs.yawPosition
: rawGyroRotation.plus(Rotation2d.fromRadians(kinematics.toTwist2d(wheelDeltas).dtheta));
// Update odometry
poseEstimator.update(rawGyroRotation, getModulePositions());
}

/**
Expand Down Expand Up @@ -164,6 +167,10 @@ public double getCharacterizationVelocity() {
return driveVelocityAverage / 4.0;
}

public ChassisSpeeds getChassisSpeeds() {
return kinematics.toChassisSpeeds(getModuleStates());
}

/**
* Returns the module states (turn angles and drive velocities) for all the modules.
*/
Expand All @@ -181,21 +188,34 @@ private SwerveModuleState[] getModuleStates() {
*/
@AutoLogOutput(key = "Odometry/Robot")
public Pose2d getPose() {
return pose;
return poseEstimator.getEstimatedPosition();
}

/**
* Returns the current odometry rotation.
*/
public Rotation2d getRotation() {
return pose.getRotation();
return getPose().getRotation();
}

/**
* Resets the current odometry pose.
*/
public void setPose(Pose2d pose) {
this.pose = pose;
poseEstimator.resetPosition(rawGyroRotation, getModulePositions(), pose);
}

public void addVisionMeasurement(TimestampedVisionPose visionPose) {
poseEstimator.addVisionMeasurement(visionPose.poseMeters(), visionPose.timestampSecs());
}

public SwerveModulePosition[] getModulePositions() {
return new SwerveModulePosition[]{
modules[0].getPosition(),
modules[1].getPosition(),
modules[2].getPosition(),
modules[3].getPosition(),
};
}

/**
Expand Down
16 changes: 12 additions & 4 deletions src/main/java/org/team1540/robot2024/subsystems/drive/GyroIOPigeon2.java
View file
Original file line number Diff line number Diff line change
@@ -1,32 +1,40 @@
package org.team1540.robot2024.subsystems.drive;

import com.ctre.phoenix6.BaseStatusSignal;
import com.ctre.phoenix6.StatusCode;
import com.ctre.phoenix6.StatusSignal;
import com.ctre.phoenix6.configs.Pigeon2Configuration;
import com.ctre.phoenix6.hardware.Pigeon2;
import edu.wpi.first.math.geometry.Rotation2d;
import edu.wpi.first.math.util.Units;
import org.team1540.robot2024.util.PhoenixTimeSyncSignalRefresher;

import static org.team1540.robot2024.Constants.SwerveConfig.*;

/**
* IO implementation for Pigeon2
*/
public class GyroIOPigeon2 implements GyroIO {
private final Pigeon2 pigeon = new Pigeon2(20);
private final Pigeon2 pigeon = new Pigeon2(PIGEON_ID);
private final StatusSignal<Double> yaw = pigeon.getYaw();
private final StatusSignal<Double> yawVelocity = pigeon.getAngularVelocityZDevice();

public GyroIOPigeon2() {
private final PhoenixTimeSyncSignalRefresher odometrySignalRefresher;

public GyroIOPigeon2(PhoenixTimeSyncSignalRefresher odometrySignalRefresher) {
pigeon.getConfigurator().apply(new Pigeon2Configuration());
pigeon.getConfigurator().setYaw(0.0);
yaw.setUpdateFrequency(100.0);
yawVelocity.setUpdateFrequency(100.0);
pigeon.optimizeBusUtilization();

this.odometrySignalRefresher = odometrySignalRefresher;
odometrySignalRefresher.registerSignals(yaw, yawVelocity);
}

@Override
public void updateInputs(GyroIOInputs inputs) {
inputs.connected = BaseStatusSignal.refreshAll(yaw, yawVelocity).equals(StatusCode.OK);
odometrySignalRefresher.refreshSignals();
inputs.connected = BaseStatusSignal.isAllGood(yaw, yawVelocity);
inputs.yawPosition = Rotation2d.fromDegrees(yaw.getValueAsDouble());
inputs.yawVelocityRadPerSec = Units.degreesToRadians(yawVelocity.getValueAsDouble());
}
Expand Down
17 changes: 2 additions & 15 deletions src/main/java/org/team1540/robot2024/subsystems/drive/Module.java
View file
Original file line number Diff line number Diff line change
Expand Up @@ -20,8 +20,6 @@ public class Module {
private final PIDController turnFeedback;
private Rotation2d angleSetpoint = null; // Setpoint for closed loop control, null for open loop
private Double speedSetpoint = null; // Setpoint for closed loop control, null for open loop
private Rotation2d turnRelativeOffset = null; // Relative + Offset = Absolute
private boolean forceTurn = false;
private double lastPositionMeters = 0.0; // Used for delta calculation

public Module(ModuleIO io, int index) {
Expand Down Expand Up @@ -55,17 +53,10 @@ public Module(ModuleIO io, int index) {

public void periodic() {
io.updateInputs(inputs);
Logger.processInputs("Drive/Module" + index, inputs);

// On first cycle, reset relative turn encoder
// Wait until absolute angle is nonzero in case it wasn't initialized yet
if (turnRelativeOffset == null && inputs.turnAbsolutePosition.getRadians() != 0.0) {
turnRelativeOffset = inputs.turnAbsolutePosition.minus(inputs.turnPosition);
}
Logger.processInputs("Drivetrain/Module" + index, inputs);

// Run closed loop turn control
if (angleSetpoint != null) {

io.setTurnVoltage(
turnFeedback.calculate(getAngle().getRadians(), angleSetpoint.getRadians()));

Expand Down Expand Up @@ -143,11 +134,7 @@ public void setBrakeMode(boolean enabled) {
* Returns the current turn angle of the module.
*/
public Rotation2d getAngle() {
if (turnRelativeOffset == null) {
return new Rotation2d();
} else {
return inputs.turnPosition.plus(turnRelativeOffset);
}
return inputs.turnPosition;
}

/**
Expand Down
Loading

0 comments on commit 552691f

Please sign in to comment.