diff --git a/.pathplanner/settings.json b/.pathplanner/settings.json new file mode 100644 index 0000000..bccf75d --- /dev/null +++ b/.pathplanner/settings.json @@ -0,0 +1,14 @@ +{ + "robotWidth": 0.9, + "robotLength": 0.9, + "holonomicMode": true, + "pathFolders": [], + "autoFolders": [ + "New Folder" + ], + "defaultMaxVel": 3.0, + "defaultMaxAccel": 3.0, + "defaultMaxAngVel": 540.0, + "defaultMaxAngAccel": 720.0, + "maxModuleSpeed": 4.5 +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/autos/New Auto.auto b/src/main/deploy/pathplanner/autos/New Auto.auto new file mode 100644 index 0000000..ebd13e8 --- /dev/null +++ b/src/main/deploy/pathplanner/autos/New Auto.auto @@ -0,0 +1,19 @@ +{ + "version": 1.0, + "startingPose": null, + "command": { + "type": "sequential", + "data": { + "commands": [ + { + "type": "path", + "data": { + "pathName": "Gains Path" + } + } + ] + } + }, + "folder": null, + "choreoAuto": false +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/Gains Path.path b/src/main/deploy/pathplanner/paths/Gains Path.path new file mode 100644 index 0000000..e5daeda --- /dev/null +++ b/src/main/deploy/pathplanner/paths/Gains Path.path @@ -0,0 +1,81 @@ +{ + "version": 1.0, + "waypoints": [ + { + "anchor": { + "x": 2.053233435210415, + "y": 7.1495765710980645 + }, + "prevControl": null, + "nextControl": { + "x": 3.890081170868021, + "y": 7.389779736530214 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 6.066039257723953, + "y": 6.344189487002038 + }, + "prevControl": { + "x": 5.066039257723953, + "y": 7.344189487002038 + }, + "nextControl": { + "x": 7.066039257723953, + "y": 5.344189487002038 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 6.461668000788668, + "y": 3.1650299445177192 + }, + "prevControl": { + "x": 6.857296743853384, + "y": 4.450823359478042 + }, + "nextControl": { + "x": 6.237127153147944, + "y": 2.4352721896853664 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 7.15401830115192, + "y": 0.9325534657953997 + }, + "prevControl": { + "x": 5.61389212279285, + "y": 0.9466830637619968 + }, + "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/RobocketsController.java b/src/main/java/frc/robot/RobocketsController.java index 133a1eb..deb0570 100644 --- a/src/main/java/frc/robot/RobocketsController.java +++ b/src/main/java/frc/robot/RobocketsController.java @@ -80,39 +80,39 @@ public void teleopPeriodic() { } } // Intake - if (map.intake != null) { - map.intake.rotate(deadzone(getRightX(), 0.1)); - - if (getLeftBumperPressed()) { - map.intake.intake(); - } - else if (getRightBumperPressed()) { - map.intake.outtake(); - } - } + // if (map.intake != null) { + // map.intake.rotate(deadzone(getRightX(), 0.1)); + + // if (getLeftBumperPressed()) { + // map.intake.intake(); + // } + // else if (getRightBumperPressed()) { + // map.intake.outtake(); + // } + // } // Shooter - if (map.shooter != null) { - if (getAButtonPressed()) { - //CommandScheduler.getInstance().schedule(new Shoot(SmartDashboard.getNumber("Shooter Speed", 0.5))); - map.shooter.setSpeed(SmartDashboard.getNumber("Shooter In Speed", 0.5)); - } - if (getBButtonPressed()) { - //CommandScheduler.getInstance().schedule(new Shoot(-SmartDashboard.getNumber("Shooter Speed", 0.5))); - map.shooter.setSpeed(-SmartDashboard.getNumber("Shooter Out Speed", 0.5)); - } - if (getYButtonPressed()) { - map.shooter.setIntakeSpeed(SmartDashboard.getNumber("Shooter Intake Speed", 0.5)); - } - if (getXButtonPressed()) { - map.shooter.setIntakeSpeed(-SmartDashboard.getNumber("Shooter Outtake Speed", 0.5)); - } - if (getAButtonReleased() || getBButtonReleased()) { - map.shooter.setSpeed(0); - } - if (getXButtonReleased() || getYButtonReleased()) { - map.shooter.setIntakeSpeed(0); - } - } + // if (map.shooter != null) { + // if (getAButtonPressed()) { + // //CommandScheduler.getInstance().schedule(new Shoot(SmartDashboard.getNumber("Shooter Speed", 0.5))); + // map.shooter.setSpeed(SmartDashboard.getNumber("Shooter In Speed", 0.5)); + // } + // if (getBButtonPressed()) { + // //CommandScheduler.getInstance().schedule(new Shoot(-SmartDashboard.getNumber("Shooter Speed", 0.5))); + // map.shooter.setSpeed(-SmartDashboard.getNumber("Shooter Out Speed", 0.5)); + // } + // if (getYButtonPressed()) { + // map.shooter.setIntakeSpeed(SmartDashboard.getNumber("Shooter Intake Speed", 0.5)); + // } + // if (getXButtonPressed()) { + // map.shooter.setIntakeSpeed(-SmartDashboard.getNumber("Shooter Outtake Speed", 0.5)); + // } + // if (getAButtonReleased() || getBButtonReleased()) { + // map.shooter.setSpeed(0); + // } + // if (getXButtonReleased() || getYButtonReleased()) { + // map.shooter.setIntakeSpeed(0); + // } + // } // Vision if (map.vision != null) { if(getAButtonPressed()){ @@ -149,8 +149,8 @@ else if (getRightBumperPressed()) { // West Coast - if (map.westcoast != null) { - map.westcoast.arcadeDrive(getLeftY(), getRightX()); - } + // if (map.westcoast != null) { + // map.westcoast.arcadeDrive(getLeftY(), getRightX()); + // } } } diff --git a/src/main/java/frc/robot/RobocketsShuffleboard.java b/src/main/java/frc/robot/RobocketsShuffleboard.java index 0ba7b8e..9750ab3 100644 --- a/src/main/java/frc/robot/RobocketsShuffleboard.java +++ b/src/main/java/frc/robot/RobocketsShuffleboard.java @@ -9,6 +9,7 @@ public static void teleopInit() { addNumber("Shooter Out Speed", 0.5); addNumber("Shooter Intake Speed", 0.5); addNumber("Shooter Outtake Speed", 0.5); + } diff --git a/src/main/java/frc/robot/Robot.java b/src/main/java/frc/robot/Robot.java index 92cc871..f8c4a9b 100644 --- a/src/main/java/frc/robot/Robot.java +++ b/src/main/java/frc/robot/Robot.java @@ -23,7 +23,7 @@ public class Robot extends TimedRobot { // This is also the file where all the subsystems reside private static RobotMap map = new RobotMap(); public static RobotMap getMap() { return map; } - public static RobocketsController controller = new RobocketsController(Constants.CONTROLLER_PORT, map); + // public static RobocketsController controller = new RobocketsController(Constants.CONTROLLER_PORT, map); /** * This function is run when the robot is first started up and should be used for any @@ -31,7 +31,7 @@ public class Robot extends TimedRobot { */ @Override public void robotInit() { - AutoConstruct.sendAutoOptionsToSmartDashboard(); + // AutoConstruct.sendAutoOptionsToSmartDashboard(); } /** @@ -56,14 +56,15 @@ public void robotPeriodic() {} */ @Override public void autonomousInit() { - // CommandScheduler.getInstance().schedule(new SwerveGoCartesianF(map.swerve, new Translation2d(0, 6))); AutoConstruct.scheduleSelectedCommand(map); } /** This function is called periodically during autonomous. */ @Override public void autonomousPeriodic() { - CommandScheduler.getInstance().run(); + + + //CommandScheduler.getInstance().run(); //Gian: I'm not so sure why we would ever use this if all the auto code is done in the commandscheduler @@ -88,15 +89,9 @@ public void teleopInit() { /** This function is called periodically during operator control. */ @Override public void teleopPeriodic() { - controller.teleopPeriodic(); - if (map.vision != null) { - var result = map.vision.getLatestResult(); - - if(result.hasTargets()){ - System.out.println(result.getBestTarget()); - } - } + // controller.teleopPeriodic(); // Run any commands + map.vision.getPoseFromTag(); CommandScheduler.getInstance().run(); } diff --git a/src/main/java/frc/robot/RobotMap.java b/src/main/java/frc/robot/RobotMap.java index f5602c9..8f7e098 100644 --- a/src/main/java/frc/robot/RobotMap.java +++ b/src/main/java/frc/robot/RobotMap.java @@ -7,7 +7,7 @@ import frc.robot.subsystems.swerve.SwerveDriveSubsystem; import frc.robot.subsystems.swerve.SwerveModuleNeo; import frc.robot.subsystems.vision.VisionSubsystem; -import frc.robot.subsystems.westcoast.WestCoastSubsystem; +// import frc.robot.subsystems.westcoast.WestCoastSubsystem; // Contains all physical items on the robot (motors, encoders, LEDs, etc) public class RobotMap @@ -32,8 +32,8 @@ public class RobotMap public RobotMap() { // intake = new IntakeSubsystem(); + vision = new VisionSubsystem(); swerve = new SwerveDriveSubsystem(new Translation2d(-0.31115, 0.31115), new Translation2d(0.31115, 0.31115), new Translation2d(-0.31115, -0.31115), new Translation2d(0.31115, -0.31115)); // All translations are the swerve module positions relative to the center of the bot - // vision = new VisionSubsystem(); //shooter = new ShooterSubsystem(); // leds=new LedSubsystem(); diff --git a/src/main/java/frc/robot/field/Field.java b/src/main/java/frc/robot/field/Field.java index 1446636..dab4f3e 100644 --- a/src/main/java/frc/robot/field/Field.java +++ b/src/main/java/frc/robot/field/Field.java @@ -12,19 +12,19 @@ public class Field { //------- // Speaker //------- - public static final Speaker SPEAKER = new Speaker(SPEAKER_TOP_LEFT, SPEAKER_BOTTOM_RIGHT); - public static final Subwoofer SUBWOOFER = new Subwoofer(SUBWOOFER_TOP_LEFT, SUBWOOFER_BOTTOM_RIGHT); + // public static final Speaker SPEAKER = new Speaker(SPEAKER_TOP_LEFT, SPEAKER_BOTTOM_RIGHT); + // public static final Subwoofer SUBWOOFER = new Subwoofer(SUBWOOFER_TOP_LEFT, SUBWOOFER_BOTTOM_RIGHT); //------- // Stage //------- - public static final Stage STAGE = - new Stage( - LEFT_STAGE_POST_TOP_LEFT, TOP_STAGE_POST_CENTER, BOTTOM_STAGE_POST_CENTER, - RIGHT_TRAP_POINT1, RIGHT_TRAP_POINT2, - TOP_TRAP_POINT1, TOP_TRAP_POINT2, - BOTTOM_TRAP_POINT1, BOTTOM_TRAP_POINT2 - ); + // public static final Stage STAGE = + // new Stage( + // LEFT_STAGE_POST_TOP_LEFT, TOP_STAGE_POST_CENTER, BOTTOM_STAGE_POST_CENTER, + // RIGHT_TRAP_POINT1, RIGHT_TRAP_POINT2, + // TOP_TRAP_POINT1, TOP_TRAP_POINT2, + // BOTTOM_TRAP_POINT1, BOTTOM_TRAP_POINT2 + // ); //------- // Notes diff --git a/src/main/java/frc/robot/field/TagPositions.java b/src/main/java/frc/robot/field/TagPositions.java index 3dfeb08..e7d2641 100644 --- a/src/main/java/frc/robot/field/TagPositions.java +++ b/src/main/java/frc/robot/field/TagPositions.java @@ -20,4 +20,4 @@ public static AprilTagFieldLayout getAprilTagFieldLayout(){ public static Optional getTagPose3d(int ID){ return getAprilTagFieldLayout().getTagPose(ID); } -} +} \ No newline at end of file diff --git a/src/main/java/frc/robot/subsystems/shooter/Shoot.java b/src/main/java/frc/robot/subsystems/shooter/Shoot.java index da31a61..d57b9ff 100644 --- a/src/main/java/frc/robot/subsystems/shooter/Shoot.java +++ b/src/main/java/frc/robot/subsystems/shooter/Shoot.java @@ -18,7 +18,7 @@ public void initialize() {} @Override public void execute() { - Robot.getMap().shooter.setSpeed(speed); + // Robot.getMap().shooter.setSpeed(speed); } @Override @@ -30,6 +30,6 @@ public boolean isFinished() { @Override public void end(boolean interrupted) { - Robot.getMap().shooter.setSpeed(0); + // Robot.getMap().shooter.setSpeed(0); } } diff --git a/src/main/java/frc/robot/subsystems/vision/VisionSubsystem.java b/src/main/java/frc/robot/subsystems/vision/VisionSubsystem.java index 2af37f6..8cad6c4 100644 --- a/src/main/java/frc/robot/subsystems/vision/VisionSubsystem.java +++ b/src/main/java/frc/robot/subsystems/vision/VisionSubsystem.java @@ -3,17 +3,29 @@ // the WPILib BSD license file in the root directory of this project. package frc.robot.subsystems.vision; +import edu.wpi.first.math.geometry.Pose3d; +import edu.wpi.first.math.geometry.Rotation3d; +import edu.wpi.first.math.geometry.Transform3d; import edu.wpi.first.apriltag.AprilTagFieldLayout; import edu.wpi.first.apriltag.AprilTagFields; import edu.wpi.first.math.geometry.Pose3d; import edu.wpi.first.math.geometry.Translation2d; +import edu.wpi.first.math.geometry.Translation3d; +import edu.wpi.first.math.geometry.proto.Transform3dProto; import edu.wpi.first.math.util.Units; +import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import edu.wpi.first.wpilibj2.command.SubsystemBase; +import frc.robot.RobocketsShuffleboard; +import frc.robot.field.TagPositions; import java.util.List; +import java.util.Optional; +import org.photonvision.EstimatedRobotPose; import org.photonvision.PhotonCamera; +import org.photonvision.PhotonPoseEstimator; import org.photonvision.PhotonUtils; +import org.photonvision.PhotonPoseEstimator.PoseStrategy; import org.photonvision.targeting.PhotonPipelineResult; import org.photonvision.targeting.PhotonTrackedTarget; @@ -21,13 +33,12 @@ public class VisionSubsystem extends SubsystemBase { //**ALL ANGLES IN RADIANS ALL DISTANCES IN METERS**// PhotonCamera mCamera; - // Initialization has been moved to RobotMap - //private static final VisionSubsystem INSTANCE = new VisionSubsystem(); - //public static VisionSubsystem getInstance(){ - // return INSTANCE; - //} - double mCameraHeight = .19; - double mCameraPitch = Units.degreesToRadians(30); + + private Translation3d cameraTranslate = new Translation3d(.3915,0.0,0.35); // Meters + private Rotation3d camerRotation = new Rotation3d(0.0, Math.PI / 12, 0.0); // Radians + private Transform3d robotToCameraTransform = new Transform3d(cameraTranslate, camerRotation); + Pose3d cameraPose3d = new Pose3d(cameraTranslate, camerRotation); + private PhotonPoseEstimator photonEstimator = new PhotonPoseEstimator(TagPositions.getAprilTagFieldLayout(), PoseStrategy.AVERAGE_BEST_TARGETS, mCamera, robotToCameraTransform); private boolean mDriverMode = false; private AprilTagFieldLayout tagFieldLayout; @@ -35,12 +46,23 @@ public class VisionSubsystem extends SubsystemBase { public VisionSubsystem(){ //Replace with name of cam mCamera = new PhotonCamera("Camera"); - tagFieldLayout = AprilTagFields.k2024Crescendo.loadAprilTagLayoutField(); } @Override public void periodic() { - + // var result = mCamera.getLatestResult(); + // if(result.hasTargets()){ + // Optional estimatedRobotPose = photonEstimator.update(result); + // if(estimatedRobotPose.isPresent()){ + // EstimatedRobotPose e = estimatedRobotPose.get(); + // // RobocketsShuffleboard.addNumber("Pose X", e.estimatedPose.getX()); + // // RobocketsShuffleboard.addNumber("Pose Y", e.estimatedPose.getY()); + // } + // } + Pose3d robotPose = getPoseFromTag(); + // SmartDashboard.putNumber("X", robotPose.getX()); + // SmartDashboard.putNumber("Y", robotPose.getY()); + // SmartDashboard.putNumber("Z", robotPose.getZ()); } public Pose3d getTagPose(int id) { @@ -58,23 +80,37 @@ public int getBestTagID(){ public boolean hasTargets(){ return mCamera.getLatestResult().hasTargets(); } - public Translation2d getTransDiff(double targetHeight){ - if(hasTargets()){ - double pitch = Units.degreesToRadians(mCamera.getLatestResult().getBestTarget().getPitch()); - double distance = PhotonUtils.calculateDistanceToTargetMeters( - mCameraHeight, - targetHeight, - mCameraPitch, - pitch); - double yaw = Units.degreesToRadians(mCamera.getLatestResult().getBestTarget().getYaw()); - double yDiff = distance * Math.cos(pitch); - double xDiff = yDiff / Math.cos(yaw) * Math.sin(yaw); - return new Translation2d(xDiff, yDiff - 1); - } - else{ - return new Translation2d(); - } + + public Pose3d getPoseFromTag(){ + var result = getLatestResult(); + System.out.println("Got here"); + if(result.hasTargets()){ + //Gets ID and ambiguity (if tag is trustworthy) + int tag = result.getBestTarget().getFiducialId(); + // SmartDashboard.putNumber("April Tag ID", tag); + double ambiguity = result.getBestTarget().getPoseAmbiguity(); + //Get tag pose + Optional tagPoseopt = TagPositions.getTagPose3d(tag); + if(ambiguity < .2){ + //Gets tag pose and transfroms over camera + Pose3d tagPose = tagPoseopt.get(); + Transform3d cam2tag = result.getBestTarget().getBestCameraToTarget(); + //Cam pose on field + Pose3d camPose = tagPose.transformBy(cam2tag.inverse()); + //Robot pose on field + Pose3d robotPose = camPose.transformBy(robotToCameraTransform.inverse()); + return robotPose; + } + else { + // SmartDashboard.putNumber("April Tag ID", 5309); + } + } + else { + // SmartDashboard.putNumber("April Tag ID", 5309); + } + return new Pose3d(50, 10, 10000, new Rotation3d()); } + private static long l = 0L;