diff --git a/src/main/java/frc/robot/commands/ModeAuto/GrimperReservoir.java b/src/main/java/frc/robot/commands/ModeAuto/GrimperReservoir.java index 0b0adf6..03e8629 100644 --- a/src/main/java/frc/robot/commands/ModeAuto/GrimperReservoir.java +++ b/src/main/java/frc/robot/commands/ModeAuto/GrimperReservoir.java @@ -63,8 +63,6 @@ public class GrimperReservoir extends Command { BotPose = limeLight3G.getBotPoseBlue(); botx = BotPose[0]; boty = BotPose[1]; - pigeonAngle = drivetrain.getPigeon2().getYaw().getValueAsDouble()+180; - System.out.println(pigeonAngle); if(angle < 0){ angle = angle + 360; } @@ -85,23 +83,25 @@ public class GrimperReservoir extends Command { } } else{ + pigeonAngle = drivetrain.getPigeon2().getYaw().getValueAsDouble()+180; + System.out.println(pigeonAngle); y = 2.6; - x = 1.11; - angle = 0; - if(pigeonAngle> 358 || pigeonAngle< 2){ - drivetrain.setControl(drive.withVelocityY(MathUtil.clamp(((y-boty)*5.5), -2, 2)).withVelocityX(MathUtil.clamp(((x-botx)*5.5),-2,2)).withRotationalRate(0)); - } - else{ - if(pigeonAngle>0 && pigeonAngle<180){ - drivetrain.setControl(drive.withRotationalRate(-1)); - System.out.println("x"); + x = 1.11; + angle = 0; + if(pigeonAngle> 358 || pigeonAngle< 2){ + drivetrain.setControl(drive.withVelocityY(MathUtil.clamp(((y-boty)*5.5), -2, 2)).withVelocityX(MathUtil.clamp(((x-botx)*5.5),-2,2)).withRotationalRate(0)); } - else if(pigeonAngle>180){ - System.out.println("e"); - drivetrain.setControl(drive.withRotationalRate(1)); + else{ + if(pigeonAngle>0 && pigeonAngle<180){ + drivetrain.setControl(drive.withRotationalRate(-1)); + System.out.println("x"); + } + else if(pigeonAngle>180){ + System.out.println("e"); + drivetrain.setControl(drive.withRotationalRate(1)); + } } } - } } else{ drivetrain.setControl(drive.withVelocityX(0).withVelocityY(0).withRotationalRate(0));