start

diff --git a/src/main/java/frc/robot/commands/gpm/FieldConstants.java b/src/main/java/frc/robot/commands/gpm/FieldConstants.java
new file mode 100644 (file)
index 0000000..514e810
--- /dev/null
+++ b/src/main/java/frc/robot/commands/gpm/FieldConstants.java
@@ -0,0 +1,5 @@
+package frc.robot.commands.gpm;
+
+public class FieldConstants {
+
+}

diff --git a/src/main/java/frc/robot/commands/gpm/TurretAutoShoot.java b/src/main/java/frc/robot/commands/gpm/TurretAutoShoot.java
new file mode 100644 (file)
index 0000000..dff06bd
--- /dev/null
+++ b/src/main/java/frc/robot/commands/gpm/TurretAutoShoot.java
@@ -0,0 +1,74 @@
+package frc.robot.commands.gpm;
+
+import edu.wpi.first.math.geometry.Translation2d;
+import edu.wpi.first.math.util.Units;
+import edu.wpi.first.wpilibj.DriverStation.Alliance;
+import edu.wpi.first.wpilibj2.command.Command;
+import frc.robot.Robot;
+import frc.robot.constants.FieldConstants;
+import frc.robot.subsystems.drivetrain.Drivetrain;
+import frc.robot.subsystems.turret.Turret;
+import frc.robot.util.Vision.TurretVision;
+
+public class TurretAutoShoot extends Command {
+    private Turret turret;
+    private Drivetrain drivetrain;
+    private TurretVision turretVision;
+
+    double turretSetpoint;
+    double yawToTagCamera;
+    double yawToTag;
+
+    public TurretAutoShoot(Turret turret, Drivetrain drivetrain, TurretVision turretVision){
+        this.turret = turret;
+        this.drivetrain = drivetrain;
+        this.turretVision = turretVision;
+    }
+
+    public void align() {
+        Translation2d drivepose = drivetrain.getPose().getTranslation();
+        Translation2d target = FieldConstants.HUB_TRANSLATION3D.toTranslation2d();
+        double D_y = target.getY() - drivepose.getY();
+        double D_x = target.getX() - drivepose.getX();
+        double angleRad = Math.atan2(D_y, D_x);
+        System.out.println("Aligning the turn to degree angle: " + Units.radiansToDegrees(angleRad));
+        turretSetpoint = Units.radiansToDegrees(angleRad);
+    }
+
+    public void adjustWithTurretCam(){
+        if(Robot.getAlliance() == Alliance.Blue){
+            yawToTagCamera = turretVision.getYawToTagRad(26).get();
+        }
+        else{
+            yawToTagCamera = turretVision.getYawToTagRad(10).get();
+        }
+        
+
+    }
+
+    public void updateYawToTag(){
+        double D_y = FieldConstants.HUB_TRANSLATION3D.getY() - drivetrain.getPose().getY();
+        double D_x = FieldConstants.HUB_TRANSLATION3D.getX() - drivetrain.getPose().getX();
+        double angleToTag = Units.radiansToDegrees(Math.atan())
+    }
+
+    @Override
+    public void initialize() {
+        // TODO Auto-generated method stub
+        super.initialize();
+    }
+
+    @Override
+    public void execute() {
+        // TODO Auto-generated method stub
+        super.execute();
+    }
+
+    @Override
+    public void end(boolean interrupted) {
+        // TODO Auto-generated method stub
+        super.end(interrupted);
+    }
+
+    
+}

diff --git a/src/main/java/frc/robot/subsystems/turret/Turret.java b/src/main/java/frc/robot/subsystems/turret/Turret.java
index 332d18f30805afcb08d77ef9226d17d6835a27c1..b089e16c077e83f9f3038ce225776c5c967a622c 100644 (file)
--- a/src/main/java/frc/robot/subsystems/turret/Turret.java
+++ b/src/main/java/frc/robot/subsystems/turret/Turret.java
@@ -177,27 +177,10 @@ public class Turret extends SubsystemBase {
         }
     }
 
-    public void align() {
-        Translation2d drivepose = drivetrain.getPose().getTranslation();
-        // Using tag #??
-        int tagNumber = 17;
-        Translation2d tagpose = FieldConstants.field.getTagPose(tagNumber ).get().toPose2d().getTranslation();
-        double D_y = tagpose.getY() - drivepose.getY();
-        double D_x = tagpose.getX() - drivepose.getX();
-        double angleRad = Math.atan2(D_y, D_x);
-        System.out.println("Aligning the turn to degree angle: " + Units.radiansToDegrees(angleRad));
-        setSetpoint(Units.radiansToDegrees(angleRad));
-    }
-
     @Override
     public void periodic() {
         position = Units.rotationsToDegrees(motor.getPosition().getValueAsDouble()) / gearRatio; // Gear Ratio
         velocity = Units.rotationsPerMinuteToRadiansPerSecond(motor.getVelocity().getValueAsDouble() * 60);
-
-        // SmartDashboard.putNumber("Turret Position", position);
-        if (alignOn) {
-            align();
-        }
         
         ligament2d.setAngle(position);
     }

diff --git a/src/main/java/frc/robot/util/Vision/TurretVision.java b/src/main/java/frc/robot/util/Vision/TurretVision.java
new file mode 100644 (file)
index 0000000..6ed4648
--- /dev/null
+++ b/src/main/java/frc/robot/util/Vision/TurretVision.java
@@ -0,0 +1,52 @@
+package frc.robot.util.Vision;
+
+import java.util.Optional;
+
+import org.photonvision.PhotonCamera;
+import org.photonvision.targeting.PhotonPipelineResult;
+import org.photonvision.targeting.PhotonTrackedTarget;
+
+import edu.wpi.first.math.util.Units;
+
+public class TurretVision {
+
+    private final PhotonCamera camera;
+
+    public TurretVision(String cameraName) {
+        camera = new PhotonCamera(cameraName);
+    }
+
+  /**
+   * @param tagId AprilTag ID to aim at
+   * @return Optional yaw error in radians (positive = target to the right, negative = target to the left)
+   */
+    public Optional<Double> getYawToTagRad(int tagId) {
+        PhotonPipelineResult result = camera.getLatestResult();
+
+        if (!result.hasTargets()) {
+        return Optional.empty();
+        }
+
+        for (PhotonTrackedTarget target : result.getTargets()) {
+        if (target.getFiducialId() == tagId) {
+            // PhotonVision yaw is in DEGREES
+            double yawRad = Units.degreesToRadians(target.getYaw());
+            return Optional.of(yawRad);
+        }
+        }
+
+        return Optional.empty();
+    }
+
+    public boolean canSeeTag(int tagId) {
+        PhotonPipelineResult result = camera.getLatestResult();
+        if (!result.hasTargets()) return false;
+
+        for (PhotonTrackedTarget target : result.getTargets()) {
+        if (target.getFiducialId() == tagId) {
+            return true;
+        }
+        }
+        return false;
+    }
+}