diff --git a/src/main/java/frc/robot/commands/Limelight/AprilTag3.java b/src/main/java/frc/robot/commands/Limelight/AprilTag3.java index 642fbe7..78ad1da 100644 --- a/src/main/java/frc/robot/commands/Limelight/AprilTag3.java +++ b/src/main/java/frc/robot/commands/Limelight/AprilTag3.java @@ -63,10 +63,22 @@ public class AprilTag3 extends Command { } else if(Botyaw >90 && Botyaw <=180){ drivetrain.setControl(drive. - withRotationalRate(a/20). - withVelocityX(0). - withVelocityY( b/50)); - System.out.println(b/50); + withRotationalRate(tx/20). + withVelocityX(MathUtil.clamp(-x/4, -1, 1)). + withVelocityY(MathUtil.clamp(z/4, -1, 1))); + } + else if(Botyaw >-90 && Botyaw <=0){ + drivetrain.setControl(drive. + withRotationalRate(tx/20). + withVelocityX(MathUtil.clamp(-x/4, -1, 1)). + withVelocityY(MathUtil.clamp(-z/4, -1, 1))); + } + else if(Botyaw >-180 && Botyaw <=-90){ + drivetrain.setControl(drive. + withRotationalRate(tx/20). + withVelocityX(MathUtil.clamp(x/4, -1, 1)). + withVelocityY(MathUtil.clamp(-z/4, -1, 1))); + } } else{ drivetrain.setControl(drive.