From b83b860e1f838ebc26b5744620529a8bd76b684d Mon Sep 17 00:00:00 2001 From: VladislavOstapov Date: Sun, 5 Feb 2023 15:48:43 +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=BD=D0=B5=D0=BF=D1=80=D0=B0=D0=B2?= =?UTF-8?q?=D0=B8=D0=BB=D1=8C=D0=BD=D0=BE=D0=B9=20=D1=83=D1=81=D1=82=D0=B0?= =?UTF-8?q?=D0=BD=D0=BE=D0=B2=D0=BA=D0=B8=20LOCK-=D0=B7=D0=BE=D0=BD,=20?= =?UTF-8?q?=D0=BD=D0=B0=D0=B1=D0=BE=D1=80=20=D0=B8=D1=81=D0=BF=D1=80=D0=B0?= =?UTF-8?q?=D0=B2=D0=BB=D0=B5=D0=BD=D0=B8=D0=B9=20=D0=BD=D0=BE=D1=87=D0=BD?= =?UTF-8?q?=D0=BE=D0=B3=D0=BE=20=D1=80=D0=B5=D0=B6=D0=B8=D0=BC=D0=B0?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- scheduler.c | 78 ++++++++++++++++++++---------------------------- utils.c | 86 ++++++++++++++++++++++------------------------------- 2 files changed, 69 insertions(+), 95 deletions(-) diff --git a/scheduler.c b/scheduler.c index 972c747..8db7c62 100644 --- a/scheduler.c +++ b/scheduler.c @@ -82,13 +82,38 @@ short scheduler_find_task(const struct scheduler_task* tasks, const short curr_p } -void schedule_one_robot(const struct scheduler_task* tasks, const struct robot_regs* r, struct robot_code* code, const short robot_id) { +void schedule_one_robot(const short robot_id) { + struct scheduler_task tasks[BARRELS_COUNT]; + + for (short i = 0; i < BARRELS_COUNT; i++) { + // для каждой задачи: + tasks[i].start_zone = barrels[i].zone; + // определяем можно ли ее выполнить и что вообще нужно выполнить + tasks[i].dest_zone = can_move(barrels + i, robot_id); + if (tasks[i].dest_zone >= 0) { + tasks[i].priority = get_operation_priority(i); + } + } + + // узнаем текущую зону робота + short current_zone; + if (robot_id == ROBOT_1) { + current_zone = robot1.dx.current_zone; + } else { + current_zone = robot2.dx.current_zone; + } + // формируем список задач - short target_task = scheduler_find_task(tasks, r->dx.current_zone); + short target_task = scheduler_find_task(tasks, current_zone); if (target_task >= 0) { - create_operation(code, target_task, tasks[target_task].start_zone, tasks[target_task].dest_zone, - r->dx.current_zone, robot_id); + if (robot_id == ROBOT_1) { + create_operation(&robot1_code, target_task, tasks[target_task].start_zone, tasks[target_task].dest_zone, + current_zone, robot_id); + } else { + create_operation(&robot2_code, target_task, tasks[target_task].start_zone, tasks[target_task].dest_zone, + current_zone, robot_id); + } } } @@ -98,7 +123,6 @@ void scheduler_main() #endif { if (scheduler_start_signal) { - scheduler_correction_stage = 0; robot1_code.PC = -1; robot2_code.PC = -1; @@ -220,8 +244,6 @@ void scheduler_main() 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]; - if (one_robot_mode) { // режим одного робота char robot_id = ROBOT_NONE; @@ -248,21 +270,7 @@ void scheduler_main() robot2_code.PC = 0; } } else { - for (short i = 0; i < BARRELS_COUNT; i++) { - // для каждой задачи: - tasks[i].start_zone = barrels[i].zone; - // определяем можно ли ее выполнить и что вообще нужно выполнить - tasks[i].dest_zone = can_move(barrels + i, robot_id); - if (tasks[i].dest_zone >= 0) { - tasks[i].priority = get_operation_priority(i); - } - } - - if (robot_id == ROBOT_1) { - schedule_one_robot(tasks, &robot1, &robot1_code, ROBOT_1); - } else { - schedule_one_robot(tasks, &robot2, &robot2_code, ROBOT_2); - } + schedule_one_robot(robot_id); } } } else { @@ -302,23 +310,13 @@ void scheduler_main() } else { // отдельно просчитаем все для первого робота if (robot1_code.PC < 0) { - for (short i = 0; i < BARRELS_COUNT; i++) { - // для каждой задачи: - tasks[i].start_zone = barrels[i].zone; - // определяем можно ли ее выполнить и что вообще нужно выполнить - tasks[i].dest_zone = can_move(barrels + i, ROBOT_1); - if (tasks[i].dest_zone >= 0) { - tasks[i].priority = get_operation_priority(i); - } - } - - schedule_one_robot(tasks, &robot1, &robot1_code, ROBOT_1); + schedule_one_robot(ROBOT_1); } // и отдельно для второго (только если не в ночном режиме) if (robot2_code.PC < 0) { if (robot2.dx.current_zone < ZONE_WASHING_1A) { - // начальная позиция робота 1 - обезжир + // начальная позиция робота 2 - промывка 1a robot2_lock_zone = ZONE_WASHING_1A; if (robot2_code.PC < 0) { robot2_code.barrel_id = -1; @@ -328,17 +326,7 @@ void scheduler_main() robot2_code.PC = 0; } } else { - for (short i = 0; i < BARRELS_COUNT; i++) { - // для каждой задачи: - tasks[i].start_zone = barrels[i].zone; - // определяем можно ли ее выполнить и что вообще нужно выполнить - tasks[i].dest_zone = can_move(barrels + i, ROBOT_2); - if (tasks[i].dest_zone >= 0) { - tasks[i].priority = get_operation_priority(i); - } - } - - schedule_one_robot(tasks, &robot2, &robot2_code, ROBOT_2); + schedule_one_robot(ROBOT_2); } } } diff --git a/utils.c b/utils.c index 680144e..1bea2bd 100644 --- a/utils.c +++ b/utils.c @@ -34,7 +34,7 @@ char zone_is_busy(short zone) { short get_first_night_zone() { // TODO сделать корректное вычисление ночной зоны - // всего зон, куда можно сныкать барабаны (всего 9 мест: 8 промывок и выгрузка) + // всего зон, куда можно сныкать барабаны (всего 9 мест: загрузка 1 и 8 промывок) for (short nz = 0; nz < 9; nz++) { char found = 0; for (short i = 0; i < BARRELS_COUNT; i++) { @@ -144,12 +144,7 @@ short _get_dest_zone(struct barrel *bar) { case ZONE_WASHING_4B: // последняя промывка в процессе - if (hla_night_mode) { - // TODO изменить логику для ночного режима - return -1; - } else { - return ZONE_UNLOAD; - } + return ZONE_UNLOAD; case ZONE_UNLOAD: // последняя промывка, нужно разрешение на выгрузку @@ -188,11 +183,11 @@ short can_move(struct barrel *bar, char robot_id) { } // сразу отсекаем варианты, при которых невозможно переместить барабан if (!bar->flags.is_exist) { - return -3; + return -4; } if (bar->flags.robot != 0) { - return -3; + return -5; } // проверка ночного режима @@ -225,7 +220,7 @@ short can_move(struct barrel *bar, char robot_id) { } #endif - if (bar->software_timer > -approximate_time) { + if (bar->software_timer > approximate_time) { return -2; } } @@ -253,7 +248,7 @@ short can_move(struct barrel *bar, char robot_id) { border += LOCK_ZONE_BORDER; if (bar->zone <= border || dest_zone <= border) { - dest_zone = -1; + dest_zone = -6; } } else { // если робот 2, то это новый, который ближе к началу линнии. @@ -262,14 +257,14 @@ short can_move(struct barrel *bar, char robot_id) { if (border > robot1_lock_zone) { border = robot1_lock_zone; } - if (border > ZONE_WASHING_3B) { - border = ZONE_WASHING_3B; + if (border > ZONE_PASSIVATION) { + border = ZONE_PASSIVATION; } border -= LOCK_ZONE_BORDER; if (bar->zone >= border || dest_zone >= border) { - dest_zone = -1; + dest_zone = -6; } } } @@ -495,8 +490,8 @@ void create_operation(struct robot_code *code, short barrel_id, const short star if (start_zone < dest_zone) { tmp = start_zone; } - if (tmp > ZONE_WASHING_3B) { - tmp = ZONE_WASHING_3B; + if (tmp > ZONE_PASSIVATION) { + tmp = ZONE_PASSIVATION; } robot1_lock_zone = tmp; } else { @@ -504,18 +499,13 @@ void create_operation(struct robot_code *code, short barrel_id, const short star if (start_zone > dest_zone) { tmp = start_zone; } - if (tmp < ZONE_WASHING_2A) { - tmp = ZONE_WASHING_2A; + if (tmp < ZONE_ETCHING_2) { + tmp = ZONE_ETCHING_2; } robot2_lock_zone = tmp; } } - // если ночной режим, то нужно сразу обновить флаг у барабана - if (hla_night_mode && barrel_id >= 0) { - barrels[barrel_id].flags.is_night = -1; - } - // первым делом добавляем команду опустить траверсу code->code[cmd_index++] = ROBOT_CMD_DOWN(); @@ -524,14 +514,16 @@ void create_operation(struct robot_code *code, short barrel_id, const short star if (current_zone != start_zone) { code->code[cmd_index++] = ROBOT_CMD_MOVE_TO_ZONE(ZONE_WASHING_3B); } - code->code[cmd_index++] = ROBOT_CMD_SET_LOCK_ZONE(ZONE_WASHING_4A); + if (!one_robot_mode) { + code->code[cmd_index++] = ROBOT_CMD_SET_LOCK_ZONE(ZONE_PASSIVATION); + } code->code[cmd_index++] = ROBOT_CMD_UP_WITH_BARREL(); code->code[cmd_index++] = ROBOT_CMD_MOVE_TO_ZONE_WITH_BARREL(ZONE_PASSIVATION); code->code[cmd_index++] = ROBOT_CMD_DOWN_WITH_BARREL(); // NOTE таймер робота работает точнее чем таймер барабанов -// code->code[cmd_index++] = ROBOT_CMD_TMR_SET(barrels[barrel_id].time_passivation); - code->code[cmd_index++] = ROBOT_CMD_WAIT(barrels[barrel_id].time_passivation); + code->code[cmd_index++] = ROBOT_CMD_TMR_SET(barrels[barrel_id].time_passivation); +// code->code[cmd_index++] = ROBOT_CMD_WAIT(barrels[barrel_id].time_passivation); code->code[cmd_index++] = ROBOT_CMD_UP_WITH_BARREL(); code->code[cmd_index++] = ROBOT_CMD_WAIT(hla_time_digging); @@ -555,32 +547,32 @@ void create_operation(struct robot_code *code, short barrel_id, const short star } // теперь обновляем LOCK-зону - if (!one_robot_mode && !hla_night_mode) { + if (!one_robot_mode) { if (robot_id == 1) { // ставим lock-зону только если она ближе к концу линии short tmp = dest_zone; - // lock-зона этого робота не может выходить за промывку 3А, потому что это не имеет смысла - if (tmp > ZONE_WASHING_3B) { - tmp = ZONE_WASHING_3B; + // lock-зона этого робота должна быть такой, чтобы была свободна промывка 3а + if (tmp > ZONE_PASSIVATION) { + tmp = ZONE_PASSIVATION; } // в любом случае lock-зону нельзя двигать к началу линии if (tmp > robot1_lock_zone) { - code->code[cmd_index++] = ROBOT_CMD_SET_LOCK_ZONE(dest_zone); + code->code[cmd_index++] = ROBOT_CMD_SET_LOCK_ZONE(tmp); } } else { // ставим lock-зону только если она ближе к началу линии short tmp = dest_zone; // lock-зона этого робота не может выходить за промывку 3А, потому что это не имеет смысла - if (tmp < ZONE_WASHING_2A) { - tmp = ZONE_WASHING_2A; + if (tmp < ZONE_ETCHING_2) { + tmp = ZONE_ETCHING_2; } // в любом случае lock-зону нельзя двигать к концу линии if (tmp < robot2_lock_zone) { - code->code[cmd_index++] = ROBOT_CMD_SET_LOCK_ZONE(dest_zone); + code->code[cmd_index++] = ROBOT_CMD_SET_LOCK_ZONE(tmp); } } } @@ -710,29 +702,23 @@ void create_operation(struct robot_code *code, short barrel_id, const short star } } - if (!one_robot_mode && !hla_night_mode) { + if (!one_robot_mode) { if (robot_id == 2) { - // суть этого предела в том, чтобы робот всегда держал свободной зону gal1, - // но при этом если зоной обмена выбрана промывка 2б, то за ней тоже, получается min(ZONE_G1, exchange_zone) - const short robot2_border = (ZONE_GALVANIZING_1 < hla_exchange_zone ? ZONE_GALVANIZING_1 : hla_exchange_zone) - 1; - - if (dest_zone >= robot2_border) { - // из промывки 2б он перекладывал, пусть едет в промывку 2а - code->code[cmd_index++] = ROBOT_CMD_SET_LOCK_ZONE(robot2_border - 1); - code->code[cmd_index++] = ROBOT_CMD_MOVE_TO_ZONE(robot2_border - 1); + // 2-му роботу нужно держать всегда свободной зону промывки 2б + if (dest_zone > ZONE_ETCHING_2) { + code->code[cmd_index++] = ROBOT_CMD_SET_LOCK_ZONE(ZONE_ETCHING_2); + code->code[cmd_index++] = ROBOT_CMD_MOVE_TO_ZONE(ZONE_ETCHING_2); } else if (dest_zone < ZONE_WASHING_1A) { + // на случай если робот уехал в выгрузку или обезжир, надо его отогнать в зону промывки 1а // code->code[cmd_index++] = ROBOT_CMD_SET_LOCK_ZONE(ZONE_WASHING_1A); code->code[cmd_index++] = ROBOT_CMD_MOVE_TO_ZONE(ZONE_WASHING_1A); } } else if (robot_id == 1) { - // суть этого предела в том, чтобы робот всегда был свободен ЗА зоной gal8, - // но при этом если зоной обмена выбрана промывка 3а, то за ней тоже, получается max(ZONE_G8, exchange_zone) - const short robot1_border = (ZONE_GALVANIZING_8 > hla_exchange_zone ? ZONE_GALVANIZING_8 : hla_exchange_zone) + 1; - - if (dest_zone <= robot1_border) { + // 2-му роботу нужно держать всегда свободной зону промывки 3а + if (dest_zone < ZONE_PASSIVATION) { // чтобы из этой зоны можно было переложить барабан первому роботу - code->code[cmd_index++] = ROBOT_CMD_SET_LOCK_ZONE(robot1_border + 1); - code->code[cmd_index++] = ROBOT_CMD_MOVE_TO_ZONE(robot1_border + 1); + code->code[cmd_index++] = ROBOT_CMD_SET_LOCK_ZONE(ZONE_PASSIVATION); + code->code[cmd_index++] = ROBOT_CMD_MOVE_TO_ZONE(ZONE_PASSIVATION); } } } else {