[#475] - improved chase pieces logic

frc-862 · Mar 27, 2024 · ac8dd11 · ac8dd11
1 parent 3489550
commit ac8dd11
Showing 1 changed file with 7 additions and 4 deletions.
diff --git a/src/main/java/frc/robot/command/ChasePieces.java b/src/main/java/frc/robot/command/ChasePieces.java
@@ -3,6 +3,7 @@
 import java.sql.Driver;
 
 import edu.wpi.first.math.controller.PIDController;
+import edu.wpi.first.math.geometry.Pose2d;
 import edu.wpi.first.util.datalog.DataLog;
 import edu.wpi.first.wpilibj.DataLogManager;
 import edu.wpi.first.util.datalog.BooleanLogEntry;
@@ -41,6 +42,8 @@ public class ChasePieces extends Command {
     private double drivePower;
     private double rotPower;
 
+    private Pose2d startingPose;
+
     private boolean onTarget;
     private boolean hasPiece;
     private boolean isDone;
@@ -112,7 +115,7 @@ public void initialize() {
         smartCollect.initialize();
 
         hasSeenTarget = false;
-
+        startingPose = drivetrain.getPose();
     }
 
     /**
@@ -194,11 +197,11 @@ private void autoChase(){
                 }
             } else {
                 if (!hasSeenTarget) {
-                    if (drivetrain.getPose().getY() > VisionConstants.HALF_FIELD_HEIGHT) {
+                    if (startingPose.getY() > VisionConstants.HALF_FIELD_HEIGHT) {
                         if (DriverStation.getAlliance().get() == Alliance.Blue) {
-                            drivetrain.setRobot(0, 0, -rotPower);
+                            drivetrain.setRobot(0.5, 0, -rotPower);
                         } else {
-                            drivetrain.setRobot(0, 0, rotPower);
+                            drivetrain.setRobot(0.5, 0, rotPower);
                         }
                     } else {
                         if (DriverStation.getAlliance().get() == Alliance.Blue) {