debug for auto aim test

Team334 · Jan 26, 2024 · a0e7b26 · a0e7b26
1 parent f30a09f
commit a0e7b26
Show file tree

Hide file tree

Showing 3 changed files with 17 additions and 6 deletions.
diff --git a/src/main/java/frc/robot/commands/shooter/AutoAim.java b/src/main/java/frc/robot/commands/shooter/AutoAim.java
@@ -62,24 +62,33 @@ public void execute() {
 
     double currentSwerveHeading = _swerve.getHeading().getDegrees();
 
-    if (_vision.isApriltagVisible()) {
-      desiredShooterAngle = _vision.anglesToSpeaker()[1]; // ty
-      desiredSwerveHeading = _vision.anglesToSpeaker()[0];
+    SmartDashboard.putBoolean("VISIBLE TAG", _vision.isApriltagVisible());
+
+    double[] visionAngles = _vision.anglesToSpeaker();
+
+    if (_vision.isApriltagVisible() && visionAngles != null) {
+      desiredShooterAngle = visionAngles[1];
+      desiredSwerveHeading = visionAngles[0];
+
+      SmartDashboard.putNumberArray("VISION ANGLES", visionAngles);
 
     } else {
       desiredShooterAngle = _swerve.anglesToSpeaker()[1];
       desiredSwerveHeading = _swerve.anglesToSpeaker()[0];
     }
 
-    desiredSwerveHeading += currentSwerveHeading;
+    desiredSwerveHeading -= currentSwerveHeading;
 
     _shooter.setAngle(desiredShooterAngle);
 
+    SmartDashboard.putNumber("DESIRED HEADING", desiredSwerveHeading);
+
     _swerve.driveChassis(
       new ChassisSpeeds(
         _xSpeed.getAsDouble() * Constants.Speeds.SWERVE_DRIVE_MAX_SPEED * Constants.Speeds.SWERVE_DRIVE_COEFF,
         _ySpeed.getAsDouble() * Constants.Speeds.SWERVE_DRIVE_MAX_SPEED * Constants.Speeds.SWERVE_DRIVE_COEFF,
-        _headingController.calculate(currentSwerveHeading, desiredSwerveHeading)
+        // _headingController.calculate(currentSwerveHeading, desiredSwerveHeading),
+        0
       )
     );
   }

diff --git a/src/main/java/frc/robot/subsystems/SwerveDriveSubsystem.java b/src/main/java/frc/robot/subsystems/SwerveDriveSubsystem.java
@@ -156,6 +156,8 @@ public ChassisSpeeds getRobotRelativeSpeeds() {
   public SwerveDriveSubsystem(VisionSubsystem visionSubsystem) {
     _visionSubsystem = visionSubsystem;
 
+    resetPose(_visionSubsystem.getBotpose().get());
+
     // setupOrchestra();
 
     // pathplannerlib setup

diff --git a/src/main/java/frc/robot/subsystems/VisionSubsystem.java b/src/main/java/frc/robot/subsystems/VisionSubsystem.java
@@ -100,7 +100,7 @@ public boolean isApriltagVisible() {
   public boolean isApriltagVisible(int ID) {
     if (!isApriltagVisible()) return false;
 
-    return _limelight.getTag(ID) != null;
+    return (_limelight.getTag(ID) != null);
   }
 
   /**