Skip to content

Commit 552691f

Browse files
mimizh2418rutmanz
mimizh2418
and
rutmanz
authored
Add Phoenix Pro features (#6)
* 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]>
1 parent 3879f28 commit 552691f

19 files changed

+1487
-113
lines changed

‎src/main/java/org/team1540/robot2024/Constants.java

+29
Original file line numberDiff line numberDiff line change
@@ -1,6 +1,8 @@
11
package org.team1540.robot2024;
22

3+
import edu.wpi.first.math.geometry.Pose3d;
34
import edu.wpi.first.math.geometry.Rotation2d;
5+
import edu.wpi.first.math.geometry.Rotation3d;
46
import edu.wpi.first.math.util.Units;
57

68
/**
@@ -44,6 +46,9 @@ public static class SwerveConfig {
4446
public static final int FRONT_RIGHT = IS_COMPETITION_ROBOT ? 4 : 0;
4547
public static final int BACK_LEFT = IS_COMPETITION_ROBOT ? 7 : 0;
4648
public static final int BACK_RIGHT = IS_COMPETITION_ROBOT ? 1 : 0;
49+
50+
// TODO: set this id
51+
public static final int PIGEON_ID = 0;
4752
}
4853

4954
public static class Drivetrain {
@@ -59,6 +64,30 @@ public static class Drivetrain {
5964
public static final double MAX_ANGULAR_SPEED = MAX_LINEAR_SPEED / DRIVE_BASE_RADIUS;
6065
}
6166

67+
68+
public static class Vision {
69+
public static final String FRONT_CAMERA_NAME = "limelight-front";
70+
public static final String REAR_CAMERA_NAME = "limelight-rear";
71+
72+
// TODO: measure these offsets
73+
public static final Pose3d FRONT_CAMERA_POSE = new Pose3d(0, 0, 0.5, new Rotation3d());
74+
public static final Pose3d REAR_CAMERA_POSE = new Pose3d(0, 0, 0.5, new Rotation3d(0, 0, Math.PI));
75+
76+
// TODO: find these values
77+
public static final double MAX_VISION_DELAY_SECS = 0.08;
78+
public static final double MAX_ACCEPTED_ROT_SPEED_RAD_PER_SEC = 1.0;
79+
public static final double MAX_ACCEPTED_LINEAR_SPEED_MPS = 4.0;
80+
public static final double MIN_ACCEPTED_NUM_TAGS = 1;
81+
public static final double MAX_ACCEPTED_AVG_TAG_DIST_METERS = 8.0;
82+
public static final double MAX_ACCEPTED_ELEVATOR_SPEED_MPS = 0.05;
83+
84+
public static final int SIM_RES_WIDTH = 1280;
85+
public static final int SIM_RES_HEIGHT = 960;
86+
public static final Rotation2d SIM_DIAGONAL_FOV = Rotation2d.fromDegrees(100);
87+
public static final double SIM_FPS = 14.5;
88+
public static final double SIM_AVG_LATENCY_MS = 67.0;
89+
}
90+
6291
public static class Shooter {
6392
public static class Flywheels {
6493
// TODO: determine ids

‎src/main/java/org/team1540/robot2024/Robot.java

+1
Original file line numberDiff line numberDiff line change
@@ -90,6 +90,7 @@ public void robotPeriodic() {
9090
// This must be called from the robot's periodic block in order for anything in
9191
// the Command-based framework to work.
9292
CommandScheduler.getInstance().run();
93+
if (Constants.currentMode == Constants.Mode.REAL) robotContainer.odometrySignalRefresher.periodic();
9394

9495
// Update mechanism visualiser in sim
9596
if (Robot.isSimulation()) MechanismVisualiser.periodic();

‎src/main/java/org/team1540/robot2024/RobotContainer.java

+34-6
Original file line numberDiff line numberDiff line change
@@ -18,8 +18,14 @@
1818
import org.team1540.robot2024.subsystems.tramp.TrampIOSim;
1919
import org.team1540.robot2024.subsystems.tramp.TrampIOSparkMax;
2020
import org.team1540.robot2024.subsystems.shooter.*;
21+
import org.team1540.robot2024.subsystems.vision.AprilTagVision;
22+
import org.team1540.robot2024.subsystems.vision.AprilTagVisionIO;
23+
import org.team1540.robot2024.subsystems.vision.AprilTagVisionIOLimelight;
24+
import org.team1540.robot2024.subsystems.vision.AprilTagVisionIOSim;
25+
import org.team1540.robot2024.util.PhoenixTimeSyncSignalRefresher;
2126
import org.team1540.robot2024.util.swerve.SwerveFactory;
2227
import org.littletonrobotics.junction.networktables.LoggedDashboardChooser;
28+
import org.team1540.robot2024.util.vision.VisionPoseAcceptor;
2329

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

@@ -34,6 +40,7 @@ public class RobotContainer {
3440
public final Drivetrain drivetrain;
3541
public final Tramp tramp;
3642
public final Shooter shooter;
43+
public final AprilTagVision aprilTagVision;
3744

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

52+
public final PhoenixTimeSyncSignalRefresher odometrySignalRefresher = new PhoenixTimeSyncSignalRefresher(SwerveConfig.CAN_BUS);
53+
4554
// TODO: testing dashboard inputs, remove for comp
4655
public final LoggedDashboardNumber leftFlywheelSetpoint = new LoggedDashboardNumber("Shooter/Flywheels/leftSetpoint", 6000);
4756
public final LoggedDashboardNumber rightFlywheelSetpoint = new LoggedDashboardNumber("Shooter/Flywheels/rightSetpoint", 6000);
@@ -55,13 +64,19 @@ public RobotContainer() {
5564
// Real robot, instantiate hardware IO implementations
5665
drivetrain =
5766
new Drivetrain(
58-
new GyroIONavx(),
59-
new ModuleIOTalonFX(SwerveFactory.getModuleMotors(SwerveConfig.FRONT_LEFT, SwerveFactory.SwerveCorner.FRONT_LEFT)),
60-
new ModuleIOTalonFX(SwerveFactory.getModuleMotors(SwerveConfig.FRONT_RIGHT, SwerveFactory.SwerveCorner.FRONT_RIGHT)),
61-
new ModuleIOTalonFX(SwerveFactory.getModuleMotors(SwerveConfig.BACK_LEFT, SwerveFactory.SwerveCorner.BACK_LEFT)),
62-
new ModuleIOTalonFX(SwerveFactory.getModuleMotors(SwerveConfig.BACK_RIGHT, SwerveFactory.SwerveCorner.BACK_RIGHT)));
67+
new GyroIOPigeon2(odometrySignalRefresher),
68+
new ModuleIOTalonFX(SwerveFactory.getModuleMotors(SwerveConfig.FRONT_LEFT, SwerveFactory.SwerveCorner.FRONT_LEFT), odometrySignalRefresher),
69+
new ModuleIOTalonFX(SwerveFactory.getModuleMotors(SwerveConfig.FRONT_RIGHT, SwerveFactory.SwerveCorner.FRONT_RIGHT), odometrySignalRefresher),
70+
new ModuleIOTalonFX(SwerveFactory.getModuleMotors(SwerveConfig.BACK_LEFT, SwerveFactory.SwerveCorner.BACK_LEFT), odometrySignalRefresher),
71+
new ModuleIOTalonFX(SwerveFactory.getModuleMotors(SwerveConfig.BACK_RIGHT, SwerveFactory.SwerveCorner.BACK_RIGHT), odometrySignalRefresher));
6372
tramp = new Tramp(new TrampIOSparkMax());
6473
shooter = new Shooter(new ShooterPivotIOTalonFX(), new FlywheelsIOTalonFX());
74+
aprilTagVision = new AprilTagVision(
75+
new AprilTagVisionIOLimelight(Constants.Vision.FRONT_CAMERA_NAME, Constants.Vision.FRONT_CAMERA_POSE),
76+
new AprilTagVisionIOLimelight(Constants.Vision.REAR_CAMERA_NAME, Constants.Vision.REAR_CAMERA_POSE),
77+
drivetrain::addVisionMeasurement,
78+
() -> 0.0, // TODO: ACTUALLY GET ELEVATOR HEIGHT HERE
79+
new VisionPoseAcceptor(drivetrain::getChassisSpeeds, () -> 0.0)); // TODO: ACTUALLY GET ELEVATOR VELOCITY HERE
6580
break;
6681
case SIM:
6782
// Sim robot, instantiate physics sim IO implementations
@@ -74,8 +89,14 @@ public RobotContainer() {
7489
new ModuleIOSim());
7590
tramp = new Tramp(new TrampIOSim());
7691
shooter = new Shooter(new ShooterPivotIOSim(), new FlywheelsIOSim());
92+
aprilTagVision =
93+
new AprilTagVision(
94+
new AprilTagVisionIOSim(Constants.Vision.FRONT_CAMERA_NAME, Constants.Vision.FRONT_CAMERA_POSE, drivetrain::getPose),
95+
new AprilTagVisionIOSim(Constants.Vision.REAR_CAMERA_NAME, Constants.Vision.REAR_CAMERA_POSE, drivetrain::getPose),
96+
ignored -> {},
97+
() -> 0.0, // TODO: ACTUALLY GET ELEVATOR HEIGHT HERE
98+
new VisionPoseAcceptor(drivetrain::getChassisSpeeds, () -> 0.0)); // TODO: ACTUALLY GET ELEVATOR VELOCITY HERE
7799
break;
78-
79100
default:
80101
// Replayed robot, disable IO implementations
81102
drivetrain =
@@ -86,6 +107,13 @@ public RobotContainer() {
86107
new ModuleIO() {},
87108
new ModuleIO() {});
88109
shooter = new Shooter(new ShooterPivotIO() {}, new FlywheelsIO() {});
110+
aprilTagVision =
111+
new AprilTagVision(
112+
new AprilTagVisionIO() {},
113+
new AprilTagVisionIO() {},
114+
(ignored) -> {},
115+
() -> 0.0,
116+
new VisionPoseAcceptor(drivetrain::getChassisSpeeds, () -> 0.0));
89117
tramp = new Tramp(new TrampIO() {});
90118
break;
91119
}

‎src/main/java/org/team1540/robot2024/subsystems/drive/Drivetrain.java

+48-28
Original file line numberDiff line numberDiff line change
@@ -5,10 +5,11 @@
55
import com.pathplanner.lib.util.HolonomicPathFollowerConfig;
66
import com.pathplanner.lib.util.PathPlannerLogging;
77
import com.pathplanner.lib.util.ReplanningConfig;
8+
import edu.wpi.first.math.VecBuilder;
9+
import edu.wpi.first.math.estimator.SwerveDrivePoseEstimator;
810
import edu.wpi.first.math.geometry.Pose2d;
911
import edu.wpi.first.math.geometry.Rotation2d;
1012
import edu.wpi.first.math.geometry.Translation2d;
11-
import edu.wpi.first.math.geometry.Twist2d;
1213
import edu.wpi.first.math.kinematics.ChassisSpeeds;
1314
import edu.wpi.first.math.kinematics.SwerveDriveKinematics;
1415
import edu.wpi.first.math.kinematics.SwerveModulePosition;
@@ -18,6 +19,7 @@
1819
import org.team1540.robot2024.util.LocalADStarAK;
1920
import org.littletonrobotics.junction.AutoLogOutput;
2021
import org.littletonrobotics.junction.Logger;
22+
import org.team1540.robot2024.util.vision.TimestampedVisionPose;
2123

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

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

2931
private final SwerveDriveKinematics kinematics = new SwerveDriveKinematics(getModuleTranslations());
30-
private Pose2d pose = new Pose2d();
31-
private Rotation2d lastGyroRotation = new Rotation2d();
32+
private Rotation2d rawGyroRotation = new Rotation2d();
3233
private boolean forceModuleAngleChange = false;
3334

35+
private final SwerveDrivePoseEstimator poseEstimator;
36+
3437
public Drivetrain(
3538
GyroIO gyroIO,
3639
ModuleIO flModuleIO,
@@ -43,6 +46,15 @@ public Drivetrain(
4346
modules[2] = new Module(blModuleIO, 2);
4447
modules[3] = new Module(brModuleIO, 3);
4548

49+
poseEstimator = new SwerveDrivePoseEstimator(
50+
kinematics,
51+
rawGyroRotation,
52+
getModulePositions(),
53+
new Pose2d(),
54+
// TODO: tune std devs (scale vision by distance?)
55+
VecBuilder.fill(0.1, 0.1, 0.1),
56+
VecBuilder.fill(0.5, 0.5, 5.0)); // Trust the gyro more than the AprilTags
57+
4658
// Configure AutoBuilder for PathPlanner
4759
AutoBuilder.configureHolonomic(
4860
this::getPose,
@@ -60,44 +72,35 @@ public Drivetrain(
6072
(targetPose) -> Logger.recordOutput("Odometry/TrajectorySetpoint", targetPose));
6173
}
6274

75+
@Override
6376
public void periodic() {
6477
gyroIO.updateInputs(gyroInputs);
6578
Logger.processInputs("Drivetrain/Gyro", gyroInputs);
6679
for (Module module : modules) {
6780
module.periodic();
6881
}
6982

70-
// Stop moving when disabled
71-
if (DriverStation.isDisabled()) {
72-
for (Module module : modules) {
73-
module.stop();
74-
}
75-
}
76-
// Log empty setpoint states when disabled
7783
if (DriverStation.isDisabled()) {
84+
// Stop moving when disabled
85+
for (Module module : modules) module.stop();
86+
87+
// Log empty setpoint states when disabled
7888
Logger.recordOutput("SwerveStates/Setpoints", new SwerveModuleState[]{});
7989
Logger.recordOutput("SwerveStates/SetpointsOptimized", new SwerveModuleState[]{});
8090
}
8191

82-
// Update odometry
92+
// Calculate module deltas
8393
SwerveModulePosition[] wheelDeltas = new SwerveModulePosition[4];
8494
for (int i = 0; i < 4; i++) {
8595
wheelDeltas[i] = modules[i].getPositionDelta();
8696
}
87-
// The twist represents the motion of the robot since the last
88-
// loop cycle in x, y, and theta based only on the modules,
89-
// without the gyro. The gyro is always disconnected in simulation.
90-
Twist2d twist = kinematics.toTwist2d(wheelDeltas);
91-
if (gyroInputs.connected) {
92-
// If the gyro is connected, replace the theta component of the twist
93-
// with the change in angle since the last loop cycle.
94-
twist =
95-
new Twist2d(
96-
twist.dx, twist.dy, gyroInputs.yawPosition.minus(lastGyroRotation).getRadians());
97-
lastGyroRotation = gyroInputs.yawPosition;
98-
}
99-
// Apply the twist (change since last loop cycle) to the current pose
100-
pose = pose.exp(twist);
97+
// Use gyro rotation if gyro is connected, otherwise use module deltas to calculate rotation delta
98+
rawGyroRotation =
99+
gyroInputs.connected ?
100+
gyroInputs.yawPosition
101+
: rawGyroRotation.plus(Rotation2d.fromRadians(kinematics.toTwist2d(wheelDeltas).dtheta));
102+
// Update odometry
103+
poseEstimator.update(rawGyroRotation, getModulePositions());
101104
}
102105

103106
/**
@@ -164,6 +167,10 @@ public double getCharacterizationVelocity() {
164167
return driveVelocityAverage / 4.0;
165168
}
166169

170+
public ChassisSpeeds getChassisSpeeds() {
171+
return kinematics.toChassisSpeeds(getModuleStates());
172+
}
173+
167174
/**
168175
* Returns the module states (turn angles and drive velocities) for all the modules.
169176
*/
@@ -181,21 +188,34 @@ private SwerveModuleState[] getModuleStates() {
181188
*/
182189
@AutoLogOutput(key = "Odometry/Robot")
183190
public Pose2d getPose() {
184-
return pose;
191+
return poseEstimator.getEstimatedPosition();
185192
}
186193

187194
/**
188195
* Returns the current odometry rotation.
189196
*/
190197
public Rotation2d getRotation() {
191-
return pose.getRotation();
198+
return getPose().getRotation();
192199
}
193200

194201
/**
195202
* Resets the current odometry pose.
196203
*/
197204
public void setPose(Pose2d pose) {
198-
this.pose = pose;
205+
poseEstimator.resetPosition(rawGyroRotation, getModulePositions(), pose);
206+
}
207+
208+
public void addVisionMeasurement(TimestampedVisionPose visionPose) {
209+
poseEstimator.addVisionMeasurement(visionPose.poseMeters(), visionPose.timestampSecs());
210+
}
211+
212+
public SwerveModulePosition[] getModulePositions() {
213+
return new SwerveModulePosition[]{
214+
modules[0].getPosition(),
215+
modules[1].getPosition(),
216+
modules[2].getPosition(),
217+
modules[3].getPosition(),
218+
};
199219
}
200220

201221
/**

‎src/main/java/org/team1540/robot2024/subsystems/drive/GyroIOPigeon2.java

+12-4
Original file line numberDiff line numberDiff line change
@@ -1,32 +1,40 @@
11
package org.team1540.robot2024.subsystems.drive;
22

33
import com.ctre.phoenix6.BaseStatusSignal;
4-
import com.ctre.phoenix6.StatusCode;
54
import com.ctre.phoenix6.StatusSignal;
65
import com.ctre.phoenix6.configs.Pigeon2Configuration;
76
import com.ctre.phoenix6.hardware.Pigeon2;
87
import edu.wpi.first.math.geometry.Rotation2d;
98
import edu.wpi.first.math.util.Units;
9+
import org.team1540.robot2024.util.PhoenixTimeSyncSignalRefresher;
10+
11+
import static org.team1540.robot2024.Constants.SwerveConfig.*;
1012

1113
/**
1214
* IO implementation for Pigeon2
1315
*/
1416
public class GyroIOPigeon2 implements GyroIO {
15-
private final Pigeon2 pigeon = new Pigeon2(20);
17+
private final Pigeon2 pigeon = new Pigeon2(PIGEON_ID);
1618
private final StatusSignal<Double> yaw = pigeon.getYaw();
1719
private final StatusSignal<Double> yawVelocity = pigeon.getAngularVelocityZDevice();
1820

19-
public GyroIOPigeon2() {
21+
private final PhoenixTimeSyncSignalRefresher odometrySignalRefresher;
22+
23+
public GyroIOPigeon2(PhoenixTimeSyncSignalRefresher odometrySignalRefresher) {
2024
pigeon.getConfigurator().apply(new Pigeon2Configuration());
2125
pigeon.getConfigurator().setYaw(0.0);
2226
yaw.setUpdateFrequency(100.0);
2327
yawVelocity.setUpdateFrequency(100.0);
2428
pigeon.optimizeBusUtilization();
29+
30+
this.odometrySignalRefresher = odometrySignalRefresher;
31+
odometrySignalRefresher.registerSignals(yaw, yawVelocity);
2532
}
2633

2734
@Override
2835
public void updateInputs(GyroIOInputs inputs) {
29-
inputs.connected = BaseStatusSignal.refreshAll(yaw, yawVelocity).equals(StatusCode.OK);
36+
odometrySignalRefresher.refreshSignals();
37+
inputs.connected = BaseStatusSignal.isAllGood(yaw, yawVelocity);
3038
inputs.yawPosition = Rotation2d.fromDegrees(yaw.getValueAsDouble());
3139
inputs.yawVelocityRadPerSec = Units.degreesToRadians(yawVelocity.getValueAsDouble());
3240
}

‎src/main/java/org/team1540/robot2024/subsystems/drive/Module.java

+2-15
Original file line numberDiff line numberDiff line change
@@ -20,8 +20,6 @@ public class Module {
2020
private final PIDController turnFeedback;
2121
private Rotation2d angleSetpoint = null; // Setpoint for closed loop control, null for open loop
2222
private Double speedSetpoint = null; // Setpoint for closed loop control, null for open loop
23-
private Rotation2d turnRelativeOffset = null; // Relative + Offset = Absolute
24-
private boolean forceTurn = false;
2523
private double lastPositionMeters = 0.0; // Used for delta calculation
2624

2725
public Module(ModuleIO io, int index) {
@@ -55,17 +53,10 @@ public Module(ModuleIO io, int index) {
5553

5654
public void periodic() {
5755
io.updateInputs(inputs);
58-
Logger.processInputs("Drive/Module" + index, inputs);
59-
60-
// On first cycle, reset relative turn encoder
61-
// Wait until absolute angle is nonzero in case it wasn't initialized yet
62-
if (turnRelativeOffset == null && inputs.turnAbsolutePosition.getRadians() != 0.0) {
63-
turnRelativeOffset = inputs.turnAbsolutePosition.minus(inputs.turnPosition);
64-
}
56+
Logger.processInputs("Drivetrain/Module" + index, inputs);
6557

6658
// Run closed loop turn control
6759
if (angleSetpoint != null) {
68-
6960
io.setTurnVoltage(
7061
turnFeedback.calculate(getAngle().getRadians(), angleSetpoint.getRadians()));
7162

@@ -143,11 +134,7 @@ public void setBrakeMode(boolean enabled) {
143134
* Returns the current turn angle of the module.
144135
*/
145136
public Rotation2d getAngle() {
146-
if (turnRelativeOffset == null) {
147-
return new Rotation2d();
148-
} else {
149-
return inputs.turnPosition.plus(turnRelativeOffset);
150-
}
137+
return inputs.turnPosition;
151138
}
152139

153140
/**

0 commit comments

Comments
 (0)