Fixed Auto Defense method; tuned radians vs degrees.

lasarobotics · Jul 12, 2024 · fd10a23 · fd10a23
1 parent d39d806
commit fd10a23
Show file tree

Hide file tree

Showing 2 changed files with 9 additions and 7 deletions.
diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java
@@ -170,11 +170,11 @@ private void configureBindings() {
     PRIMARY_CONTROLLER.povRight().whileTrue(feedThroughCommand());
 
     // DPAD down - auto defense
-    // PRIMARY_CONTROLLER.povDown().whileTrue(DRIVE_SUBSYSTEM.autoDefenseCommand(
-    //     () -> PRIMARY_CONTROLLER.getLeftY(),
-    //     () -> PRIMARY_CONTROLLER.getLeftX(),
-    //     () -> PRIMARY_CONTROLLER.getRightX()
-    // ));
+    PRIMARY_CONTROLLER.povDown().whileTrue(DRIVE_SUBSYSTEM.autoDefenseCommand(
+        () -> PRIMARY_CONTROLLER.getLeftY(),
+        () -> PRIMARY_CONTROLLER.getLeftX(),
+        () -> PRIMARY_CONTROLLER.getRightX()
+    ));
   }
 
   /**

diff --git a/src/main/java/frc/robot/subsystems/drive/DriveSubsystem.java b/src/main/java/frc/robot/subsystems/drive/DriveSubsystem.java
@@ -677,16 +677,18 @@ private void autoDefense(double xRequest, double yRequest, double rotateRequest)
     double rotateOutput = -m_rotatePIDController.calculate(getAngle(), getRotateRate(), rotateRequest);
 
     if (objectYaw.isEmpty()) {
+      System.out.println(rotateOutput);
       drive(
         m_controlCentricity,
         Units.MetersPerSecond.of(-velocityOutput * Math.cos(moveDirection)),
         Units.MetersPerSecond.of(-velocityOutput * Math.sin(moveDirection)),
-        Units.RadiansPerSecond.of(rotateOutput),
+        Units.DegreesPerSecond.of(rotateOutput),
         getInertialVelocity()
       );
       return;
     }
 
+    System.out.println("object yaw is not empy");
     moveRequest = Math.hypot(xRequest, 0.0);
     moveDirection = Math.atan2(0.0, xRequest);
     velocityOutput = m_throttleMap.throttleLookup(moveRequest);
@@ -695,7 +697,7 @@ private void autoDefense(double xRequest, double yRequest, double rotateRequest)
       ControlCentricity.ROBOT_CENTRIC,
       Units.MetersPerSecond.of(velocityOutput),
       DRIVE_MAX_LINEAR_SPEED.times(objectYaw.get().in(Units.Degrees)/Constants.VisionHardware.CAMERA_OBJECT_FOV.getDegrees()),
-      Units.RadiansPerSecond.of(rotateOutput),
+      Units.DegreesPerSecond.of(rotateOutput),
       getInertialVelocity()
     );
   }