Skip to content

Commit 9df8b63

Browse files
Wisdo1Wisdo1
committed
Adding pose estimation from other teams
1 parent 4f87d91 commit 9df8b63

File tree

6 files changed

+119
-70
lines changed

6 files changed

+119
-70
lines changed

‎simgui.json‎

Lines changed: 2 additions & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -27,7 +27,8 @@
2727
"types": {
2828
"/FMSInfo": "FMSInfo",
2929
"/SmartDashboard/Field": "Field2d",
30-
"/SmartDashboard/Pigeon 2 (v6) [11]": "Gyro"
30+
"/SmartDashboard/Pigeon 2 (v6) [11]": "Gyro",
31+
"/SmartDashboard/VisionSystemSim-Vision/Sim Field": "Field2d"
3132
}
3233
},
3334
"NetworkTables Info": {

‎src/main/java/frc/robot/Constants.java‎

Lines changed: 26 additions & 4 deletions
Original file line numberDiff line numberDiff line change
@@ -8,10 +8,12 @@
88
import com.pathplanner.lib.util.PIDConstants;
99
import com.pathplanner.lib.util.ReplanningConfig;
1010

11+
import edu.wpi.first.math.geometry.Rotation3d;
1112
import edu.wpi.first.math.geometry.Translation3d;
1213
import edu.wpi.first.math.util.Units;
1314
import swervelib.math.Matter;
1415

16+
1517
/**
1618
* The Constants class provides a convenient place for teams to hold robot-wide numerical or boolean constants. This
1719
* class should not be used for any other purpose. All constants should be declared globally (i.e. public static). Do
@@ -22,7 +24,7 @@
2224
*/
2325
public final class Constants
2426
{
25-
public static final Mode currentMode = Mode.REAL;
27+
public static final Mode currentMode = Mode.SIM;
2628

2729
public static enum Mode {
2830
/** Running on a real robot. */
@@ -46,10 +48,10 @@ public static final class AutonConstants
4648
public static final PIDConstants TRANSLATION_PID = new PIDConstants(0.7, 0, 0);
4749
public static final PIDConstants ANGLE_PID = new PIDConstants(0.4, 0, 0.01);
4850

49-
public static final HolonomicPathFollowerConfig config = new HolonomicPathFollowerConfig( // HolonomicPathFollowerConfig, this should likely live in your Constants class
50-
AutonConstants.TRANSLATION_PID,
51+
public static final HolonomicPathFollowerConfig config = new HolonomicPathFollowerConfig(
52+
TRANSLATION_PID,
5153
// Translation PID constants
52-
AutonConstants.ANGLE_PID,
54+
ANGLE_PID,
5355
// Rotation PID constants
5456
4.5,
5557
// TODO: Max module speed, in m/s Drive base radius from center of robot to the farthest wheel in meters
@@ -76,4 +78,24 @@ public static class OperatorConstants
7678
public static final double RIGHT_X_DEADBAND = 0.1;
7779
public static final double TURN_CONSTANT = 6;
7880
}
81+
82+
83+
//For Easier camera setup to be used with already made vision examples
84+
public static class PoseCameraConstants
85+
{
86+
public static final String CAM1N = "W";
87+
public static final Rotation3d CAM1R = new Rotation3d(0, 0, 0);
88+
public static final Translation3d CAM1T = new Translation3d(Units.inchesToMeters(-4.628),
89+
Units.inchesToMeters(-10.687),
90+
Units.inchesToMeters(16.129));
91+
92+
public static final String CAM2N = "W";
93+
public static final Rotation3d CAM2R = new Rotation3d(0, 0, 0);
94+
public static final Translation3d CAM2T = new Translation3d(Units.inchesToMeters(-4.628),
95+
Units.inchesToMeters(-10.687),
96+
Units.inchesToMeters(16.129));
97+
98+
//it goes up to 4 but it is commented out in SwerveVision
99+
100+
}
79101
}

‎src/main/java/frc/robot/subsystems/Swerve/SWERVEVision.java‎

Lines changed: 19 additions & 28 deletions
Original file line numberDiff line numberDiff line change
@@ -13,9 +13,10 @@
1313
import edu.wpi.first.math.geometry.Translation3d;
1414
import edu.wpi.first.math.numbers.N1;
1515
import edu.wpi.first.math.numbers.N3;
16-
import edu.wpi.first.math.util.Units;
1716
import edu.wpi.first.wpilibj.smartdashboard.Field2d;
1817
import frc.robot.Robot;
18+
import frc.robot.Constants.PoseCameraConstants;
19+
1920
import java.awt.Desktop;
2021
import java.io.IOException;
2122
import java.net.URI;
@@ -366,33 +367,23 @@ public void updateVisionField()
366367
*/
367368
enum Cameras
368369
{
369-
/**
370-
* Left Camera
371-
*/
372-
LEFT_CAM("left",
373-
new Rotation3d(0, Math.toRadians(-24.094), Math.toRadians(30)),
374-
new Translation3d(Units.inchesToMeters(12.056),
375-
Units.inchesToMeters(10.981),
376-
Units.inchesToMeters(8.44)),
377-
VecBuilder.fill(4, 4, 8), VecBuilder.fill(0.5, 0.5, 1)),
378-
/**
379-
* Right Camera
380-
*/
381-
RIGHT_CAM("right",
382-
new Rotation3d(0, Math.toRadians(-24.094), Math.toRadians(-30)),
383-
new Translation3d(Units.inchesToMeters(12.056),
384-
Units.inchesToMeters(-10.981),
385-
Units.inchesToMeters(8.44)),
386-
VecBuilder.fill(4, 4, 8), VecBuilder.fill(0.5, 0.5, 1)),
387-
/**
388-
* Center Camera
389-
*/
390-
CENTER_CAM("center",
391-
new Rotation3d(0, Units.degreesToRadians(18), 0),
392-
new Translation3d(Units.inchesToMeters(-4.628),
393-
Units.inchesToMeters(-10.687),
394-
Units.inchesToMeters(16.129)),
395-
VecBuilder.fill(4, 4, 8), VecBuilder.fill(0.5, 0.5, 1));
370+
CAM_1(PoseCameraConstants.CAM1N,
371+
PoseCameraConstants.CAM1R,
372+
PoseCameraConstants.CAM1T,
373+
VecBuilder.fill(4, 4, 8), VecBuilder.fill(0.5, 0.5, 1)),
374+
CAM_2(PoseCameraConstants.CAM2N,
375+
PoseCameraConstants.CAM2R,
376+
PoseCameraConstants.CAM2T,
377+
VecBuilder.fill(4, 4, 8), VecBuilder.fill(0.5, 0.5, 1)), ;
378+
// CAM_3("CAM3",
379+
// CameraConstants.CAM1R,
380+
// CameraConstants.CAM1T,
381+
// VecBuilder.fill(4, 4, 8), VecBuilder.fill(0.5, 0.5, 1)),
382+
// CAM_4("CAM4",
383+
// CameraConstants.CAM1R,
384+
// CameraConstants.CAM1T,
385+
// VecBuilder.fill(4, 4, 8), VecBuilder.fill(0.5, 0.5, 1)),
386+
// ;
396387

397388
/**
398389
* Latency alert to use when high latency is detected.

‎src/main/java/frc/robot/subsystems/Swerve/SwerveIO.java‎

Lines changed: 1 addition & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -26,7 +26,7 @@
2626
public class SwerveIO {
2727
SwerveDrive swerveDrive;
2828
SwerveController controller;
29-
SWERVEVision vision;
29+
3030
public SwerveIO(File directory) {
3131
// Configure the Telemetry before creating the SwerveDrive to avoid unnecessary objects being created.
3232
SwerveDriveTelemetry.verbosity = TelemetryVerbosity.HIGH;

‎src/main/java/frc/robot/subsystems/Swerve/SwerveSubsystem.java‎

Lines changed: 57 additions & 36 deletions
Original file line numberDiff line numberDiff line change
@@ -8,8 +8,6 @@
88
import com.pathplanner.lib.commands.PathPlannerAuto;
99
import com.pathplanner.lib.path.PathConstraints;
1010
import com.pathplanner.lib.path.PathPlannerPath;
11-
import com.pathplanner.lib.util.HolonomicPathFollowerConfig;
12-
import com.pathplanner.lib.util.ReplanningConfig;
1311
import edu.wpi.first.apriltag.AprilTagFieldLayout;
1412
import edu.wpi.first.apriltag.AprilTagFields;
1513
import edu.wpi.first.math.geometry.Pose2d;
@@ -21,15 +19,19 @@
2119
import edu.wpi.first.math.trajectory.Trajectory;
2220
import edu.wpi.first.math.util.Units;
2321
import edu.wpi.first.wpilibj.DriverStation;
24-
import edu.wpi.first.wpilibj.DriverStation.Alliance;
2522
import edu.wpi.first.wpilibj.Timer;
23+
import edu.wpi.first.wpilibj.DriverStation.Alliance;
2624
import edu.wpi.first.wpilibj2.command.Command;
2725
import edu.wpi.first.wpilibj2.command.SubsystemBase;
2826
import edu.wpi.first.wpilibj2.command.sysid.SysIdRoutine.Config;
2927
import frc.robot.Constants;
3028
import frc.robot.Constants.AutonConstants;
3129

3230
import java.util.function.DoubleSupplier;
31+
32+
import org.photonvision.PhotonCamera;
33+
import org.photonvision.targeting.PhotonPipelineResult;
34+
3335
// import org.photonvision.PhotonCamera;
3436
// import org.photonvision.targeting.PhotonPipelineResult;
3537
import swervelib.SwerveController;
@@ -47,12 +49,19 @@ public class SwerveSubsystem extends SubsystemBase
4749
* Swerve drive object.
4850
*/
4951
private final SwerveIO io;
52+
53+
private SWERVEVision vision;
5054

5155
/**
5256
* AprilTag field layout.
5357
*/
5458
private final AprilTagFieldLayout aprilTagFieldLayout = AprilTagFields.k2024Crescendo.loadAprilTagLayoutField();
5559

60+
/**
61+
* Enable vision odometry updates while driving.
62+
*/
63+
private boolean visionEnabled = false;
64+
5665
/**
5766
* Initialize {@link SwerveDrive} with the directory provided.
5867
*
@@ -79,6 +88,13 @@ public SwerveSubsystem(SwerveIO swerveIO)
7988

8089
io.swerveDrive.setHeadingCorrection(false); // Heading correction should only be used while controlling the robot via angle.
8190
io.swerveDrive.setCosineCompensator(false);//!SwerveDriveTelemetry.isSimulation); // Disables cosine compensation for simulations since it causes discrepancies not seen in real life.
91+
if (visionEnabled)
92+
{
93+
setupPhotonVision();
94+
// Stop the odometry thread if we are using vision that way we can synchronize updates better.
95+
io.swerveDrive.stopOdometryThread();
96+
}
97+
8298
setupPathPlanner();
8399
}
84100

@@ -103,6 +119,15 @@ public void setupPathPlanner()
103119
this // Reference to this subsystem to set requirements
104120
);
105121
}
122+
/**
123+
* Setup the photon vision class.
124+
*/
125+
public void setupPhotonVision()
126+
{
127+
vision = new SWERVEVision(io.swerveDrive::getPose, io.swerveDrive.field);
128+
}
129+
130+
106131

107132
/**
108133
* Get the distance to the speaker.
@@ -157,20 +182,20 @@ public Command aimAtSpeaker(double tolerance)
157182
* @param camera {@link PhotonCamera} to communicate with.
158183
* @return A {@link Command} which will run the alignment.
159184
*/
160-
// public Command aimAtTarget(UsbCamera camera)
161-
// {
185+
public Command aimAtTarget(PhotonCamera camera)
186+
{
162187

163-
// // return run(() -> {
164-
// // PhotonPipelineResult result = camera.getLatestResult();
165-
// // if (result.hasTargets())
166-
// // {
167-
// // drive(getTargetSpeeds(0,
168-
// // 0,
169-
// // Rotation2d.fromDegrees(result.getBestTarget()
170-
// // .getYaw()))); // Not sure if this will work, more math may be required.
171-
// // }
172-
// // });
173-
// }
188+
return run(() -> {
189+
PhotonPipelineResult result = camera.getLatestResult();
190+
if (result.hasTargets())
191+
{
192+
drive(getTargetSpeeds(0,
193+
0,
194+
Rotation2d.fromDegrees(result.getBestTarget()
195+
.getYaw()))); // Not sure if this will work, more math may be required.
196+
}
197+
});
198+
}
174199

175200
/**
176201
* Get the path follower with events.
@@ -347,6 +372,12 @@ public void drive(ChassisSpeeds velocity)
347372
@Override
348373
public void periodic()
349374
{
375+
// When vision is enabled we must manually update odometry in SwerveDrive
376+
if (visionEnabled)
377+
{
378+
io.swerveDrive.updateOdometry();
379+
vision.updatePoseEstimation(io.swerveDrive);
380+
}
350381
}
351382

352383
@Override
@@ -562,30 +593,12 @@ public Rotation2d getPitch()
562593
{
563594
return io.swerveDrive.getPitch();
564595
}
565-
566-
/**
567-
* Add a fake vision reading for testing purposes.
568-
*/
569-
public void addFakeVisionReading()
570-
{
571-
io.swerveDrive.addVisionMeasurement(new Pose2d(3, 3, Rotation2d.fromDegrees(65)), Timer.getFPGATimestamp());
572-
}
573-
574-
/**
575-
* Setup the photon vision class.
576-
*/
577-
public void setupPhotonVision()
578-
{
579-
io.vision = new SWERVEVision(io.swerveDrive::getPose, io.swerveDrive.field);
580-
io.vision.updatePoseEstimation(io.swerveDrive);
581-
}
582-
583596
/**
584597
* Update the pose estimation with vision data.
585598
*/
586599
public void updatePoseWithVision()
587600
{
588-
io.vision.updatePoseEstimation(io.swerveDrive);
601+
vision.updatePoseEstimation(io.swerveDrive);
589602
}
590603

591604
/**
@@ -595,7 +608,15 @@ public void updatePoseWithVision()
595608
*/
596609
public Pose2d getVisionPose()
597610
{
598-
io.vision.updatePoseEstimation(io.swerveDrive);
611+
vision.updatePoseEstimation(io.swerveDrive);
599612
return io.swerveDrive.getPose();
600613
}
614+
615+
/**
616+
* Add a fake vision reading for testing purposes.
617+
*/
618+
public void addFakeVisionReading()
619+
{
620+
io.swerveDrive.addVisionMeasurement(new Pose2d(3, 3, Rotation2d.fromDegrees(65)), Timer.getFPGATimestamp());
621+
}
601622
}

‎src/main/java/frc/robot/subsystems/Vision/Vision.java‎

Lines changed: 14 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,14 @@
1+
package frc.robot.subsystems.Vision;
2+
3+
import edu.wpi.first.wpilibj2.command.SubsystemBase;
4+
5+
/*
6+
* USED FOR OBJECT DETECTION AND DRIVER CAMERAS
7+
* POSE CAMERAS ARE NOT USED HERE
8+
* APRIL TAG TRACKING INDEPENDENT OF POSE IS NOT IMPLEMENTED
9+
*/
10+
11+
public class Vision extends SubsystemBase{
12+
13+
14+
}

0 commit comments

Comments
 (0)