From c4ed3b76f1959006df74258902dab24f0ded9d37 Mon Sep 17 00:00:00 2001 From: VladislavOstapov Date: Wed, 1 Feb 2023 18:12:34 +0300 Subject: [PATCH] =?UTF-8?q?=D0=98=D1=81=D0=BF=D1=80=D0=B0=D0=B2=D0=BB?= =?UTF-8?q?=D0=B5=D0=BD=D0=B8=D0=B5=20=D0=BA=D0=BE=D1=80=D1=80=D0=B5=D0=BA?= =?UTF-8?q?=D1=86=D0=B8=D0=B8?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- scheduler.c | 154 ++++++++++++++++++++++++++++------------------------ 1 file changed, 84 insertions(+), 70 deletions(-) diff --git a/scheduler.c b/scheduler.c index 5ba4e22..be9e9ea 100644 --- a/scheduler.c +++ b/scheduler.c @@ -114,7 +114,89 @@ void scheduler_main() scheduler_start_signal = 0; } - if (scheduler_en) { + if (hla_correct_command) { + if (one_robot_mode) { + switch (scheduler_correction_stage) { + case 0: + if (hla_robot2_en) { + if (robot2_code.PC < 0) { + robot2_code.barrel_id = -1; + robot2_code.code[0] = ROBOT_CMD_CORRECT_AXIS(ROBOT_AXIS_Z); + robot2_code.code[1] = ROBOT_CMD_CORRECT_AXIS(ROBOT_AXIS_X); + robot2_code.code[2] = ROBOT_CMD_END(); + robot2_code.PC = 0; + + scheduler_correction_stage++; + } + } else { + if (robot1_code.PC < 0) { + robot1_code.barrel_id = -1; + robot1_code.code[0] = ROBOT_CMD_CORRECT_AXIS(ROBOT_AXIS_Z); + robot1_code.code[1] = ROBOT_CMD_CORRECT_AXIS(ROBOT_AXIS_X); + robot1_code.code[2] = ROBOT_CMD_MOVE_TO_PARKING(); + robot1_code.code[3] = ROBOT_CMD_END(); + robot1_code.PC = 0; + + scheduler_correction_stage++; + } + } + case 1: + if (robot2_code.PC < 0 && robot1_code.PC < 0) { + scheduler_correction_stage++; + } + break; + + default: + scheduler_correction_stage = 0; + hla_correct_command = 0; + } + } else { + switch (scheduler_correction_stage) { + case 0: + // сначала робота 2 отправляем на коррекцию + if (robot2_code.PC < 0) { + robot2_code.barrel_id = -1; + robot2_code.code[0] = ROBOT_CMD_CORRECT_AXIS(ROBOT_AXIS_Z); + robot2_code.code[1] = ROBOT_CMD_CORRECT_AXIS(ROBOT_AXIS_X); + robot2_code.code[2] = ROBOT_CMD_END(); + robot2_code.PC = 0; + + scheduler_correction_stage++; + } + break; + + case 1: + if (robot2_code.PC < 0) { + scheduler_correction_stage++; + } + break; + + case 2: + // потом робот 1 + if (robot1_code.PC < 0) { + robot1_code.barrel_id = -1; + robot1_code.code[0] = ROBOT_CMD_CORRECT_AXIS(ROBOT_AXIS_Z); + robot1_code.code[1] = ROBOT_CMD_CORRECT_AXIS(ROBOT_AXIS_X); + robot1_code.code[2] = ROBOT_CMD_MOVE_TO_PARKING(); + robot1_code.code[3] = ROBOT_CMD_END(); + robot1_code.PC = 0; + + scheduler_correction_stage++; + } + break; + + case 3: + if (robot1_code.PC < 0) { + scheduler_correction_stage++; + } + break; + + default: + scheduler_correction_stage = 0; + hla_correct_command = 0; + } + } + } else if (scheduler_en) { // программный таймер, применяется ко всем существующим барабанам if (_scheduler_software_timer) { @@ -134,75 +216,7 @@ void scheduler_main() increment_zone(ROBOT_ZONE_GAL); } - if (hla_correct_command) { - if (one_robot_mode) { - if (hla_robot2_en) { - if (robot2_code.PC < 0) { - robot2_code.barrel_id = -1; - robot2_code.code[0] = ROBOT_CMD_CORRECT_AXIS(ROBOT_AXIS_Z); - robot2_code.code[1] = ROBOT_CMD_CORRECT_AXIS(ROBOT_AXIS_X); - robot2_code.code[2] = ROBOT_CMD_END(); - robot2_code.PC = 0; - - hla_correct_command = 0; - } - } else { - if (robot1_code.PC < 0) { - robot1_code.barrel_id = -1; - robot1_code.code[0] = ROBOT_CMD_CORRECT_AXIS(ROBOT_AXIS_Z); - robot1_code.code[1] = ROBOT_CMD_CORRECT_AXIS(ROBOT_AXIS_X); - robot1_code.code[2] = ROBOT_CMD_END(); - robot1_code.PC = 0; - - hla_correct_command = 0; - } - } - } else { - switch (scheduler_correction_stage) { - case 0: - // сначала робота 2 отправляем на коррекцию - if (robot2_code.PC < 0) { - robot2_code.barrel_id = -1; - robot2_code.code[0] = ROBOT_CMD_CORRECT_AXIS(ROBOT_AXIS_Z); - robot2_code.code[1] = ROBOT_CMD_CORRECT_AXIS(ROBOT_AXIS_X); - robot2_code.code[2] = ROBOT_CMD_END(); - robot2_code.PC = 0; - - scheduler_correction_stage++; - } - break; - - case 1: - if (robot2_code.PC < 0) { - scheduler_correction_stage++; - } - break; - - case 2: - // потом робот 1 - if (robot1_code.PC < 0) { - robot1_code.barrel_id = -1; - robot1_code.code[0] = ROBOT_CMD_CORRECT_AXIS(ROBOT_AXIS_Z); - robot1_code.code[1] = ROBOT_CMD_CORRECT_AXIS(ROBOT_AXIS_X); - robot1_code.code[2] = ROBOT_CMD_END(); - robot1_code.PC = 0; - - scheduler_correction_stage++; - } - break; - - case 3: - if (robot1_code.PC < 0) { - scheduler_correction_stage++; - } - break; - - default: - scheduler_correction_stage = 0; - hla_correct_command = 0; - } - } - } else if (!auto_mode_pause) { + if (!auto_mode_pause) { if ((hla_robot1_en && robot1_code.PC < 0) || (hla_robot2_en && robot2_code.PC < 0)) { struct scheduler_task tasks[BARRELS_COUNT];