From 5ad225fa85ec1e8402e2e89d5fe178af8ee4406d Mon Sep 17 00:00:00 2001 From: VladislavOstapov Date: Mon, 9 Jan 2023 22:38:38 +0300 Subject: [PATCH] =?UTF-8?q?=D0=94=D0=BE=D0=B1=D0=B0=D0=B2=D0=BB=D0=B5?= =?UTF-8?q?=D0=BD=D0=B8=D0=B5=20=D0=BF=D0=BE=D0=BB=D0=BD=D0=BE=D1=86=D0=B5?= =?UTF-8?q?=D0=BD=D0=BD=D0=BE=D0=B3=D0=BE=20=D0=BD=D0=BE=D1=87=D0=BD=D0=BE?= =?UTF-8?q?=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 --- robot.cpp | 12 ++ scheduler.c | 215 ++++++++++++++++++-------------- utils.c | 348 ++++++++++++++++++++++++++++------------------------ 3 files changed, 327 insertions(+), 248 deletions(-) diff --git a/robot.cpp b/robot.cpp index caab3f8..b7164f9 100644 --- a/robot.cpp +++ b/robot.cpp @@ -18,6 +18,18 @@ static bool robot_move(robot_regs& r, short target, char robot_id) { robot2_offset_pos = false; } + // перемещения в зоне парковки + if (target < 0 && r.dx.current_zone == 0) { + r.mx.correct_sensor = true; + return true; + } + + if (r.mx.correct_sensor) { + r.dx.current_zone = 0; + r.mx.correct_sensor = false; + return target == 0; + } + if (r.dx.current_zone == target) { return true; } diff --git a/scheduler.c b/scheduler.c index f4ac760..b9a625b 100644 --- a/scheduler.c +++ b/scheduler.c @@ -11,73 +11,87 @@ struct scheduler_task { short scheduler_find_task(const struct scheduler_task* tasks, const short curr_pos) { - // для начала надо найти максимальный приоритет у операций - short max_priority = -1; - for (short i = 0; i < BARRELS_COUNT; i++) { - if (tasks[i].dest_zone >= 0) { - if (tasks[i].priority > max_priority) { - max_priority = tasks[i].priority; + if (hla_night_mode) { + // надо просто найти самый левый барабан + short b = -1; // собсна барабан, который можно переместить + short z = ZONE_UNLOAD; // зона, ищем минимальную поэтому ставим максимальную - зону выгрузки + for (short i = 0; i < BARRELS_COUNT; i++) { + if (tasks[i].dest_zone >= 0 && tasks[i].start_zone <= z) { + b = i; + z = tasks[i].start_zone; } } - } - if (max_priority < 0) { - return -1; // тасков нет - } - - // ищем первый барабан слева, и ближайший справа - short left = -1, right = -1; - for (short i = 0; i < BARRELS_COUNT; i++) { - short target = tasks[i].start_zone; // фактическая зона откуда тащить барабан - if (tasks[i].dest_zone < 0) { - continue; - } - - // чтобы не получилось перемещать барабаны с приоритетом ниже - if (tasks[i].priority < max_priority) { - continue; - } - - if (curr_pos <= target) { - // это таск справа, надо найти ближайший - if (right == -1) { - right = i; - } else { - if (barrels[right].zone > target) { - right = i; - } - } - } else { - // таск слева, ищем максимально дальний (с минимальной зоной) - if (left == -1) { - left = i; - } else { - if (barrels[left].zone > target) { - left = i; - } - } - } - } - - // итого есть результат: есть ли таски, которые надо тащить вперед (и если надо то какой ближний), и есть первый таск - if (left < 0) { - return right; // вернем таск справа (если его нет, в переменной будет -1) - } - if (right < 0) { - return left; // если вдруг задачи справа не оказалось, вернем задачу слева если есть - } - - // вычисляем что ближе - short ld = curr_pos - left; // левая дельта - short rd = right - curr_pos; // правая дельта - - // дальше сравниваем дельты - // по идее если они равны то с большим приоритетом робот поедет в левую часть, - // а левую дельту вообще уменьшу на 1, чтобы цель слева казалась ближе - if (rd > ld - 1) { - return left; + return b; } else { - return right; + // для начала надо найти максимальный приоритет у операций + short max_priority = -1; + for (short i = 0; i < BARRELS_COUNT; i++) { + if (tasks[i].dest_zone >= 0) { + if (tasks[i].priority > max_priority) { + max_priority = tasks[i].priority; + } + } + } + + if (max_priority < 0) { + return -1; // тасков нет + } + + // ищем первый барабан слева, и ближайший справа + short left = -1, right = -1; + for (short i = 0; i < BARRELS_COUNT; i++) { + short target = tasks[i].start_zone; // фактическая зона откуда тащить барабан + if (tasks[i].dest_zone < 0) { + continue; + } + + // чтобы не получилось перемещать барабаны с приоритетом ниже + if (tasks[i].priority < max_priority) { + continue; + } + + if (curr_pos <= target) { + // это таск справа, надо найти ближайший + if (right == -1) { + right = i; + } else { + if (barrels[right].zone > target) { + right = i; + } + } + } else { + // таск слева, ищем максимально дальний (с минимальной зоной) + if (left == -1) { + left = i; + } else { + if (barrels[left].zone > target) { + left = i; + } + } + } + } + + // итого есть результат: есть ли таски, которые надо тащить вперед (и если надо то какой ближний), и есть первый таск + if (left < 0) { + return right; // вернем таск справа (если его нет, в переменной будет -1) + } + if (right < 0) { + return left; // если вдруг задачи справа не оказалось, вернем задачу слева если есть + } + + // вычисляем что ближе + short ld = curr_pos - left; // левая дельта + short rd = right - curr_pos; // правая дельта + + // дальше сравниваем дельты + // по идее если они равны то с большим приоритетом робот поедет в левую часть, + // а левую дельту вообще уменьшу на 1, чтобы цель слева казалась ближе + if (rd > ld - 1) { + return left; + } else { + return right; + } } } @@ -89,10 +103,6 @@ void schedule_one_robot(const struct scheduler_task* tasks, const struct robot_r 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); - -#ifdef EMULATOR - debug_print_robot_code(code, robot_id, 0); -#endif } } @@ -114,6 +124,11 @@ void scheduler_main() scheduler_start_signal = 0; } + if (hla_night_mode) { + robot1_lock_zone = -1; + robot2_lock_zone = -1; + } + if (scheduler_en) { // программный таймер, применяется ко всем существующим барабанам @@ -201,31 +216,52 @@ void scheduler_main() // а вот для режима двух роботов все интересно // для каждого робота нужно получить свой список задач // и надо еще сделать так, чтобы роботы не столкнулись + char cmd_en = 0; - // логика для того, чтобы роботы не столкнулись в начале - if (robot1.dx.current_zone < ZONE_GALVANIZING_1) { - // начальная позиция робота 1 - промывка 3Б - robot2_lock_zone = -1; - robot1_lock_zone = ZONE_WASHING_3B; - if (robot1_code.PC < 0) { - robot1_code.barrel_id = -1; - robot1_code.code[0] = ROBOT_CMD_DOWN(); - robot1_code.code[1] = ROBOT_CMD_MOVE_TO_ZONE(ZONE_WASHING_3B); - robot1_code.code[2] = ROBOT_CMD_END(); - robot1_code.PC = 0; - } - } else if (robot2.dx.current_zone < ZONE_DEGREASING) { - // начальная позиция робота 2 - обезжиривание - if (robot2_code.PC < 0) { - robot2_code.barrel_id = -1; - robot2_code.code[0] = ROBOT_CMD_DOWN(); - robot2_code.code[1] = ROBOT_CMD_MOVE_TO_ZONE(ZONE_DEGREASING); - robot2_code.code[2] = ROBOT_CMD_END(); - robot2_code.PC = 0; + // добавляем ночной режим: в режиме двух роботов требуется чтобы во время ночного режима робот 2 стоял на парковке + if (hla_night_mode) { + if (!robot2.mx.correct_sensor) { + // уводим робота в парковку + if (robot2_code.PC < 0) { + robot2_code.barrel_id = -1; + robot2_code.code[0] = ROBOT_CMD_DOWN(); + robot2_code.code[1] = ROBOT_CMD_MOVE_TO_PARKING(); + robot2_code.code[2] = ROBOT_CMD_END(); + robot2_code.PC = 0; + } + } else { + cmd_en = 1; } } else { - // отдельно просчитаем все для первого робота - if (robot1_code.PC < 0) { + // логика для того, чтобы роботы не столкнулись в начале + if (robot1.dx.current_zone < ZONE_GALVANIZING_1) { + // начальная позиция робота 1 - промывка 3Б + robot2_lock_zone = -1; + robot1_lock_zone = ZONE_WASHING_3B; + if (robot1_code.PC < 0) { + robot1_code.barrel_id = -1; + robot1_code.code[0] = ROBOT_CMD_DOWN(); + robot1_code.code[1] = ROBOT_CMD_MOVE_TO_ZONE(ZONE_WASHING_3B); + robot1_code.code[2] = ROBOT_CMD_END(); + robot1_code.PC = 0; + } + } else if (robot2.dx.current_zone < ZONE_DEGREASING) { + // начальная позиция робота 2 - обезжиривание + if (robot2_code.PC < 0) { + robot2_code.barrel_id = -1; + robot2_code.code[0] = ROBOT_CMD_DOWN(); + robot2_code.code[1] = ROBOT_CMD_MOVE_TO_ZONE(ZONE_DEGREASING); + robot2_code.code[2] = ROBOT_CMD_END(); + robot2_code.PC = 0; + } + } else { + cmd_en = 1; + } + } + + if (cmd_en) { + // отдельно просчитаем все для первого робота (только если не в ночном режиме) + if (!hla_night_mode && robot1_code.PC < 0) { for (short i = 0; i < BARRELS_COUNT; i++) { // для каждой задачи: tasks[i].start_zone = barrels[i].zone; @@ -251,7 +287,6 @@ void scheduler_main() } } - schedule_one_robot(tasks, &robot2, &robot2_code, 2); } } diff --git a/utils.c b/utils.c index 56ebb09..e5c8b46 100644 --- a/utils.c +++ b/utils.c @@ -44,166 +44,193 @@ short can_move(struct barrel *bar, char robot_id) { return -2; } - // дальше нужно проверить, что можно передвигать бочку - short dest_zone = -1; + if (hla_night_mode) { + // задача - найти первую свободную промывку + const short zones[] = { + ZONE_LOAD_1, + ZONE_WASHING_1A, ZONE_WASHING_1B, + ZONE_WASHING_2A, ZONE_WASHING_2B, + ZONE_WASHING_3A, ZONE_WASHING_3B, + ZONE_WASHING_4A, ZONE_WASHING_4B + }; - switch (bar->zone) { - case ZONE_LOAD_2: - // загрузка 2, только в нее можно грузить новые барабаны, нужно обезжиривание - if (!zone_is_busy(ZONE_DEGREASING)) { - dest_zone = ZONE_DEGREASING; - } - break; - - case ZONE_DEGREASING: - // обезжиривание, нужна промывка 1А - if (!zone_is_busy(ZONE_WASHING_1A)) { - dest_zone = ZONE_WASHING_1A; - } - break; - - case ZONE_WASHING_1A: - // промывка 1А, нужна промывка 1Б - if (!zone_is_busy(ZONE_WASHING_1B)) { - dest_zone = ZONE_WASHING_1B; - } - break; - - case ZONE_WASHING_1B: - // промывка 1Б, нужно травление (зоны 5-6) - if (etching_zone < 0) { - break; - } - if (!zone_is_busy((short)(ZONE_ETCHING_1 + etching_zone))) { - dest_zone = (short)(ZONE_ETCHING_1 + etching_zone); - } - break; - - case ZONE_ETCHING_1: - case ZONE_ETCHING_2: - // травление, нужна промывка 2А - if (!zone_is_busy(ZONE_WASHING_2A)) { - dest_zone = ZONE_WASHING_2A; - } - break; - - case ZONE_WASHING_2A: - // промывка 2А, нужна промывка 2Б - if (!zone_is_busy(ZONE_WASHING_2B)) { - dest_zone = ZONE_WASHING_2B; - } - break; - - case ZONE_WASHING_2B: - // промывка 2Б, нужно цинкование (зоны 9-16) - if (galvanizing_zone < 0) { - break; - } - if (!zone_is_busy((short)(ZONE_GALVANIZING_1 + galvanizing_zone))) { - dest_zone = (short)(ZONE_GALVANIZING_1 + galvanizing_zone); - } - break; - - case ZONE_GALVANIZING_1: - case ZONE_GALVANIZING_2: - case ZONE_GALVANIZING_3: - case ZONE_GALVANIZING_4: - case ZONE_GALVANIZING_5: - case ZONE_GALVANIZING_6: - case ZONE_GALVANIZING_7: - case ZONE_GALVANIZING_8: - // цинкование, требуется чтобы в зонах 17-22 было максимум 2 барабана (3 барабана для этой части линии - максимум) - if (!zone_is_busy(ZONE_WASHING_3A)) { - short count = 0; - // если зона 17 свободна, то диапазон начнется с 3Б - for (short i = ZONE_WASHING_3B; i <= (short)ZONE_UNLOAD; i++) { - if (zone_is_busy(i)) { - count++; - } - } - if (count < 3) { - dest_zone = ZONE_WASHING_3A; - } - } - break; - - case ZONE_WASHING_3A: - // промывка 3А, перекладываем в промывку 3Б - if (!zone_is_busy(ZONE_WASHING_3B)) { - dest_zone = ZONE_WASHING_3B; - } - break; - - case ZONE_WASHING_3B: - // это перед пассивацией, требует свободную промывку 4А и на всякий случай свободную пассивацию - if (!zone_is_busy(ZONE_PASSIVATION) && !zone_is_busy(ZONE_WASHING_4A)) { - dest_zone = ZONE_PASSIVATION; - } - // это атомарная операция, по идее вносить барабан в пассивацию нельзя - break; - - case ZONE_PASSIVATION: - // процесс пассивации, нужна промывка 4A - // чисто теоретически сюда никогда не попадем, но если вдруг выстрелит пусть будет - if (!zone_is_busy(ZONE_WASHING_4A)) { - dest_zone = ZONE_WASHING_4A; - } - break; - - case ZONE_WASHING_4A: - // промывка 4А, перекладываем в промывку 4Б - if (!zone_is_busy(ZONE_WASHING_4B)) { - dest_zone = ZONE_WASHING_4B; - } - break; - - case ZONE_WASHING_4B: - // процесс пассивации, нужна промывка 4B (зона 21) (потому что сейчас я в 4A) - if (!zone_is_busy(ZONE_UNLOAD)) { - dest_zone = ZONE_UNLOAD; - } - break; - - case ZONE_UNLOAD: - // последняя промывка, нужно разрешение на выгрузку - if (one_robot_mode && button_unload) { - // нужно промывку загрузку 0 - if (!zone_is_busy(ZONE_LOAD_1)) { - dest_zone = ZONE_LOAD_1; - } - } - break; - } - - if (!one_robot_mode) { - if (robot_id == 1) { - // если робот 1, то это старый, который ближе к концу линнии. - // Ему нельзя ехать если хоть одна из зон <= max(r2_pos, r2_lock) + кол-во пограничных зон - short border = robot2.dx.current_zone; - if (robot2_lock_zone > border) { - border = robot2_lock_zone; - } - border += LOCK_ZONE_BORDER; - - if (bar->zone <= border || dest_zone <= border) { - dest_zone = -1; - } - } else { - // если робот 2, то это новый, который ближе к началу линнии. - // Ему нельзя ехать если хоть одна из зон >= max(r2_pos, r2_lock) - кол-во пограничных зон - short border = robot1.dx.current_zone; - if (robot1_lock_zone < border) { - border = robot1_lock_zone; - } - border -= LOCK_ZONE_BORDER; - - if (bar->zone >= border || dest_zone >= border) { - dest_zone = -1; + // всего зон, куда можно сныкать барабаны 9 (8 промывок и выгрузка) + for (short i = 0; i < 9; i++) { + if (!zone_is_busy(zones[i])) { + return zones[i]; } } - } + return -1; + } else { + // если барабан ночной, то надо проверить выгрузку (туда выгружаются ночные барабаны) + if (bar->flags.is_night) { + if (!zone_is_busy(ZONE_LOAD_1)) { + return ZONE_LOAD_1; + } + return -1; + } - return dest_zone; + // дальше нужно проверить, что можно передвигать бочку + short dest_zone = -1; + + switch (bar->zone) { + case ZONE_LOAD_2: + // загрузка 2, только в нее можно грузить новые барабаны, нужно обезжиривание + if (!zone_is_busy(ZONE_DEGREASING)) { + dest_zone = ZONE_DEGREASING; + } + break; + + case ZONE_DEGREASING: + // обезжиривание, нужна промывка 1А + if (!zone_is_busy(ZONE_WASHING_1A)) { + dest_zone = ZONE_WASHING_1A; + } + break; + + case ZONE_WASHING_1A: + // промывка 1А, нужна промывка 1Б + if (!zone_is_busy(ZONE_WASHING_1B)) { + dest_zone = ZONE_WASHING_1B; + } + break; + + case ZONE_WASHING_1B: + // промывка 1Б, нужно травление (зоны 5-6) + if (etching_zone < 0) { + break; + } + if (!zone_is_busy((short)(ZONE_ETCHING_1 + etching_zone))) { + dest_zone = (short)(ZONE_ETCHING_1 + etching_zone); + } + break; + + case ZONE_ETCHING_1: + case ZONE_ETCHING_2: + // травление, нужна промывка 2А + if (!zone_is_busy(ZONE_WASHING_2A)) { + dest_zone = ZONE_WASHING_2A; + } + break; + + case ZONE_WASHING_2A: + // промывка 2А, нужна промывка 2Б + if (!zone_is_busy(ZONE_WASHING_2B)) { + dest_zone = ZONE_WASHING_2B; + } + break; + + case ZONE_WASHING_2B: + // промывка 2Б, нужно цинкование (зоны 9-16) + if (galvanizing_zone < 0) { + break; + } + if (!zone_is_busy((short)(ZONE_GALVANIZING_1 + galvanizing_zone))) { + dest_zone = (short)(ZONE_GALVANIZING_1 + galvanizing_zone); + } + break; + + case ZONE_GALVANIZING_1: + case ZONE_GALVANIZING_2: + case ZONE_GALVANIZING_3: + case ZONE_GALVANIZING_4: + case ZONE_GALVANIZING_5: + case ZONE_GALVANIZING_6: + case ZONE_GALVANIZING_7: + case ZONE_GALVANIZING_8: + // цинкование, требуется чтобы в зонах 17-22 было максимум 2 барабана (3 барабана для этой части линии - максимум) + if (!zone_is_busy(ZONE_WASHING_3A)) { + short count = 0; + // если зона 17 свободна, то диапазон начнется с 3Б + for (short i = ZONE_WASHING_3B; i <= (short)ZONE_UNLOAD; i++) { + if (zone_is_busy(i)) { + count++; + } + } + if (count < 3) { + dest_zone = ZONE_WASHING_3A; + } + } + break; + + case ZONE_WASHING_3A: + // промывка 3А, перекладываем в промывку 3Б + if (!zone_is_busy(ZONE_WASHING_3B)) { + dest_zone = ZONE_WASHING_3B; + } + break; + + case ZONE_WASHING_3B: + // это перед пассивацией, требует свободную промывку 4А и на всякий случай свободную пассивацию + if (!zone_is_busy(ZONE_PASSIVATION) && !zone_is_busy(ZONE_WASHING_4A)) { + dest_zone = ZONE_PASSIVATION; + } + // это атомарная операция, по идее вносить барабан в пассивацию нельзя + break; + + case ZONE_PASSIVATION: + // процесс пассивации, нужна промывка 4A + // чисто теоретически сюда никогда не попадем, но если вдруг выстрелит пусть будет + if (!zone_is_busy(ZONE_WASHING_4A)) { + dest_zone = ZONE_WASHING_4A; + } + break; + + case ZONE_WASHING_4A: + // промывка 4А, перекладываем в промывку 4Б + if (!zone_is_busy(ZONE_WASHING_4B)) { + dest_zone = ZONE_WASHING_4B; + } + break; + + case ZONE_WASHING_4B: + // процесс пассивации, нужна промывка 4B (зона 21) (потому что сейчас я в 4A) + if (!zone_is_busy(ZONE_UNLOAD)) { + dest_zone = ZONE_UNLOAD; + } + break; + + case ZONE_UNLOAD: + // последняя промывка, нужно разрешение на выгрузку + if (one_robot_mode && button_unload) { + // нужно промывку загрузку 0 + if (!zone_is_busy(ZONE_LOAD_1)) { + dest_zone = ZONE_LOAD_1; + } + } + break; + } + + if (!one_robot_mode) { + if (robot_id == 1) { + // если робот 1, то это старый, который ближе к концу линнии. + // Ему нельзя ехать если хоть одна из зон <= max(r2_pos, r2_lock) + кол-во пограничных зон + short border = robot2.dx.current_zone; + if (robot2_lock_zone > border) { + border = robot2_lock_zone; + } + border += LOCK_ZONE_BORDER; + + if (bar->zone <= border || dest_zone <= border) { + dest_zone = -1; + } + } else { + // если робот 2, то это новый, который ближе к началу линнии. + // Ему нельзя ехать если хоть одна из зон >= max(r2_pos, r2_lock) - кол-во пограничных зон + short border = robot1.dx.current_zone; + if (robot1_lock_zone < border) { + border = robot1_lock_zone; + } + border -= LOCK_ZONE_BORDER; + + if (bar->zone >= border || dest_zone >= border) { + dest_zone = -1; + } + } + } + + return dest_zone; + } } @@ -439,6 +466,11 @@ void create_operation(struct robot_code *code, const short barrel_id, const shor } } + // если ночной режим, то нужно сразу обновить флаг у барабана + if (hla_night_mode && barrel_id >= 0 && barrel_id < BARRELS_COUNT) { + barrels[barrel_id].flags.is_night = -1; + } + // первым делом добавляем команду опустить траверсу code->code[cmd_index++] = ROBOT_CMD_DOWN(); @@ -644,9 +676,9 @@ void create_operation(struct robot_code *code, const short barrel_id, const shor } code->code[cmd_index++] = ROBOT_CMD_END(); - code->PC = 0; - #ifdef EMULATOR printf("INFO: code length is %d\n", cmd_index); + debug_print_robot_code(code, robot_id, 0); #endif + code->PC = 0; }