make centeringOnBall work in teleop when you turn on the intake

FRC3476 · Mar 24, 2022 · 1853a13 · 1853a13
1 parent b724632
commit 1853a13
Show file tree

Hide file tree

Showing 2 changed files with 23 additions and 12 deletions.
diff --git a/src/main/java/frc/robot/Robot.java b/src/main/java/frc/robot/Robot.java
@@ -20,6 +20,7 @@
 import frc.subsystem.*;
 import frc.subsystem.Climber.ClimbState;
 import frc.utility.*;
+import frc.utility.Controller.XboxAxes;
 import frc.utility.Controller.XboxButtons;
 import frc.utility.Limelight.LedMode;
 import frc.utility.Limelight.StreamingMode;
@@ -436,7 +437,6 @@ public void teleopInit() {
     }
 
     private final Object driverForcingVisionOn = new Object();
-    private final Object buttonPanelForcingVisionOn = new Object();
     private final Object resettingPoseVisionOn = new Object();
 
     /**
@@ -464,7 +464,9 @@ public void teleopPeriodic() {
             shooter.setFiring(false);
             shooter.setSpeed(0);
             visionManager.unForceVisionOn(driverForcingVisionOn);
-            if (climber.getClimbState() == ClimbState.IDLE || climber.isPaused()) { // If we're climbing don't allow the robot to be
+            if (xbox.getRawAxis(XboxAxes.LEFT_TRIGGER) > 0.1) {
+                drive.centerOntoBall(getControllerDriveInputs(), useFieldRelative);
+            } else if (climber.getClimbState() == ClimbState.IDLE || climber.isPaused()) { // If we're climbing don't allow the robot to be
                 // driven
                 doNormalDriving();
             } else {

diff --git a/src/main/java/frc/subsystem/Drive.java b/src/main/java/frc/subsystem/Drive.java
@@ -576,7 +576,7 @@ private void updateRamsete() {
 
     PIDController centerOntoBallPID = new PIDController(1, 0, 0, 0.02);
 
-    private void centerOntoBall(ControllerDriveInputs inputs) {
+    public void centerOntoBall(ControllerDriveInputs inputs, boolean fieldRelativeEnabled) {
         Limelight limelight = Limelight.getInstance("limelight-intake");
         if (limelight.isTargetVisible()) {
 
@@ -607,14 +607,23 @@ private void centerOntoBall(ControllerDriveInputs inputs) {
             }
 
             double strafeCommand = centerOntoBallPID.calculate(pidError, 0);
-            Translation2d correction = new Translation2d(strafeCommand, 0).rotateBy(
-                    RobotTracker.getInstance().getGyroAngle()); //Make it perpendicular to the robot
+            Translation2d correction = new Translation2d(0, strafeCommand);
+
+            ChassisSpeeds chassisSpeeds;
+            if (useFieldRelative && fieldRelativeEnabled) {
+                correction = correction.rotateBy(RobotTracker.getInstance().getGyroAngle()); //Make it perpendicular to the robot
+                chassisSpeeds = ChassisSpeeds.fromFieldRelativeSpeeds(
+                        DRIVE_HIGH_SPEED_M * inputs.getX() + correction.getX(),
+                        DRIVE_HIGH_SPEED_M * inputs.getY() + correction.getY(),
+                        inputs.getRotation() * 7,
+                        RobotTracker.getInstance().getGyroAngle());
+            } else {
+                chassisSpeeds = new ChassisSpeeds(
+                        DRIVE_HIGH_SPEED_M * inputs.getX() + correction.getX(),
+                        DRIVE_HIGH_SPEED_M * inputs.getY() + correction.getY(),
+                        inputs.getRotation() * 7);
+            }
 
-            ChassisSpeeds chassisSpeeds = ChassisSpeeds.fromFieldRelativeSpeeds(
-                    DRIVE_HIGH_SPEED_M * inputs.getX() + correction.getX(),
-                    DRIVE_HIGH_SPEED_M * inputs.getY() + correction.getY(),
-                    inputs.getRotation() * 7,
-                    RobotTracker.getInstance().getGyroAngle());
             swerveDrive(chassisSpeeds);
         } else {
             swerveDriveFieldRelative(inputs);
@@ -661,8 +670,8 @@ public void update() {
     private void searchForBall() {
         Limelight intakeLimelight = Limelight.getInstance("limelight-intake");
         if (intakeLimelight.isTargetVisible()) {
-            Translation2d movement = new Translation2d(0.25, 0).rotateBy(RobotTracker.getInstance().getGyroAngle());
-            centerOntoBall(new ControllerDriveInputs(movement.getX(), movement.getY(), 0));
+            Translation2d movement = new Translation2d(0.25, 0);
+            centerOntoBall(new ControllerDriveInputs(movement.getX(), movement.getY(), 0), false);
         } else {
             swerveDrive(new ChassisSpeeds(0, 0, 0));
         }