diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 9143eef..4008157 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -4,6 +4,7 @@ package frc.robot; +import java.io.IOException; import java.util.Optional; import com.pathplanner.lib.auto.AutoBuilder; @@ -11,6 +12,8 @@ import com.pathplanner.lib.auto.NamedCommands; import edu.wpi.first.math.MathUtil; import edu.wpi.first.math.geometry.Pose2d; +import edu.wpi.first.math.geometry.Transform3d; +import edu.wpi.first.math.util.Units; import edu.wpi.first.wpilibj.DriverStation; import edu.wpi.first.wpilibj.shuffleboard.BuiltInWidgets; import edu.wpi.first.wpilibj.shuffleboard.Shuffleboard; @@ -21,12 +24,16 @@ import edu.wpi.first.wpilibj2.command.InstantCommand; import edu.wpi.first.wpilibj2.command.button.CommandXboxController; import edu.wpi.first.wpilibj2.command.button.Trigger; import frc.robot.constants.OIConstants; +import frc.robot.constants.PhotonConstants; import frc.robot.subsystems.Drivetrain; +import frc.robot.utilities.PhotonVision; public class RobotContainer { private static final String kAutoTabName = "Autonomous"; private static final String kTeleopTabName = "Teloperated"; + private PhotonVision photonvision; + private Drivetrain drivetrain; private CommandXboxController primary; @@ -45,10 +52,21 @@ public class RobotContainer { private int speakerTag; public RobotContainer() { + try { + photonvision = new PhotonVision( + PhotonConstants.kCameraName, + PhotonConstants.kCameraTransform, + PhotonConstants.kCameraHeightMeters, + PhotonConstants.kCameraPitchRadians + ); + } catch (IOException e) { + photonvision = null; + } + // TODO Need to provide a real initial pose // TODO Need to provide a real IAprilTagProvider, null means we're not using one at all // TODO Need to provide a real VisualPoseProvider, null means we're not using one at all - drivetrain = new Drivetrain(new Pose2d(), null, null); + drivetrain = new Drivetrain(new Pose2d(), photonvision, null); // An example Named Command, doesn't need to remain once we start actually adding real things // ALL Named Commands need to be defined AFTER subsystem initialization and BEFORE auto/controller configuration diff --git a/src/main/java/frc/robot/constants/DrivetrainConstants.java b/src/main/java/frc/robot/constants/DrivetrainConstants.java index 5036105..b1a5bcd 100644 --- a/src/main/java/frc/robot/constants/DrivetrainConstants.java +++ b/src/main/java/frc/robot/constants/DrivetrainConstants.java @@ -47,5 +47,5 @@ public final class DrivetrainConstants { public static final boolean kGyroReversed = true; - public static final double kRobotStartOffset = 0; + public static final double kRobotStartOffset = 90; } diff --git a/src/main/java/frc/robot/constants/PhotonConstants.java b/src/main/java/frc/robot/constants/PhotonConstants.java new file mode 100644 index 0000000..bc6cda2 --- /dev/null +++ b/src/main/java/frc/robot/constants/PhotonConstants.java @@ -0,0 +1,17 @@ +package frc.robot.constants; + +import edu.wpi.first.math.geometry.Transform3d; +import edu.wpi.first.math.util.Units; + +public class PhotonConstants { + public static final String kCameraName = "Camera_Module_v1"; + + // TODO Set this to something real if visual pose estimation is used + public static final Transform3d kCameraTransform = new Transform3d(); + + // TODO The camera will be moved, this is an old value that needs to update + public static final double kCameraHeightMeters = .517525; + + // TODO The camera will be moved, this is an old value that needs to update + public static final double kCameraPitchRadians = Units.degreesToRadians(15); +} diff --git a/src/main/java/frc/robot/subsystems/Drivetrain.java b/src/main/java/frc/robot/subsystems/Drivetrain.java index a157555..90af572 100644 --- a/src/main/java/frc/robot/subsystems/Drivetrain.java +++ b/src/main/java/frc/robot/subsystems/Drivetrain.java @@ -360,7 +360,7 @@ public class Drivetrain extends SubsystemBase { -MathUtil.applyDeadband(xSpeed.getAsDouble(), deadband), -MathUtil.applyDeadband(ySpeed.getAsDouble(), deadband), output, - () -> true, + () -> fieldRelativeControl, true ); }, diff --git a/vendordeps/photonlib.json b/vendordeps/photonlib.json index f658f52..d1265c9 100644 --- a/vendordeps/photonlib.json +++ b/vendordeps/photonlib.json @@ -1,7 +1,7 @@ { "fileName": "photonlib.json", "name": "photonlib", - "version": "v2024.1.2", + "version": "v2024.2.0", "uuid": "515fe07e-bfc6-11fa-b3de-0242ac130004", "frcYear": "2024", "mavenUrls": [ @@ -14,7 +14,7 @@ { "groupId": "org.photonvision", "artifactId": "photonlib-cpp", - "version": "v2024.1.2", + "version": "v2024.2.0", "libName": "photonlib", "headerClassifier": "headers", "sharedLibrary": true, @@ -29,7 +29,7 @@ { "groupId": "org.photonvision", "artifactId": "photontargeting-cpp", - "version": "v2024.1.2", + "version": "v2024.2.0", "libName": "photontargeting", "headerClassifier": "headers", "sharedLibrary": true, @@ -46,12 +46,12 @@ { "groupId": "org.photonvision", "artifactId": "photonlib-java", - "version": "v2024.1.2" + "version": "v2024.2.0" }, { "groupId": "org.photonvision", "artifactId": "photontargeting-java", - "version": "v2024.1.2" + "version": "v2024.2.0" } ] } \ No newline at end of file