diff --git a/src/main/java/frc/robot/command/ChasePieces.java b/src/main/java/frc/robot/command/ChasePieces.java index 4c9ba732..5385164f 100644 --- 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) {