From ce91bc2eeaeb6df873c22b28d3af506bb41006e7 Mon Sep 17 00:00:00 2001 From: Jack Nelson Date: Sun, 7 Jan 2024 14:10:30 -0500 Subject: [PATCH] add on the fly pose generation + camera based pose --- .pathplanner/settings.json | 12 ++- src/main/deploy/pathplanner/DoNothing.path | 53 ------------- src/main/deploy/pathplanner/EndAtCones.path | 49 ------------ src/main/deploy/pathplanner/FullAuto.path | 54 ------------- src/main/deploy/pathplanner/New Path.path | 24 ------ src/main/deploy/pathplanner/navgrid.json | 1 + .../pathplanner/paths/Example Path.path | 65 ++++++++++++++++ src/main/java/frc/robot/RobotContainer.java | 34 +++++++- .../java/frc/robot/commands/AimCommand.java | 30 +++---- .../frc/robot/subsystems/CameraSubsystem.java | 78 +++++++++++++++++++ .../frc/robot/subsystems/DriveSubsystem.java | 18 +++-- 11 files changed, 207 insertions(+), 211 deletions(-) delete mode 100644 src/main/deploy/pathplanner/DoNothing.path delete mode 100644 src/main/deploy/pathplanner/EndAtCones.path delete mode 100644 src/main/deploy/pathplanner/FullAuto.path delete mode 100644 src/main/deploy/pathplanner/New Path.path create mode 100644 src/main/deploy/pathplanner/navgrid.json create mode 100644 src/main/deploy/pathplanner/paths/Example Path.path create mode 100644 src/main/java/frc/robot/subsystems/CameraSubsystem.java diff --git a/.pathplanner/settings.json b/.pathplanner/settings.json index f15c9db..acd4977 100644 --- a/.pathplanner/settings.json +++ b/.pathplanner/settings.json @@ -2,6 +2,14 @@ "robotWidth": 0.75, "robotLength": 1.0, "holonomicMode": false, - "generateJSON": false, - "generateCSV": false + "pathFolders": [ + "New Folder" + ], + "autoFolders": [], + "defaultMaxVel": 3.0, + "defaultMaxAccel": 3.0, + "defaultMaxAngVel": 540.0, + "defaultMaxAngAccel": 720.0, + "maxModuleSpeed": 4.5, + "choreoProjectPath": null } \ No newline at end of file diff --git a/src/main/deploy/pathplanner/DoNothing.path b/src/main/deploy/pathplanner/DoNothing.path deleted file mode 100644 index f7a23c6..0000000 --- a/src/main/deploy/pathplanner/DoNothing.path +++ /dev/null @@ -1,53 +0,0 @@ -{ - "waypoints": [ - { - "anchorPoint": { - "x": 1.9121419127289037, - "y": 3.0 - }, - "prevControl": null, - "nextControl": { - "x": 2.9121419127289037, - "y": 3.0 - }, - "holonomicAngle": 0, - "isReversal": false, - "velOverride": null, - "isLocked": false, - "isStopPoint": false, - "stopEvent": { - "names": [ - "PlaceCube" - ], - "executionBehavior": "sequential", - "waitBehavior": "minimum", - "waitTime": 3.0 - } - }, - { - "anchorPoint": { - "x": 4.0, - "y": 3.0 - }, - "prevControl": { - "x": 3.6964541952294248, - "y": 3.0 - }, - "nextControl": null, - "holonomicAngle": 0, - "isReversal": false, - "velOverride": null, - "isLocked": false, - "isStopPoint": false, - "stopEvent": { - "names": [ - "BalanceRobot" - ], - "executionBehavior": "parallel", - "waitBehavior": "none", - "waitTime": 0 - } - } - ], - "markers": [] -} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/EndAtCones.path b/src/main/deploy/pathplanner/EndAtCones.path deleted file mode 100644 index ed0e5d6..0000000 --- a/src/main/deploy/pathplanner/EndAtCones.path +++ /dev/null @@ -1,49 +0,0 @@ -{ - "waypoints": [ - { - "anchorPoint": { - "x": 2.0, - "y": 4.5 - }, - "prevControl": null, - "nextControl": { - "x": 3.0405716965280085, - "y": 4.5 - }, - "holonomicAngle": 0, - "isReversal": false, - "velOverride": null, - "isLocked": false, - "isStopPoint": false, - "stopEvent": { - "names": [], - "executionBehavior": "sequential", - "waitBehavior": "minimum", - "waitTime": 3.0 - } - }, - { - "anchorPoint": { - "x": 6.0, - "y": 4.5 - }, - "prevControl": { - "x": 5.662926093511121, - "y": 4.5 - }, - "nextControl": null, - "holonomicAngle": 0, - "isReversal": false, - "velOverride": null, - "isLocked": false, - "isStopPoint": false, - "stopEvent": { - "names": [], - "executionBehavior": "parallel", - "waitBehavior": "none", - "waitTime": 0 - } - } - ], - "markers": [] -} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/FullAuto.path b/src/main/deploy/pathplanner/FullAuto.path deleted file mode 100644 index 7470940..0000000 --- a/src/main/deploy/pathplanner/FullAuto.path +++ /dev/null @@ -1,54 +0,0 @@ -{ - "waypoints": [ - { - "anchorPoint": { - "x": 1.9121419127289037, - "y": 3.0 - }, - "prevControl": null, - "nextControl": { - "x": 2.9121419127289037, - "y": 3.0 - }, - "holonomicAngle": 0, - "isReversal": false, - "velOverride": null, - "isLocked": false, - "isStopPoint": false, - "stopEvent": { - "names": [], - "executionBehavior": "sequential", - "waitBehavior": "minimum", - "waitTime": 3.0 - } - }, - { - "anchorPoint": { - "x": 4.0, - "y": 3.0 - }, - "prevControl": { - "x": 3.6964541952294248, - "y": 3.0 - }, - "nextControl": null, - "holonomicAngle": 0, - "isReversal": false, - "velOverride": null, - "isLocked": false, - "isStopPoint": false, - "stopEvent": { - "names": [ - "BalanceRobot" - ], - "executionBehavior": "parallel", - "waitBehavior": "none", - "waitTime": 0 - } - } - ], - "maxVelocity": null, - "maxAcceleration": null, - "isReversed": false, - "markers": [] -} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/New Path.path b/src/main/deploy/pathplanner/New Path.path deleted file mode 100644 index 5162d58..0000000 --- a/src/main/deploy/pathplanner/New Path.path +++ /dev/null @@ -1,24 +0,0 @@ -{ - "waypoints": [ - { - "anchorPoint": { - "x": 1.0, - "y": 3.0 - }, - "prevControl": null, - "nextControl": null, - "holonomicAngle": 0, - "isReversal": false, - "velOverride": null, - "isLocked": false, - "isStopPoint": false, - "stopEvent": { - "names": [], - "executionBehavior": "parallel", - "waitBehavior": "none", - "waitTime": 0 - } - } - ], - "markers": [] -} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/navgrid.json b/src/main/deploy/pathplanner/navgrid.json new file mode 100644 index 0000000..1da30af --- /dev/null +++ b/src/main/deploy/pathplanner/navgrid.json @@ -0,0 +1 @@ +{"field_size":{"x":16.54,"y":8.02},"nodeSizeMeters":0.3,"grid":[[true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true],[true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true],[true,true,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true],[true,true,true,true,true,true,true,false,false,true,true,true,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true],[true,true,true,true,true,true,true,false,true,true,true,true,true,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true],[true,true,true,true,true,true,true,false,true,true,true,true,true,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true],[true,true,true,true,true,true,true,false,true,true,true,true,true,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true],[true,true,true,true,true,true,true,false,true,true,true,true,true,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true],[true,true,true,true,true,true,true,false,true,true,true,true,true,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true],[true,true,true,true,true,true,true,false,true,true,true,true,true,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true],[true,true,true,true,true,true,true,false,true,true,true,true,true,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true],[true,true,true,true,true,true,true,false,true,true,true,true,true,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true],[true,true,true,true,true,true,true,false,true,true,true,true,true,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true],[true,true,true,true,true,true,true,false,true,true,true,true,true,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true],[true,true,true,true,true,true,true,false,false,true,true,true,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true],[true,true,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true],[true,true,true,true,true,true,true,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true],[true,true,true,true,true,true,true,true,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true],[true,true,true,true,true,true,true,true,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true],[true,true,true,true,true,true,true,true,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,true,true,true,true,true,true,true,true,true],[true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,false,false,false,false,false,false,false,true,true,true],[true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true],[true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true],[true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true],[true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true],[true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true],[true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true]]} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/Example Path.path b/src/main/deploy/pathplanner/paths/Example Path.path new file mode 100644 index 0000000..3d929c7 --- /dev/null +++ b/src/main/deploy/pathplanner/paths/Example Path.path @@ -0,0 +1,65 @@ +{ + "version": 1.0, + "waypoints": [ + { + "anchor": { + "x": 2.0, + "y": 7.0 + }, + "prevControl": null, + "nextControl": { + "x": 3.0, + "y": 6.5 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 5.0, + "y": 5.0 + }, + "prevControl": { + "x": 4.0, + "y": 6.0 + }, + "nextControl": { + "x": 6.0, + "y": 4.0 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 7.0, + "y": 1.0 + }, + "prevControl": { + "x": 6.75, + "y": 2.5 + }, + "nextControl": null, + "isLocked": false, + "linkedName": null + } + ], + "rotationTargets": [], + "constraintZones": [], + "eventMarkers": [], + "globalConstraints": { + "maxVelocity": 3.0, + "maxAcceleration": 3.0, + "maxAngularVelocity": 540.0, + "maxAngularAcceleration": 720.0 + }, + "goalEndState": { + "velocity": 0, + "rotation": 0, + "rotateFast": false + }, + "reversed": false, + "folder": null, + "previewStartingState": null, + "useDefaultConstraints": false +} \ No newline at end of file diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index cb33b0d..b9f3aa6 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -4,8 +4,12 @@ package frc.robot; +import com.pathplanner.lib.auto.AutoBuilder; import com.pathplanner.lib.auto.NamedCommands; import com.pathplanner.lib.commands.PathPlannerAuto; +import com.pathplanner.lib.path.PathConstraints; +import com.pathplanner.lib.path.PathPlannerPath; +import edu.wpi.first.math.util.Units; import edu.wpi.first.wpilibj.GenericHID; import edu.wpi.first.wpilibj.Joystick; import edu.wpi.first.wpilibj.XboxController; @@ -20,6 +24,7 @@ import frc.robot.commands.BalanceCommand; import frc.robot.commands.DefaultDrive; import frc.robot.commands.StraightCommand; +import frc.robot.subsystems.CameraSubsystem; import frc.robot.subsystems.DriveSubsystem; import frc.robot.subsystems.UltrasonicSubsystem; @@ -43,20 +48,25 @@ public class RobotContainer { new UltrasonicSubsystem(Constants.ULTRASONIC1PORT); private final DriveSubsystem m_driveSubsystem = new DriveSubsystem(); + private final CameraSubsystem m_cameraSubsystem = new CameraSubsystem(m_driveSubsystem); // The robots commands are defined here.. // private final ExampleCommand m_autoCommand = new ExampleCommand(m_exampleSubsystem); - private final AimCommand m_aimCommand = new AimCommand(m_driveSubsystem); + private final AimCommand m_aimCommand = new AimCommand(m_driveSubsystem, m_cameraSubsystem); private final BalanceCommand m_balanceCommand = new BalanceCommand(m_driveSubsystem); private final DefaultDrive m_defaultDrive = new DefaultDrive(m_driveSubsystem, this::getControllerLeftY, this::getControllerRightY); private final StraightCommand m_straightCommand = new StraightCommand(m_driveSubsystem, this::getControllerLeftY, this::getControllerRightY); - // misc init + private Command m_driveToSpeaker; + // Init Buttons private Trigger m_switchCameraButton; private Trigger m_balanceButton; private Trigger m_straightButton; + private Trigger m_brakeButton; + private Trigger m_coastButton; private JoystickButton m_aimButton; + private Trigger m_driveToSpeakerButton; // Init For Autonomous // private RamseteAutoBuilder autoBuilder; private SendableChooser autoDashboardChooser = new SendableChooser(); @@ -67,6 +77,8 @@ public RobotContainer() { configureButtonBindings(); // Initialize the autonomous command initializeAutonomous(); + // Setup On the Fly Path Planning + configureTeleopPaths(); // set default drive command m_driveSubsystem.setDefaultCommand(m_defaultDrive); @@ -81,17 +93,21 @@ public RobotContainer() { private void configureButtonBindings() { // Controller buttons m_switchCameraButton = m_controller1.x(); + m_brakeButton = m_controller1.a(); + m_coastButton = m_controller1.b(); m_balanceButton = m_controller1.rightBumper(); m_straightButton = m_controller1.rightTrigger(); + m_driveToSpeakerButton = m_controller1.y(); // Joystick buttons m_aimButton = new JoystickButton(m_flightStick, Constants.AIMBUTTON); // commands m_balanceButton.whileTrue(m_balanceCommand); m_straightButton.whileTrue(m_straightCommand); m_aimButton.whileTrue(m_aimCommand); + m_driveToSpeakerButton.whileTrue(m_driveToSpeaker); - m_controller1.a().whileTrue(new InstantCommand(() -> m_driveSubsystem.SetBrakemode())); - m_controller1.b().whileTrue(new InstantCommand(() -> m_driveSubsystem.SetCoastmode())); + m_brakeButton.whileTrue(new InstantCommand(() -> m_driveSubsystem.SetBrakemode())); + m_coastButton.whileTrue(new InstantCommand(() -> m_driveSubsystem.SetCoastmode())); } private void initializeAutonomous() { @@ -131,6 +147,16 @@ private void initializeAutonomous() { // ); } + private void configureTeleopPaths() { + // Limits for all Paths + PathConstraints constraints = + new PathConstraints(3.0, 4.0, Units.degreesToRadians(540), Units.degreesToRadians(720)); + + PathPlannerPath speakerPath = PathPlannerPath.fromPathFile("TeleopSpeakerPath"); + + m_driveToSpeaker = AutoBuilder.pathfindThenFollowPath(speakerPath, constraints); + } + public double getControllerRightY() { return -m_controller1.getRightY(); } diff --git a/src/main/java/frc/robot/commands/AimCommand.java b/src/main/java/frc/robot/commands/AimCommand.java index ad73deb..fe6a50d 100644 --- a/src/main/java/frc/robot/commands/AimCommand.java +++ b/src/main/java/frc/robot/commands/AimCommand.java @@ -7,39 +7,29 @@ import edu.wpi.first.math.util.Units; import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import edu.wpi.first.wpilibj2.command.Command; +import frc.robot.subsystems.CameraSubsystem; import frc.robot.subsystems.DriveSubsystem; -import org.photonvision.PhotonCamera; import org.photonvision.PhotonUtils; import org.photonvision.targeting.PhotonPipelineResult; /** The Aim command that uses the camera + gyro to control the robot. */ public class AimCommand extends Command { private final DriveSubsystem m_driveSubsystem; - private final PhotonCamera m_camera; - private final String CAMERANAME = "OV5647"; - // Constants such as camera and target height stored. Change per robot and goal! - final double CAMERA_HEIGHT_METERS = Units.inchesToMeters(24); - final double TARGET_HEIGHT_METERS = Units.feetToMeters(0.5); - - // Angle between horizontal and the camera. - final double CAMERA_PITCH_RADIANS = Units.degreesToRadians(0); - - // How far from the target we want to be - final double GOAL_RANGE_METERS = Units.feetToMeters(3); + private final CameraSubsystem m_cameraSubsystem; /** * Creates a new AimCommand. * * @param d_subsystem The drive subsystem used by this command. */ - public AimCommand(DriveSubsystem d_subsystem) { + public AimCommand(DriveSubsystem d_subsystem, CameraSubsystem c_subsystem) { m_driveSubsystem = d_subsystem; + m_cameraSubsystem = c_subsystem; // Change this to match the name of your camera - m_camera = new PhotonCamera(CAMERANAME); // Use addRequirements() here to declare subsystem dependencies. - addRequirements(d_subsystem); + addRequirements(d_subsystem, c_subsystem); } // Called when the command is initially scheduled. @@ -49,7 +39,7 @@ public void initialize() {} // Called every time the scheduler runs while the command is scheduled. @Override public void execute() { - PhotonPipelineResult CamResult = m_camera.getLatestResult(); + PhotonPipelineResult CamResult = m_cameraSubsystem.frontCameraResult; // will not work if cam is defined incorrectly, but will not tell you if (CamResult.hasTargets()) { SmartDashboard.putBoolean("CameraTargetDetected", true); @@ -57,11 +47,11 @@ public void execute() { SmartDashboard.putNumber("CameraTargetPitch", angleGoal); double distanceFromTarget = PhotonUtils.calculateDistanceToTargetMeters( - CAMERA_HEIGHT_METERS, - TARGET_HEIGHT_METERS, - CAMERA_PITCH_RADIANS, + m_cameraSubsystem.frontCameraHeightMeters, + m_cameraSubsystem.frontCameraTargetHeightMeters, + m_cameraSubsystem.frontCameraTargetPitchRadians, Units.degreesToRadians(CamResult.getBestTarget().getPitch())) - - GOAL_RANGE_METERS; + - m_cameraSubsystem.frontCameraGoalRangeMeters; // turn and move towards target. m_driveSubsystem.driveAndTurn(m_driveSubsystem.getYaw(), angleGoal, distanceFromTarget); // we reset the angle everytime as the target could change / move. diff --git a/src/main/java/frc/robot/subsystems/CameraSubsystem.java b/src/main/java/frc/robot/subsystems/CameraSubsystem.java new file mode 100644 index 0000000..f37b904 --- /dev/null +++ b/src/main/java/frc/robot/subsystems/CameraSubsystem.java @@ -0,0 +1,78 @@ +// Copyright (c) FIRST and other WPILib contributors. +// Open Source Software; you can modify and/or share it under the terms of +// the WPILib BSD license file in the root directory of this project. + +package frc.robot.subsystems; + +import edu.wpi.first.apriltag.AprilTagFieldLayout; +import edu.wpi.first.apriltag.AprilTagFields; +import edu.wpi.first.math.geometry.Rotation3d; +import edu.wpi.first.math.geometry.Transform3d; +import edu.wpi.first.math.geometry.Translation3d; +import edu.wpi.first.math.util.Units; +import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; +import edu.wpi.first.wpilibj2.command.SubsystemBase; +import java.util.Optional; +import org.photonvision.EstimatedRobotPose; +import org.photonvision.PhotonCamera; +import org.photonvision.PhotonPoseEstimator; +import org.photonvision.PhotonPoseEstimator.PoseStrategy; +import org.photonvision.targeting.PhotonPipelineResult; + +public class CameraSubsystem extends SubsystemBase { + private final DriveSubsystem m_driveSubsystem; + public final AprilTagFieldLayout aprilTagFieldLayout; + private final String frontCameraName = "OV5647"; + private final PhotonCamera frontCamera; + public PhotonPipelineResult frontCameraResult; + // Cam mounted facing forward, half a meter forward of center, half a meter up from center. + private final Transform3d frontCameraLocation = + new Transform3d(new Translation3d(0.5, 0.0, 0.5), new Rotation3d(0, 0, 0)); + private final PhotonPoseEstimator frontCameraPoseEstimator; + // Constants such as camera and target height stored. Change per robot and goal! + public final double frontCameraHeightMeters = Units.inchesToMeters(24); + + // Target Configruation for the front camera + public final double frontCameraTargetHeightMeters = Units.feetToMeters(0.5); + // Angle between horizontal and the camera. + public final double frontCameraTargetPitchRadians = Units.degreesToRadians(0); + // How far from the target we want to be + public final double frontCameraGoalRangeMeters = Units.feetToMeters(3); + + /** Creates a new CameraSubsystem. */ + public CameraSubsystem(DriveSubsystem d_subsystem) { + m_driveSubsystem = d_subsystem; + aprilTagFieldLayout = AprilTagFields.k2024Crescendo.loadAprilTagLayoutField(); + frontCamera = new PhotonCamera(frontCameraName); + frontCameraPoseEstimator = + new PhotonPoseEstimator( + aprilTagFieldLayout, + PoseStrategy.MULTI_TAG_PNP_ON_COPROCESSOR, + frontCamera, + frontCameraLocation); + } + + public Optional getEstimatedGlobalPose() { + return frontCameraPoseEstimator.update(); + } + + @Override + public void periodic() { + Optional pose = getEstimatedGlobalPose(); + if (pose.isPresent()) { + SmartDashboard.putBoolean("CameraConnnected", true); + m_driveSubsystem.updateVisionPose( + pose.get().estimatedPose.toPose2d(), pose.get().timestampSeconds); + } else { + SmartDashboard.putBoolean("CameraConnnected", false); + } + frontCameraResult = frontCamera.getLatestResult(); + // This method will be called once per scheduler run + SmartDashboard.putNumber("Front Camera Latency", frontCameraResult.getLatencyMillis() / 1000.0); + } + + @Override + public void simulationPeriodic() { + // This method will be called once per scheduler run during simulation + } +} diff --git a/src/main/java/frc/robot/subsystems/DriveSubsystem.java b/src/main/java/frc/robot/subsystems/DriveSubsystem.java index 77d5741..a8daae3 100644 --- a/src/main/java/frc/robot/subsystems/DriveSubsystem.java +++ b/src/main/java/frc/robot/subsystems/DriveSubsystem.java @@ -9,10 +9,10 @@ import com.pathplanner.lib.auto.AutoBuilder; import edu.wpi.first.math.MathUtil; import edu.wpi.first.math.controller.ProfiledPIDController; +import edu.wpi.first.math.estimator.DifferentialDrivePoseEstimator; import edu.wpi.first.math.geometry.Pose2d; import edu.wpi.first.math.geometry.Rotation2d; import edu.wpi.first.math.kinematics.ChassisSpeeds; -import edu.wpi.first.math.kinematics.DifferentialDriveOdometry; import edu.wpi.first.math.kinematics.DifferentialDriveWheelSpeeds; import edu.wpi.first.math.trajectory.TrapezoidProfile; import edu.wpi.first.wpilibj.Encoder; @@ -43,7 +43,7 @@ public class DriveSubsystem extends SubsystemBase { private final Encoder m_encoderRight; // Odometry class for tracking robot pose (position on field) - private final DifferentialDriveOdometry m_driveOdometry; + private final DifferentialDrivePoseEstimator m_driveOdometry; // Angle PID / RotateToAngle static final double turn_P = 0.1; @@ -143,8 +143,12 @@ public DriveSubsystem() { // configure Odemetry m_driveOdometry = - new DifferentialDriveOdometry( - getRotation2d(), m_encoderLeft.getDistance(), m_encoderRight.getDistance()); + new DifferentialDrivePoseEstimator( + DriveConstants.kDriveKinematics, + getRotation2d(), + m_encoderLeft.getDistance(), + m_encoderRight.getDistance(), + new Pose2d()); // config turn pid controller. m_turnController.enableContinuousInput(-180.0f, 180.0f); @@ -321,7 +325,11 @@ public void setWheelVelocities(DifferentialDriveWheelSpeeds speeds) { } public Pose2d getPose() { - return m_driveOdometry.getPoseMeters(); + return m_driveOdometry.getEstimatedPosition(); + } + + public void updateVisionPose(Pose2d visionRobotPose, double timestamp) { + m_driveOdometry.addVisionMeasurement(visionRobotPose, timestamp); } public void resetEncoders() {