Make Vision Work

src/main/java/frc4388/robot/Constants.java

@@ -10,6 +10,10 @@ package frc4388.robot;
 import static edu.wpi.first.units.Units.Inches;
 import static edu.wpi.first.units.Units.Rotations;
 
+import java.util.ArrayList;
+import java.util.Arrays;
+import java.util.List;
+
 import com.ctre.phoenix6.configs.CANcoderConfiguration;
 import com.ctre.phoenix6.configs.ClosedLoopRampsConfigs;
 import com.ctre.phoenix6.configs.CurrentLimitsConfigs;
@@ -27,10 +31,12 @@ import com.ctre.phoenix6.swerve.SwerveModuleConstants.SteerFeedbackType;
 import com.ctre.phoenix6.swerve.SwerveModuleConstants.SteerMotorArrangement;
 import com.ctre.phoenix6.swerve.SwerveModuleConstantsFactory;
 
+import edu.wpi.first.apriltag.AprilTag;
 import edu.wpi.first.apriltag.AprilTagFieldLayout;
 import edu.wpi.first.apriltag.AprilTagFields;
 import edu.wpi.first.math.Matrix;
 import edu.wpi.first.math.VecBuilder;
+import edu.wpi.first.math.geometry.Pose3d;
 import edu.wpi.first.math.geometry.Rotation2d;
 import edu.wpi.first.math.geometry.Rotation3d;
 import edu.wpi.first.math.geometry.Transform3d;
@@ -279,9 +285,17 @@ public final class Constants {
     public static final class VisionConstants { 
         public static final String CAMERA_NAME = "Camera_Module_v1";
 
-        public static final Transform3d CAMERA_POS = new Transform3d(new Translation3d(.3048, 0.2413, .2794), new Rotation3d(0,0.52333,Math.PI));
+        public static final Transform3d CAMERA_POS = new Transform3d(new Translation3d(-.3048, 0.2413*0, .2794), new Rotation3d(0,0.52333,Math.PI));
 
-        public static final AprilTagFieldLayout kTagLayout = AprilTagFields.k2024Crescendo.loadAprilTagLayoutField();
+        // public static final AprilTagFieldLayout kTagLayout = AprilTagFields.k2024Crescendo.loadAprilTagLayoutField();
+
+        
+        public static final AprilTagFieldLayout kTagLayout = new AprilTagFieldLayout(
+            Arrays.asList(new AprilTag[] {
+                new AprilTag(1, new Pose3d(
+                    new Translation3d(0.,0.,0.26035), new Rotation3d(0.,0.,0.)
+                )),
+            }), 100, 100);
 
         // The standard deviations of our vision estimated poses, which affect correction rate
         // (Fake values. Experiment and determine estimation noise on an actual robot.)

src/main/java/frc4388/robot/commands/GotoPositionCommand.java

@@ -1,5 +1,7 @@
 package frc4388.robot.commands;
 
+import edu.wpi.first.math.geometry.Pose2d;
+import edu.wpi.first.math.geometry.Rotation2d;
 import edu.wpi.first.math.geometry.Translation2d;
 import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
 import edu.wpi.first.wpilibj2.command.Command;
@@ -13,12 +15,16 @@ import frc4388.utility.controller.VirtualController;
 public class GotoPositionCommand extends Command {
     
 
-    private Translation2d translation2d= new Translation2d(0,0);
-    static Gains gains = new Gains(3,0,0);
+    // private Translation2d translation2d= new Translation2d(14.579471999999997,0.24587199999999998);
+    // private Translation2d translation2d= new Translation2d(16.579342-0.15,5.547867999999999);
+    private Pose2d targetpos = new Pose2d(new Translation2d(0.3,0), new Rotation2d());
+    static Gains xygains = new Gains(3,0,0);
+    static Gains rotgains = new Gains(0.1,0,0.0);
     static double tolerance = 0;
 
-    private PID xPID = new PID(gains, 0);
-    private PID yPID = new PID(gains, 0);
+    private PID xPID = new PID(xygains, 0);
+    private PID yPID = new PID(xygains, 0);
+    private PID rotPID = new PID(rotgains, 0);
 
     SwerveDrive swerveDrive;
     Vision vision;
@@ -42,24 +48,31 @@ public class GotoPositionCommand extends Command {
     
     @Override
     public void execute() {
-        double xerr = translation2d.getX() - vision.getPose2d().getX();
-        double yerr = translation2d.getY() - vision.getPose2d().getY();
+        double xerr = targetpos.getX() - vision.getPose2d().getX();
+        double yerr = targetpos.getY() - vision.getPose2d().getY();
+        double roterr = targetpos.getRotation().getDegrees() - vision.getPose2d().getRotation().getDegrees();
 
         SmartDashboard.putNumber("PID X Error", xerr);
         SmartDashboard.putNumber("PID Y Error", yerr);
 
         double xoutput = xPID.update(xerr);
         double youtput = yPID.update(yerr);
+        double rotoutput = rotPID.update(roterr);
 
         Translation2d leftStick = new Translation2d(
             Math.max(Math.min(youtput, 1), -1),
             Math.max(Math.min(xoutput, 1), -1)
+            // 0,0
         );
 
-        Translation2d rightStick = new Translation2d();
+        Translation2d rightStick = new Translation2d(
+            Math.max(Math.min(rotoutput, 1), -1), 
+           0
+        );
 
         SmartDashboard.putNumber("PID X Output", xoutput);
         SmartDashboard.putNumber("PID Y Output", youtput);
+        // SmartDashboard.putNumber("PID Y Output", youtput);
 
         swerveDrive.driveWithInput(leftStick, rightStick, true);
     }

src/main/java/frc4388/robot/subsystems/SwerveDrive.java

@@ -155,6 +155,23 @@ public class SwerveDrive extends Subsystem {
     return false;
   }
 
+  public void driveWithInputRotation(Translation2d leftStick, Rotation2d rot) {
+    // if (leftStick.getNorm() < 0.05 && stopped == false) // if no imput and the swerve drive is still going:
+    //   stopModules(); // stop the swerve
+    
+    // if (leftStick.getNorm() < 0.05) //if no imput
+    //   return; // don't bother doing swerve drive math and return early.
+
+    leftStick = leftStick.rotateBy(Rotation2d.fromDegrees(SwerveDriveConstants.FORWARD_OFFSET));
+    
+    swerveDriveTrain.setControl(new SwerveRequest.FieldCentricFacingAngle()
+      .withVelocityX(leftStick.getX()*-speedAdjust)
+      .withVelocityY(leftStick.getY()*speedAdjust)
+      .withTargetDirection(rot)
+    );
+      // double
+  }
+
   public double getGyroAngle() {
     return swerveDriveTrain.getRotation3d().getAngle();
   }

src/main/java/frc4388/robot/subsystems/Vision.java

@@ -108,16 +108,13 @@ public class Vision extends Subsystem {
         lastVisionPose = EstimatedRobotPose.get().estimatedPose.toPose2d();
         // lastVisionPose.rotateBy(lastVisionPose.getRotation().minus(lastPhysOdomPose.getRotation()));
         // lastVisionPose = new Pose2d(
-        //     lastVisionPose.getTranslation(), 
+        //     lastVisionPose.getTranslation(),
         //     lastPhysOdomPose.getRotation()
         // );
 
         
         
         field.setRobotPose(getPose2d());
-
-
-
     }