Как организовать возврат ПИД-двигателя в ноль после съемки с использованием конечных автоматов в FTC Java? ⇐ JAVA

[Anonymous]

Я работаю над FTC TeleOp, в котором моторизованный спиндексер (с энкодером + ПИД) выдвигается для стрельбы по игровым элементам. После съемки я хочу, чтобы спиндексер автоматически возвращался в положение кодировщика 0.
Я понимаю, что мне, вероятно, нужно использовать флаги состояния, но я не уверен, где или как структурировать их в моем контуре управления.

Что я пытаюсь сделать

• Нажмите вправо Stick → спиндексер перемещается для стрельбы (управляется ПИД)
• После завершения выстрела &shot=true → спиндексер должен вращаться обратно в положение энкодера 0 (targert=0)
• Пока это происходит, я не хочу:
  • немедленного отключения контура ПИД
  • команды возврата к нулю для прерывания движения стрельбы

Текущая проблема

У меня уже есть логический флаг выстрела, который я использую для переключения поведения правого джойстика / кнопки B.

Однако, если я повторно использую этот флаг, чтобы также активировать логику возврата к нулю, двигатель не ждет завершения движения стрельбы — он немедленно переопределяет текущую цель.
Я попробовал что-то сделать например:
if (!sorter.isBusy()) {
target = 0;
arm.setPosition(0.81);
}

Я узнал, что isBusy() работает только в режиме RUN_TO_POSITION, поэтому он не может работать.

Вот полный соответствующий код:
@TeleOp
public class sorter extends LinearOpMode {

static final int LOAD = 155;
static final int STEP_TICKS = 191;
static final int BACK_STEPS = 7;
static final int TOLERANCE = 3;
static double MAX_POWER = 0.3;

boolean shot = false;
PIDController pid = new PIDController(0.004 , 0.0005, 0.0);
int target = 0;

DcMotor sorter;
Servo arm;

@Override
public void runOpMode() {

sorter = hardwareMap.get(DcMotor.class, "sorter");
arm = hardwareMap.get(Servo.class, "arm");

sorter.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER);
sorter.setMode(DcMotor.RunMode.RUN_USING_ENCODER);

waitForStart();

while (opModeIsActive()) {

if (gamepad1.bWasPressed()) {
if (!shot) {
shot = true;
MAX_POWER = 0.3;

Thread t = new Thread(() -> {
try {
target -= LOAD - 20;
pid.reset(getRuntime());
arm.setPosition(0.81);
Thread.sleep(450);
target += LOAD - 10;
pid.reset(getRuntime());
} catch (InterruptedException ignored) {}
});
t.start();

} else {
MAX_POWER = 0.85;
shot = false;
target += BACK_STEPS * STEP_TICKS;
pid.reset(getRuntime());
}
}

int pos = sorter.getCurrentPosition();
int error = target - pos;

double power = pid.update(target, pos, getRuntime());
power = Math.max(-MAX_POWER, Math.min(MAX_POWER, power));

if (Math.abs(error)

Подробнее здесь: https://stackoverflow.com/questions/798 ... machines-i