Fully automatic aiming system at target!

frc3512 · Feb 10, 2024 · d8ac87b · d8ac87b
1 parent 3fdfb59
commit d8ac87b
Show file tree

Hide file tree

Showing 3 changed files with 8 additions and 9 deletions.
diff --git a/src/main/deploy/swerve/comp/swervedrive.json b/src/main/deploy/swerve/comp/swervedrive.json
@@ -4,7 +4,7 @@
     "id": 0,
     "canbus": null
   },
-  "invertedIMU": true,
+  "invertedIMU": false,
   "modules": [
     "frontleft.json",
     "frontright.json",

diff --git a/src/main/java/frc3512/robot/Constants.java b/src/main/java/frc3512/robot/Constants.java
@@ -30,7 +30,7 @@ public static final class GeneralConstants {
   }
 
   public static final class VisionConstants {
-    public static final String cameraName = "jonny";
+    public static final String cameraName = "USB_GS_Camera";
     public static final Transform3d robotToCam = new Transform3d();
   }
 }
diff --git a/src/main/java/frc3512/robot/subsystems/Swerve.java b/src/main/java/frc3512/robot/subsystems/Swerve.java
@@ -1,14 +1,14 @@
 package frc3512.robot.subsystems;
 
 import edu.wpi.first.math.MathUtil;
+import edu.wpi.first.math.controller.PIDController;
 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.math.kinematics.ChassisSpeeds;
 import edu.wpi.first.math.kinematics.SwerveDriveKinematics;
 import edu.wpi.first.math.trajectory.Trajectory;
 import edu.wpi.first.wpilibj.Filesystem;
-import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
 import edu.wpi.first.wpilibj2.command.Command;
 import edu.wpi.first.wpilibj2.command.SubsystemBase;
 import frc3512.lib.logging.SpartanEntryManager;
@@ -75,21 +75,20 @@ public Command driveCommand(
       PhotonCamera camera) {
     return run(
         () -> {
-          SmartDashboard.putBoolean("Diagnostics/Vision/DoVisionAim", doAim.getAsBoolean());
           PhotonPipelineResult result = camera.getLatestResult();
+          final double ANGULAR_P = 0.1;
+          final double ANGULAR_D = 0.0;
+          PIDController turnController = new PIDController(ANGULAR_P, 0, ANGULAR_D);
           if (result.hasTargets() && doAim.getAsBoolean()) {
             drive(
-                getTargetSpeeds(
+                swerve.swerveController.getRawTargetSpeeds(
                     MathUtil.applyDeadband(
                         translationX.getAsDouble() * swerve.getMaximumVelocity(),
                         Constants.SwerveConstants.swerveDeadband),
                     MathUtil.applyDeadband(
                         translationY.getAsDouble() * swerve.getMaximumVelocity(),
                         Constants.SwerveConstants.swerveDeadband),
-                    Rotation2d.fromDegrees(
-                        result
-                            .getBestTarget()
-                            .getYaw()))); // Not sure if this will work, more math may be required.
+                    -turnController.calculate(result.getBestTarget().getYaw(), 0)));
           } else {
             swerve.drive(
                 new Translation2d(