diff --git a/src/main/java/frc/robot/commands/ModeAuto/RetourMilieuDroite.java b/src/main/java/frc/robot/commands/ModeAuto/RetourMilieuDroite.java index 9e00296..1294f7d 100644 --- a/src/main/java/frc/robot/commands/ModeAuto/RetourMilieuDroite.java +++ b/src/main/java/frc/robot/commands/ModeAuto/RetourMilieuDroite.java @@ -50,12 +50,18 @@ public class RetourMilieuDroite extends Command { // Called every time the scheduler runs while the command is scheduled. @Override public void execute() { + double[] BotPose = new double[6]; + BotPose = limelight3g.getBotPoseBlue(); + botx = BotPose[0]; + boty = BotPose[1]; + if(alliance.get() == Alliance.Blue){ angle = 0; } else{ angle = 180; } + if(pigeon2.getYaw().getValueAsDouble() <355 || pigeon2.getYaw().getValueAsDouble() > 5){ if(pigeon2.getYaw().getValueAsDouble() >0 && pigeon2.getYaw().getValueAsDouble() <180){ drivetrain.setControl(drive.withRotationalRate(0.5)); } @@ -63,6 +69,9 @@ public class RetourMilieuDroite extends Command { drivetrain.setControl(drive.withRotationalRate(-0.5)); } } + else{ + } + } // Called once the command ends or is interrupted. @Override