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];