working aim

src/main/java/frc/robot/BuildConstants.java

@@ -5,12 +5,12 @@ public final class BuildConstants {
     public static final String MAVEN_GROUP = "";
     public static final String MAVEN_NAME = "Flash-2023";
     public static final String VERSION = "unspecified";
-    public static final int GIT_REVISION = 75;
-    public static final String GIT_SHA = "c455d8c1a08e1b79d4880ae28171f0b18322da27";
-    public static final String GIT_DATE = "2022-11-19 17:41:56 CST";
+    public static final int GIT_REVISION = 86;
+    public static final String GIT_SHA = "137bbd36eacb2d7b03e046fe545b387c99891c73";
+    public static final String GIT_DATE = "2022-11-20 13:54:34 CST";
     public static final String GIT_BRANCH = "Jonathan";
-    public static final String BUILD_DATE = "2022-11-19 18:46:28 CST";
-    public static final long BUILD_UNIX_TIME = 1668905188370L;
+    public static final String BUILD_DATE = "2022-11-20 15:16:09 CST";
+    public static final long BUILD_UNIX_TIME = 1668978969849L;
     public static final int DIRTY = 1;
 
     private BuildConstants() {}

src/main/java/frc/robot/pilot/PilotGamepad.java

@@ -1,6 +1,5 @@
 package frc.robot.pilot;
 
-import edu.wpi.first.math.util.Units;
 import edu.wpi.first.wpilibj.util.Color;
 import edu.wpi.first.wpilibj2.command.button.Trigger;
 import frc.SpectrumLib.gamepads.AxisButton;
@@ -14,9 +13,8 @@
 import frc.robot.leds.commands.SnowfallLEDCommand;
 import frc.robot.pilot.commands.PilotCommands;
 import frc.robot.pilot.commands.SpinMove;
-import frc.robot.swerve.commands.SwerveCommands;
 import frc.robot.pose.commands.PoseCommands;
-import frc.robot.swerve.commands.LockSwerve;
+import frc.robot.vision.VisionCommands;
 
 /** Used to add buttons to the pilot gamepad and configure the joysticks */
 public class PilotGamepad extends Gamepad {
@@ -39,15 +37,14 @@ public PilotGamepad() {
     }
 
     public void setupTeleopButtons() {
-        // gamepad.aButton.whileTrue(PilotCommands.aimPilotDrive(Math.PI * 1 / 2).withName("Snap
-        // 90"));
-        gamepad.aButton.whileTrue(PilotCommands.aimPilotDrive(0).withName("Aim to 0"));
+        gamepad.aButton.whileTrue(PilotCommands.aimPilotDrive(Math.PI * 1 / 2).withName("Snap 90"));
         // gamepad.bButton.whileTrue(PilotCommands.fpvPilotSwerve());
         gamepad.bButton.whileTrue(
                 PilotCommands.aimPilotDrive(() -> Robot.vision.getRadiansToTarget())
                         .withName("Aim to target"));
         // gamepad.xButton.whileTrue(new LockSwerve());
-        gamepad.xButton.whileTrue(PilotCommands.aimPilotDrive(Units.degreesToRadians(90)));
+        //get information about target and robot yaw 
+        gamepad.xButton.whileTrue(VisionCommands.printYawInfo());
         gamepad.yButton.whileTrue(new SpinMove());
 
         // Right Stick points the robot in that direction

src/main/java/frc/robot/swerve/SwerveModule.java

@@ -56,11 +56,12 @@ public SwerveModule(
         lastAngle = getState().angle;
     }
 
-    /** Set motors to desired speed using state
-     * 
-     *  @param desiredState
-     *  @param isOpenLoop - if true, use percent output for motors, if false, use velocity
-     *  **/
+    /**
+     * Set motors to desired speed using state
+     *
+     * @param desiredState
+     * @param isOpenLoop - if true, use percent output for motors, if false, use velocity *
+     */
     public void setDesiredState(SwerveModuleState desiredState, boolean isOpenLoop) {
         // Custom optimize command, since default WPILib optimize assumes continuous controller
         // which CTRE is not

src/main/java/frc/robot/vision/Vision.java

@@ -5,7 +5,6 @@
 import edu.wpi.first.wpilibj2.command.SubsystemBase;
 import frc.robot.Robot;
 import frc.robot.RobotTelemetry;
-
 import java.text.DecimalFormat;
 import org.photonvision.PhotonCamera;
 import org.photonvision.targeting.PhotonPipelineResult;
@@ -21,7 +20,6 @@ public class Vision extends SubsystemBase {
 
     // testing
     private DecimalFormat df = new DecimalFormat();
-    private double lastCaptureTime = 0;
     private double lastYaw = 0;
     private boolean targetFound = true;
 
@@ -40,16 +38,17 @@ public void periodic() {
         PhotonPipelineResult results = camera.getLatestResult();
         if (results.hasTargets()) {
             PhotonTrackedTarget target = results.getBestTarget();
-            yaw = target.getYaw();
+            // negate it because the target.getYaw is the yaw of the robot from the target which is
+            // the opposite direction. Or photonvision yaw is CW+ CCW-
+            yaw = -target.getYaw();
             pitch = target.getPitch();
             area = target.getArea();
             targetId = target.getFiducialId();
             poseAmbiguity = target.getPoseAmbiguity();
             captureTime = Timer.getFPGATimestamp() - (results.getLatencyMillis() / 1000d);
 
-            if (lastCaptureTime == 0) {
-                lastCaptureTime = captureTime;
-            }
+
+            //printing 
             if (lastYaw == 0) {
                 lastYaw = yaw;
             }
@@ -58,10 +57,10 @@ public void periodic() {
                 // printDebug(targetId, yaw, pitch, area, poseAmbiguity, captureTime);
             }
 
-            lastCaptureTime = captureTime;
             lastYaw = yaw;
             targetFound = true;
         } else {
+            //no target found
             yaw = 0.0;
             if (targetFound) {
                 RobotTelemetry.print("Lost target");
@@ -71,8 +70,14 @@ public void periodic() {
     }
 
     public double getRadiansToTarget() {
-        RobotTelemetry.print("Yaw (D): " + yaw + "|| gyro (D): " + Robot.swerve.getHeading().getDegrees());
-        return Units.degreesToRadians(yaw) + Robot.swerve.getHeading().getDegrees();
+        RobotTelemetry.print(
+                "Yaw (D): "
+                        + yaw
+                        + "|| gyro (D): "
+                        + Robot.swerve.getHeading().getDegrees()
+                        + " || Aiming at: "
+                        + (yaw + Robot.swerve.getHeading().getDegrees()));
+        return Units.degreesToRadians(yaw) + Robot.swerve.getHeading().getRadians();
     }
 
     private void printDebug(
@@ -96,4 +101,8 @@ private void printDebug(
                         + " | Capture Time: "
                         + df.format(captureTime));
     }
+
+    public double getYaw() {
+        return yaw;
+    }
 }

src/main/java/frc/robot/vision/VisionCommands.java

@@ -1,3 +1,23 @@
 package frc.robot.vision;
 
-public class VisionCommands {}
+import edu.wpi.first.wpilibj2.command.Command;
+import edu.wpi.first.wpilibj2.command.InstantCommand;
+import frc.robot.Robot;
+import frc.robot.RobotTelemetry;
+
+public class VisionCommands {
+
+    public static Command printYawInfo() {
+        return new InstantCommand(
+                () -> {
+                    RobotTelemetry.print(
+                            "Yaw (D): "
+                                    + Robot.vision.getYaw()
+                                    + "|| gyro (D): "
+                                    + Robot.swerve.getHeading().getDegrees()
+                                    + " || Aiming at: "
+                                    + (Robot.vision.getYaw() + Robot.swerve.getHeading().getDegrees()));
+                },
+                Robot.vision);
+    }
+}

src/main/java/frc/robot/vision/VisionConfig.java

@@ -10,7 +10,7 @@ public final class VisionConfig {
             Units.degreesToRadians(26.138); // angle from looking straight forward
 
     // testing
-    
+
     private final double TARGET_HEIGHT_METERS = Units.inchesToMeters(36.2); // 36.2 inches
 
     /* Location of camera relative to robot center */