Skip to content

Commit aa22196

Browse files
Pugga12Pugga12
committed
Merge branch 'obj-detection'
2 parents b19c31f + 02835db commit aa22196

File tree

12 files changed

+2415
-387
lines changed

12 files changed

+2415
-387
lines changed

‎src/main/java/org/steelhawks/Constants.java‎

Lines changed: 2 additions & 2 deletions
Original file line numberDiff line numberDiff line change
@@ -45,7 +45,7 @@ public enum RobotType {
4545
}
4646

4747
// Change this based on what robot is being used.
48-
private static final RobotType ROBOT = RobotType.SIMBOT;
48+
private static final RobotType ROBOT = RobotType.OMEGABOT;
4949

5050
/**
5151
* The robot type.
@@ -309,7 +309,7 @@ public static Transform3d fromOnshapeCoordinates(
309309
new Rotation3d(
310310
Units.degreesToRadians(roll),
311311
Units.degreesToRadians(pitch),
312-
Units.degreesToRadians(yaw )
312+
Units.degreesToRadians(yaw)
313313
)
314314
);
315315
}

‎src/main/java/org/steelhawks/RobotContainer.java‎

Lines changed: 15 additions & 58 deletions
Original file line numberDiff line numberDiff line change
@@ -32,6 +32,7 @@
3232
import org.steelhawks.subsystems.elevator.ElevatorConstants.State;
3333
import org.steelhawks.subsystems.swerve.*;
3434
import org.steelhawks.subsystems.vision.*;
35+
import org.steelhawks.subsystems.vision.objdetect.ObjectVision;
3536
import org.steelhawks.util.DoublePressTrigger;
3637

3738
import java.util.Objects;
@@ -43,6 +44,7 @@ public class RobotContainer {
4344
private final LED s_LED = LED.getInstance();
4445
public static Swerve s_Swerve = null;
4546
public static Vision s_Vision = null;
47+
public static ObjectVision s_ObjVision = null;
4648
public static Elevator s_Elevator = null;
4749
public static Claw s_Claw = null;
4850
public static Align s_Align = null;
@@ -73,20 +75,9 @@ public RobotContainer() {
7375
new ModuleIOTalonFX(TunerConstants.BackRight));
7476
s_Vision =
7577
new Vision(
76-
s_Swerve::accept,
77-
false,
78-
new VisionIOPhoton(
79-
VisionConstants.cameraNames()[0],
80-
VisionConstants.robotToCamera()[0]),
81-
new VisionIOPhoton(
82-
VisionConstants.cameraNames()[1],
83-
VisionConstants.robotToCamera()[1]),
84-
new VisionIOPhoton(
85-
VisionConstants.cameraNames()[2],
86-
VisionConstants.robotToCamera()[2]),
87-
new VisionIOPhoton(
88-
VisionConstants.cameraNames()[3],
89-
VisionConstants.robotToCamera()[3]));
78+
s_Swerve::accept, false);
79+
s_ObjVision =
80+
new ObjectVision();
9081
s_Elevator =
9182
new Elevator(
9283
new ElevatorIOTalonFX());
@@ -113,8 +104,7 @@ public RobotContainer() {
113104
new ModuleIOTalonFX(TunerConstantsAlpha.BackRight));
114105
s_Vision =
115106
new Vision(
116-
s_Swerve::accept,
117-
new VisionIOLimelight(VisionConstants.cameraNames()[0], () -> s_Swerve.getRotation()));
107+
s_Swerve::accept);
118108
s_Elevator =
119109
new Elevator(
120110
new ElevatorIOTalonFX());
@@ -137,10 +127,7 @@ public RobotContainer() {
137127
new ModuleIOTalonFX(TunerConstantsHawkRider.BackLeft),
138128
new ModuleIOTalonFX(TunerConstantsHawkRider.BackRight));
139129
s_Vision =
140-
new Vision(
141-
s_Swerve::accept,
142-
new VisionIOLimelight(VisionConstants.cameraNames()[0], () -> s_Swerve.getRotation()),
143-
new VisionIOLimelight(VisionConstants.cameraNames()[1], () -> s_Swerve.getRotation()));
130+
new Vision(s_Swerve::accept);
144131
s_Elevator =
145132
new Elevator(
146133
new ElevatorIOTalonFX());
@@ -150,8 +137,8 @@ public RobotContainer() {
150137
Logger.recordOutput("Pose/CoralStationBottom", FieldConstants.Position.CORAL_STATION_BOTTOM.getPose());
151138
Logger.recordOutput("Swerve/ModuleTranslations", Swerve.getModuleTranslations());
152139

153-
for (int i = 0; i < VisionConstants.cameraNames().length; i++) {
154-
Logger.recordOutput("Camera/" + VisionConstants.cameraNames()[i], VisionConstants.robotToCamera()[i]);
140+
for (int i = 0; i < VisionConstants.getCameraConfig().length; i++) {
141+
Logger.recordOutput("Camera/" + VisionConstants.getCameraConfig()[i].name(), VisionConstants.getCameraConfig()[i].robotToCamera());
155142
}
156143

157144
for (ReefUtil.CoralBranch branch : ReefUtil.CoralBranch.values()) {
@@ -167,20 +154,9 @@ public RobotContainer() {
167154
new ModuleIOSim(Swerve.getDriveSimulation().getModules()[3]));
168155
s_Vision =
169156
new Vision(
170-
s_Swerve::accept,
171-
true,
172-
new VisionIOPhotonSim(
173-
VisionConstants.cameraNames()[0],
174-
VisionConstants.robotToCamera()[0],
175-
Swerve.getDriveSimulation()::getSimulatedDriveTrainPose),
176-
new VisionIOPhotonSim(
177-
VisionConstants.cameraNames()[1],
178-
VisionConstants.robotToCamera()[1],
179-
Swerve.getDriveSimulation()::getSimulatedDriveTrainPose),
180-
new VisionIOPhotonSim(
181-
VisionConstants.cameraNames()[2],
182-
VisionConstants.robotToCamera()[2],
183-
Swerve.getDriveSimulation()::getSimulatedDriveTrainPose));
157+
s_Swerve::accept, true);
158+
s_ObjVision =
159+
new ObjectVision();
184160
s_Elevator =
185161
new Elevator(
186162
new ElevatorIOSim());
@@ -207,12 +183,11 @@ public RobotContainer() {
207183
new ModuleIO() {},
208184
new ModuleIO() {});
209185

186+
s_Vision =
187+
new Vision(s_Swerve::accept);
188+
210189
switch (Constants.getRobot()) {
211190
case OMEGABOT, ALPHABOT -> {
212-
s_Vision =
213-
new Vision(
214-
s_Swerve::accept,
215-
new VisionIO() {});
216191
s_Claw =
217192
new Claw(
218193
new BeamIO() {},
@@ -221,24 +196,6 @@ public RobotContainer() {
221196
new Align(
222197
new AlignIO() {});
223198
}
224-
225-
case HAWKRIDER -> // hawkrider has 2 limelights and an orange pi running pv
226-
s_Vision =
227-
new Vision(
228-
s_Swerve::accept,
229-
new VisionIO() {},
230-
new VisionIO() {});
231-
}
232-
233-
if (Constants.getRobot() == RobotType.OMEGABOT) {
234-
s_Vision =
235-
new Vision(
236-
s_Swerve::accept,
237-
new VisionIO() {},
238-
new VisionIO() {},
239-
new VisionIO() {},
240-
new VisionIO() {},
241-
new VisionIO() {});
242199
}
243200
s_Elevator =
244201
new Elevator(

‎src/main/java/org/steelhawks/Toggles.java‎

Lines changed: 3 additions & 3 deletions
Original file line numberDiff line numberDiff line change
@@ -13,7 +13,7 @@ public interface Toggles {
1313
LoggedNetworkBoolean autoMark =
1414
new LoggedNetworkBoolean("Toggles/AutoMark", true);
1515
LoggedNetworkBoolean debugMode =
16-
new LoggedNetworkBoolean("Toggles/DebugMode", false);
16+
new LoggedNetworkBoolean("Toggles/DebugMode", true);
1717
LoggedNetworkBoolean tuningMode =
1818
new LoggedNetworkBoolean("Toggles/TuningMode", false);
1919
LoggedNetworkBoolean motionMagicEnabled =
@@ -29,8 +29,8 @@ class Vision {
2929

3030
static {
3131
camerasEnabled = new HashMap<>();
32-
for (String name : VisionConstants.cameraNames()) {
33-
camerasEnabled.put(name, new LoggedNetworkBoolean("Toggles/" + name + "Enabled", true));
32+
for (VisionConstants.CameraConfig config : VisionConstants.getCameraConfig()) {
33+
camerasEnabled.put(config.name(), new LoggedNetworkBoolean("Toggles/" + config.name() + "Enabled", true));
3434
}
3535
}
3636
}

‎src/main/java/org/steelhawks/subsystems/swerve/Swerve.java‎

Lines changed: 34 additions & 4 deletions
Original file line numberDiff line numberDiff line change
@@ -18,10 +18,8 @@
1818
import edu.wpi.first.math.filter.Debouncer;
1919
import edu.wpi.first.math.filter.Debouncer.DebounceType;
2020
import edu.wpi.first.math.geometry.*;
21-
import edu.wpi.first.math.kinematics.ChassisSpeeds;
22-
import edu.wpi.first.math.kinematics.SwerveDriveKinematics;
23-
import edu.wpi.first.math.kinematics.SwerveModulePosition;
24-
import edu.wpi.first.math.kinematics.SwerveModuleState;
21+
import edu.wpi.first.math.interpolation.TimeInterpolatableBuffer;
22+
import edu.wpi.first.math.kinematics.*;
2523
import edu.wpi.first.math.numbers.N1;
2624
import edu.wpi.first.math.numbers.N3;
2725
import edu.wpi.first.math.system.plant.DCMotor;
@@ -32,6 +30,7 @@
3230
import edu.wpi.first.wpilibj.DriverStation;
3331
import edu.wpi.first.wpilibj.DriverStation.Alliance;
3432
import edu.wpi.first.wpilibj.RobotBase;
33+
import edu.wpi.first.wpilibj.Timer;
3534
import edu.wpi.first.wpilibj2.command.Command;
3635
import edu.wpi.first.wpilibj2.command.Commands;
3736
import edu.wpi.first.wpilibj2.command.SubsystemBase;
@@ -42,6 +41,8 @@
4241
import org.ironmaple.simulation.drivesims.configs.DriveTrainSimulationConfig;
4342
import org.ironmaple.simulation.drivesims.configs.SwerveModuleSimulationConfig;
4443
import org.steelhawks.Constants.*;
44+
45+
import java.util.Optional;
4546
import java.util.concurrent.locks.Lock;
4647
import java.util.concurrent.locks.ReentrantLock;
4748
import java.util.function.BooleanSupplier;
@@ -55,6 +56,7 @@
5556
import org.steelhawks.generated.TunerConstantsAlpha;
5657
import org.steelhawks.generated.TunerConstantsHawkRider;
5758
import org.steelhawks.subsystems.elevator.ElevatorConstants;
59+
import org.steelhawks.subsystems.vision.objdetect.ObjectVision;
5860
import org.steelhawks.util.LocalADStarAK;
5961
import org.steelhawks.util.LoggedTunableNumber;
6062
import org.steelhawks.util.LoopTimeUtil;
@@ -68,6 +70,7 @@ public class Swerve extends SubsystemBase {
6870

6971
public static final double ODOMETRY_FREQUENCY =
7072
Constants.getCANBus().isNetworkFD() ? 250.0 : 100.0;
73+
private static final double POSE_BUFFER_SIZE_SEC = 2.0;
7174
public static final double DRIVE_BASE_RADIUS;
7275

7376
// PathPlanner config constants
@@ -102,6 +105,10 @@ public class Swerve extends SubsystemBase {
102105

103106
private final SwerveDrivePoseEstimator mPoseEstimator =
104107
new SwerveDrivePoseEstimator(kinematics, rawGyroRotation, lastModulePositions, new Pose2d());
108+
private final SwerveDriveOdometry odometry =
109+
new SwerveDriveOdometry(kinematics, rawGyroRotation, lastModulePositions, new Pose2d());
110+
private final TimeInterpolatableBuffer<Pose2d> poseBuffer =
111+
TimeInterpolatableBuffer.createBuffer(POSE_BUFFER_SIZE_SEC);
105112

106113
private final ProfiledPIDController mAlignController;
107114
private final Debouncer mAlignDebouncer;
@@ -456,6 +463,9 @@ public void periodic() {
456463

457464
// Apply update
458465
mPoseEstimator.updateWithTime(sampleTimestamps[i], rawGyroRotation, modulePositions);
466+
Pose2d odometryOnlyPose =
467+
odometry.update(rawGyroRotation, modulePositions);
468+
poseBuffer.addSample(sampleTimestamps[i], odometryOnlyPose);
459469
}
460470

461471
FieldConstants.FIELD_2D.setRobotPose(getPose());
@@ -631,6 +641,18 @@ public Pose2d getPose() {
631641
return mPoseEstimator.getEstimatedPosition();
632642
}
633643

644+
@AutoLogOutput(key = "Odometry/RobotWheelOdom")
645+
public Pose2d getWheelOdomPose() {
646+
return odometry.getPoseMeters();
647+
}
648+
649+
/**
650+
* Returns a pose at a certain timestamp. Interpolates inbetween to find.
651+
*/
652+
public Optional<Pose2d> getPoseAtTime(double timestamp) {
653+
return poseBuffer.getSample(timestamp);
654+
}
655+
634656
/**
635657
* Returns the current odometry rotation.
636658
*/
@@ -665,6 +687,14 @@ public void setPose(Pose2d pose) {
665687
DRIVE_SIMULATION.setSimulationWorldPose(pose);
666688
}
667689
mPoseEstimator.resetPosition(rawGyroRotation, getModulePositions(), pose);
690+
odometry.resetPosition(rawGyroRotation, getModulePositions(), pose);
691+
692+
// reset buffer b/c of discontinuous jump
693+
poseBuffer.clear();
694+
poseBuffer.addSample(Timer.getFPGATimestamp(), pose);
695+
if (RobotContainer.s_ObjVision != null) {
696+
RobotContainer.s_ObjVision.reset();
697+
}
668698
}
669699

670700
/**

‎src/main/java/org/steelhawks/subsystems/vision/Vision.java‎

Lines changed: 10 additions & 10 deletions
Original file line numberDiff line numberDiff line change
@@ -12,7 +12,6 @@
1212
import edu.wpi.first.wpilibj.Alert;
1313
import edu.wpi.first.wpilibj.Alert.AlertType;
1414
import edu.wpi.first.wpilibj.DriverStation;
15-
import edu.wpi.first.wpilibj2.command.SubsystemBase;
1615
import org.steelhawks.Constants;
1716
import org.steelhawks.Robot;
1817
import org.steelhawks.RobotContainer;
@@ -22,8 +21,9 @@
2221
import java.util.List;
2322
import org.littletonrobotics.junction.Logger;
2423
import org.steelhawks.util.LoopTimeUtil;
24+
import org.steelhawks.util.VirtualSubsystem;
2525

26-
public class Vision extends SubsystemBase {
26+
public class Vision extends VirtualSubsystem {
2727
private final VisionConsumer consumer;
2828
private final VisionIO[] io;
2929
private final VisionIOInputsAutoLogged[] inputs;
@@ -35,14 +35,14 @@ public class Vision extends SubsystemBase {
3535

3636
private final QuestNavImpl questNav;
3737

38-
public Vision(VisionConsumer consumer, VisionIO... io) {
39-
this(consumer, false, io);
38+
public Vision(VisionConsumer consumer) {
39+
this(consumer, false);
4040
}
4141

42-
public Vision(VisionConsumer consumer, boolean useQuestNav, VisionIO... io) {
42+
public Vision(VisionConsumer consumer, boolean useQuestNav) {
4343
this.consumer = consumer;
4444
this.useQuestNav = useQuestNav;
45-
this.io = io;
45+
this.io = VisionConstants.getIO();
4646

4747
if (useQuestNav) {
4848
questNav = new QuestNavImpl(consumer);
@@ -114,7 +114,7 @@ public void periodic() {
114114
if (Toggles.Vision.camerasEnabled.get(io[i].getName()).get()) {
115115
io[i].updateInputs(inputs[i]);
116116
}
117-
Logger.processInputs("Vision/Camera" + i, inputs[i]);
117+
Logger.processInputs("Vision/" + io[i].getName(), inputs[i]);
118118
}
119119

120120
boolean currPathfinding = RobotContainer.s_Swerve.isPathfinding();
@@ -199,9 +199,9 @@ public void periodic() {
199199
linearStdDev *= LINEAR_STD_DEV_MEGATAG2_FACTOR;
200200
angularStdDev *= ANGULAR_STD_DEV_MEGATAG2_FACTOR;
201201
}
202-
if (cameraIndex < CAMERA_STD_DEV_FACTORS.length) {
203-
linearStdDev *= CAMERA_STD_DEV_FACTORS[cameraIndex];
204-
angularStdDev *= CAMERA_STD_DEV_FACTORS[cameraIndex];
202+
if (cameraIndex < VisionConstants.getCameraConfig().length) {
203+
linearStdDev *= getCameraConfig()[cameraIndex].factors().getFactors()[0];
204+
angularStdDev *= getCameraConfig()[cameraIndex].factors().getFactors()[1];
205205
}
206206
if (useQuestNav && !Robot.isFirstRun()) {
207207
assert questNav != null;

0 commit comments

Comments
 (0)