From d5c7c0e645931d5b9305406d655e51cd8ce5fb77 Mon Sep 17 00:00:00 2001 From: VladislavOstapov Date: Mon, 2 Jan 2023 00:20:23 +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=D1=80=D0=B5=D0=B6=D0=B8=D0=BC?= =?UTF-8?q?=D0=B0=20=D0=B4=D0=B2=D1=83=D1=85=20=D1=80=D0=BE=D0=B1=D0=BE?= =?UTF-8?q?=D1=82=D0=BE=D0=B2?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- CMakeLists.txt | 6 +- emulator.cpp | 59 ++++++++------ emulator.h | 2 - robot.cpp | 9 +++ scheduler.c | 148 ++++++++++++++++++++++++++-------- utils.c | 210 ++++++++++++++++++++++++++++++++++++------------- utils.h | 15 +++- 7 files changed, 328 insertions(+), 121 deletions(-) diff --git a/CMakeLists.txt b/CMakeLists.txt index afc4fbc..4823333 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -1,10 +1,10 @@ cmake_minimum_required(VERSION 3.21) -project(sdp_sheduler C CXX) +project(sdp-scheduler C CXX) set(CMAKE_C_STANDARD 17) set(CMAKE_CXX_STANDARD 17) -add_executable(sdp_sheduler scheduler.c emulator.cpp emulator.h robot.cpp robot.h utils.h utils.c) +add_executable(sdp-scheduler scheduler.c emulator.cpp emulator.h robot.cpp robot.h utils.h utils.c) -add_executable(remote_listener remote_listener.cpp) +add_executable(remote-listener remote_listener.cpp) add_definitions(-DEMULATOR=1) diff --git a/emulator.cpp b/emulator.cpp index 85c0919..f390b58 100644 --- a/emulator.cpp +++ b/emulator.cpp @@ -43,9 +43,11 @@ short hla_time_washing_2 = 20; char hla_robot1_en = 1; -char hla_robot2_en = 0; +char hla_robot2_en = 1; char one_robot_mode = (char)(hla_robot1_en ^ hla_robot2_en); -char _scheduler_en = 1; +char scheduler_en = 1; +char scheduler_start_signal = 1; +char auto_mode_pause = 0; short etching_zone = 0, galvanizing_zone = 0; @@ -151,7 +153,31 @@ static void showAll() { image_init(); image_draw_borders(); - char tmp[16]; + char tmp[64]; + sprintf(tmp, "Lock1=%02d Lock2=%02d", robot1_lock_zone, robot2_lock_zone); + image_insert_sprite(0, 2, tmp); + + int barrels_count = 0, barrels_time = 0; + static int max_time = 0; + for (int i = 0; i < BARRELS_COUNT; i++) { + if (barrels[i].flags.is_exist) { + barrels_count++; + if (barrels[i].zone <= ZONE_LOAD_2 || barrels[i].zone == ZONE_PASSIVATION || barrels[i].zone == ZONE_UNLOAD) { + continue; + } + if (barrels[i].flags.robot == 0 && barrels[i].software_timer < 0) { + // время отрицательное, вычитаем его чтобы убрать минус + barrels_time -= barrels[i].software_timer; + } + } + } + + if (max_time < barrels_time) { + max_time = barrels_time; + } + + sprintf(tmp, "barrels=%02d time=%04d max_time=%04d", barrels_count, barrels_time, max_time); + image_insert_sprite(0, 25, tmp); // рисование бочек for (int i = 0; i < BARRELS_COUNT; i++) { @@ -199,10 +225,10 @@ static void open_socket() { } serv_addr.sin_family = AF_INET; - serv_addr.sin_port = htons(40090); + serv_addr.sin_port = htons(40000); // Convert IPv4 and IPv6 addresses from text to binary form - if (inet_pton(AF_INET, "192.168.0.160", &serv_addr.sin_addr) <= 0) { + if (inet_pton(AF_INET, "127.0.0.1", &serv_addr.sin_addr) <= 0) { printf("\nInvalid address/ Address not supported \n"); exit(-1); } @@ -213,26 +239,6 @@ static void open_socket() { } } -static struct barrel make_barrel(short flags, short zone, short timer) { - barrel b; - b.flags.raw_word = flags; - b.zone = zone; - b.software_timer = timer; - b.time_degreasing = hla_time_degreasing; - b.time_washing_1a = hla_time_washing_1a; - b.time_washing_1b = hla_time_washing_1b; - b.time_etching = hla_time_etching; - b.time_washing_2a = hla_time_washing_2a; - b.time_washing_2b = hla_time_washing_2b; - b.time_galvanizing = hla_time_galvanizing; - b.time_washing_3a = hla_time_washing_3a; - b.time_washing_3b = hla_time_washing_3b; - b.time_passivation = hla_time_passivation; - b.time_washing_4a = hla_time_washing_4a; - b.time_washing_4b = hla_time_washing_4b; - return b; -} - int main() { open_socket(); @@ -240,6 +246,9 @@ int main() { const char* message = nullptr; while (true) { _scheduler_software_timer = 1; + + button_load = 1; + robot_main(); scheduler_main(); send_str("\033c"); diff --git a/emulator.h b/emulator.h index 3551414..f655ca8 100644 --- a/emulator.h +++ b/emulator.h @@ -50,8 +50,6 @@ extern short robot1_lock_zone; extern short robot2_lock_zone; -extern char _scheduler_en; - #ifdef __cplusplus } #endif diff --git a/robot.cpp b/robot.cpp index b41a667..08e82f5 100644 --- a/robot.cpp +++ b/robot.cpp @@ -108,6 +108,15 @@ static void emulate_robot(robot_code &code, robot_regs& r, char robot_id) { code.PC++; break; + case ROBOT_CMD_SET_LOCK_ZONE_code: + if (robot_id == 1) { + robot1_lock_zone = cmd_arg; + } else { + robot2_lock_zone = cmd_arg; + } + code.PC++; + break; + case ROBOT_CMD_CORRECT_AXIS_code: if (cmd_arg == ROBOT_AXIS_X) { std::cout << "robot " << robot_id << " correct axis X..." << std::endl; diff --git a/scheduler.c b/scheduler.c index 42eaf12..6a9b371 100644 --- a/scheduler.c +++ b/scheduler.c @@ -11,7 +11,6 @@ struct scheduler_task { short scheduler_find_task(const struct scheduler_task* tasks, const short curr_pos) { - // TODO добавить поддержку ночного режима и режима двух роботов // для начала надо найти максимальный приоритет у операций short max_priority = -1; for (short i = 0; i < BARRELS_COUNT; i++) { @@ -30,9 +29,7 @@ short scheduler_find_task(const struct scheduler_task* tasks, const short curr_p short left = -1, right = -1; for (short i = 0; i < BARRELS_COUNT; i++) { short target = tasks[i].start_zone; // фактическая зона откуда тащить барабан - if (tasks[i].dest_zone == -2) { - target = 18; - } else if (tasks[i].dest_zone < 0) { + if (tasks[i].dest_zone < 0) { continue; } @@ -104,9 +101,34 @@ void schedule_one_robot(const struct scheduler_task* tasks, const struct robot_r void scheduler_main() #endif { + if (scheduler_start_signal) { + for (short i = 0; i < BARRELS_COUNT; i++) { + // после рестарта планировщика надо всем барабаном убрать робота, + // всех кто сверху удалить, + barrels[i].flags.robot = 0; + if (barrels[i].flags.is_up) { + barrels[i].flags.is_up = 0; + barrels[i].flags.is_exist = 0; + } + } + scheduler_start_signal = 0; + } - if (_scheduler_en) { - // тут возможна только вставка барабанов + if (scheduler_en) { + + // программный таймер, применяется ко всем существующим барабанам + if (_scheduler_software_timer) { + _scheduler_software_timer = 0; + for (int i = 0; i < 10; i++) { + if (barrels[i].flags.is_exist && barrels[i].software_timer > -9999) { + barrels[i].software_timer--; + } + } + } + + // кнопки на линии + + // кнопка загрузки барабана if (button_load) { if (!zone_is_busy(1)) { for (int i = 0; i < BARRELS_COUNT; i++) { @@ -145,37 +167,95 @@ void scheduler_main() remove_barrel_from_zone(22); } - // таймер, применяется ко всем существующим барабанам - if (_scheduler_software_timer) { - _scheduler_software_timer = 0; - for (int i = 0; i < 10; i++) { - if (barrels[i].flags.is_exist && barrels[i].software_timer > -9999) { - barrels[i].software_timer--; - } - } - } + 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 ((hla_robot1_en && robot1_code.PC < 0) || (hla_robot2_en && robot2_code.PC < 0)) { - 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); - if (tasks[i].dest_zone >= 0) { - tasks[i].priority = get_operation_priority(i); - } - } + if (one_robot_mode) { + // режим одного робота + char robot_id = 0; + if (hla_robot1_en && robot1_code.PC < 0) { + robot_id = 1; + } else if (robot2_code.PC < 0 && hla_robot2_en) { + robot_id = 2; + } - if (one_robot_mode) { - // режим одного робота - if (hla_robot1_en && robot1_code.PC < 0) { - schedule_one_robot(tasks, &robot1, &robot1_code, 1); - } else if (robot2_code.PC < 0 && hla_robot2_en) { - schedule_one_robot(tasks, &robot2, &robot2_code, 2); - } - } else { + if (robot_id != 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_id); + if (tasks[i].dest_zone >= 0) { + tasks[i].priority = get_operation_priority(i); + } + } + if (robot_id == 1) { + schedule_one_robot(tasks, &robot1, &robot1_code, 1); + } else { + schedule_one_robot(tasks, &robot2, &robot2_code, 2); + } + } + } else { + // а вот для режима двух роботов все интересно + // для каждого робота нужно получить свой список задач + // и надо еще сделать так, чтобы роботы не столкнулись + + // логика для того, чтобы роботы не столкнулись в начале + 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 { + // отдельно просчитаем все для первого робота + 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, 1); + if (tasks[i].dest_zone >= 0) { + tasks[i].priority = get_operation_priority(i); + } + } + + schedule_one_robot(tasks, &robot1, &robot1_code, 1); + } + + // и отдельно для второго + if (robot2_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, 2); + if (tasks[i].dest_zone >= 0) { + tasks[i].priority = get_operation_priority(i); + } + } + + + schedule_one_robot(tasks, &robot2, &robot2_code, 2); + } + } + } } } } diff --git a/utils.c b/utils.c index fe0dee2..657f6fe 100644 --- a/utils.c +++ b/utils.c @@ -9,6 +9,10 @@ #include "main.h" #endif + +#define LOCK_ZONE_BORDER 1 + + char zone_is_busy(short zone) { for (short i = 0; i < BARRELS_COUNT; i++) { if (barrels[i].flags.is_exist && barrels[i].zone == zone) { @@ -22,7 +26,11 @@ char zone_is_busy(short zone) { // TODO обновить метод для работы с двумя роботами // вернет можно ли ехать и главное куда ехать, если можно (нельзя если вернулось значение < 0) // -1 вернет что перемещать нельзя -short can_move(struct barrel* bar) { +short can_move(struct barrel *bar, char robot_id) { + if (robot_id != 1 && robot_id != 2) { + return -1; + } + // сразу отсекаем варианты, при которых невозможно переместить барабан if (!bar->flags.is_exist) { return -1; @@ -36,33 +44,28 @@ short can_move(struct barrel* bar) { return -2; } -#ifdef EMULATOR - if (!one_robot_mode) { - printf("WARMING: нет проверки того, что для перемещения барабана не мешает второй робот\n"); - } -#endif - // дальше нужно проверить, что можно передвигать бочку + short dest_zone = -1; switch (bar->zone) { case ZONE_LOAD_2: // загрузка 2, только в нее можно грузить новые барабаны, нужно обезжиривание if (!zone_is_busy(ZONE_DEGREASING)) { - return ZONE_DEGREASING; + dest_zone = ZONE_DEGREASING; } break; case ZONE_DEGREASING: // обезжиривание, нужна промывка 1А if (!zone_is_busy(ZONE_WASHING_1A)) { - return ZONE_WASHING_1A; + dest_zone = ZONE_WASHING_1A; } break; case ZONE_WASHING_1A: // промывка 1А, нужна промывка 1Б if (!zone_is_busy(ZONE_WASHING_1B)) { - return ZONE_WASHING_1B; + dest_zone = ZONE_WASHING_1B; } break; @@ -72,7 +75,7 @@ short can_move(struct barrel* bar) { break; } if (!zone_is_busy((short)(ZONE_ETCHING_1 + etching_zone))) { - return (short)(ZONE_ETCHING_1 + etching_zone); + dest_zone = (short)(ZONE_ETCHING_1 + etching_zone); } break; @@ -80,14 +83,14 @@ short can_move(struct barrel* bar) { case ZONE_ETCHING_2: // травление, нужна промывка 2А if (!zone_is_busy(ZONE_WASHING_2A)) { - return ZONE_WASHING_2A; + dest_zone = ZONE_WASHING_2A; } break; case ZONE_WASHING_2A: // промывка 2А, нужна промывка 2Б if (!zone_is_busy(ZONE_WASHING_2B)) { - return ZONE_WASHING_2B; + dest_zone = ZONE_WASHING_2B; } break; @@ -97,7 +100,7 @@ short can_move(struct barrel* bar) { break; } if (!zone_is_busy((short)(ZONE_GALVANIZING_1 + galvanizing_zone))) { - return (short)(ZONE_GALVANIZING_1 + galvanizing_zone); + dest_zone = (short)(ZONE_GALVANIZING_1 + galvanizing_zone); } break; @@ -119,7 +122,7 @@ short can_move(struct barrel* bar) { } } if (count < 3) { - return ZONE_WASHING_3A; + dest_zone = ZONE_WASHING_3A; } } break; @@ -127,14 +130,14 @@ short can_move(struct barrel* bar) { case ZONE_WASHING_3A: // промывка 3А, перекладываем в промывку 3Б if (!zone_is_busy(ZONE_WASHING_3B)) { - return 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)) { - return ZONE_PASSIVATION; + dest_zone = ZONE_PASSIVATION; } // это атомарная операция, по идее вносить барабан в пассивацию нельзя break; @@ -143,21 +146,21 @@ short can_move(struct barrel* bar) { // процесс пассивации, нужна промывка 4A // чисто теоретически сюда никогда не попадем, но если вдруг выстрелит пусть будет if (!zone_is_busy(ZONE_WASHING_4A)) { - return ZONE_WASHING_4A; + dest_zone = ZONE_WASHING_4A; } break; case ZONE_WASHING_4A: // промывка 4А, перекладываем в промывку 4Б if (!zone_is_busy(ZONE_WASHING_4B)) { - return ZONE_WASHING_4B; + dest_zone = ZONE_WASHING_4B; } break; case ZONE_WASHING_4B: // процесс пассивации, нужна промывка 4B (зона 21) (потому что сейчас я в 4A) if (!zone_is_busy(ZONE_UNLOAD)) { - return ZONE_UNLOAD; + dest_zone = ZONE_UNLOAD; } break; @@ -166,12 +169,41 @@ short can_move(struct barrel* bar) { if (one_robot_mode && button_unload) { // нужно промывку загрузку 0 if (!zone_is_busy(ZONE_LOAD_1)) { - return ZONE_LOAD_1; + dest_zone = ZONE_LOAD_1; } } break; } - return -1; + + 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; } @@ -192,7 +224,7 @@ short get_operation_priority(short barrel_id) { continue; } if (barrels[i].flags.is_exist && barrels[i].zone >= ZONE_GALVANIZING_1 && barrels[i].zone <= ZONE_GALVANIZING_8) { - if (can_move(barrels + i) >= 0) { + if (can_move(barrels + i, 1) >= 0) { is_not_one = 1; // чем больше у барабана время ожидания тем меньше у него число if (barrels[i].software_timer < barrels[barrel_id].software_timer) { @@ -224,7 +256,7 @@ short get_operation_priority(short barrel_id) { } if (barrels[i].flags.is_exist && barrels[i].zone >= ZONE_ETCHING_1 && barrels[i].zone <= ZONE_ETCHING_2) { - if (can_move(barrels + i) >= 0) { + if (can_move(barrels + i, 1) >= 0) { if (barrels[i].software_timer < barrels[barrel_id].software_timer) { return 0; // у этого барабана больше время ожидания (число меньше), значит приоритет 0 } else { @@ -296,6 +328,10 @@ void debug_print_robot_code(const struct robot_code* code, const short robot_id) printf(" set barrel timer %d secs\n", cmd_arg); break; + case ROBOT_CMD_SET_LOCK_ZONE_code: + printf(" set lock zone %d\n", cmd_arg); + break; + case ROBOT_CMD_CORRECT_AXIS_code: if (cmd_arg == ROBOT_AXIS_X) { printf(" correct axis: X\n"); @@ -325,44 +361,50 @@ void debug_print_robot_code(const struct robot_code* code, const short robot_id) /* -=== ЦИКЛОГРАММА ПЕРЕТАСКИВАНИЯ БАРАБАНА === +=== циклограмма перетаскивания барабана === // NOTE первой командой на любую транзакцию должна стоять команда опустить траверсу (в 22 зоне мы никогда не закончим, за нее не беспокоится) +если режим двух роботов: + если команда для робота 1: + robot1_lock_zone = min(зона изъятия, зона назначения) + иначе: + robot2_lock_zone = max(зона изъятия, зона назначения) * опустить траверсу -если зона изъятия != промывка 3б - если текущая зона != зона изъятия - если зона изъятия == 22 +если зона изъятия != промывка 3б: + если текущая зона != зона изъятия: + если зона изъятия == 22: * встать на смещенную * поднять траверсу - * съебать в 21 зону + * уехать в 22 зону * встать на смещенную - * опустить траверсу не до конца - * съебать в 22 зону - иначе - * съебать в зону изъятия - * поднять траверсу с барабаном - если зона изъятия != 22 и зона изъятия != 1: + * опустить траверсу + * встать в точную + иначе: + * уехать в зону изъятия + если режим двух роботов: + * установить новую lock-зону + * поднять траверсу + если зона изъятия != 22 и зона изъятия >= 2: * ждать скапывания (зависит от зоны) * ехать в зону назначения - если зона назначения == 22 - * опустить траверсу не до конца с барабаном - * съебать в 21 зону - если ЭНКОДЕРЫ СТАРЫЕ (по умолчанию) + если зона назначения == 22: + * опустить траверсу + * уехать в 21 зону + если ЭНКОДЕРЫ СТАРЫЕ (по умолчанию): * поднять траверсу - * опустить траверсу - иначе - * опустить траверсу с барабаном - если зона назначения != 0 - * установить время ожидания барабана (зависит от зоны) -иначе - если текущая зона != промывка 3б - * съебать в промывку 3б - * поднять траверсу с барабаном - * съебать в пассивацию - * опустить траверсу с барабаном - * поставить время ожидания барабана в <время пассивации> - * поднять траверсу с барабаном - * съебать в зону промывка 4а - * опустить траверсу с барабаном + * опустить траверсу + + если зона назначения != 22 и зона назначения != 0: + * установить время ожидания барабана (зависит от зоны) +иначе: + если текущая зона != промывка 3б: + * уехать в промывку 3б + * поднять траверсу + * уехать в пассивацию + * опустить траверсу + * ждать <время пассивации> + * поднять траверсу + * уехать в зону промывка 4а + * опустить траверсу * установить время ожидания барабана (для промывки 4а) */ void create_operation(struct robot_code *code, const short barrel_id, const short start_zone, const short dest_zone, @@ -371,6 +413,28 @@ void create_operation(struct robot_code *code, const short barrel_id, const shor code->barrel_id = barrel_id; short cmd_index = 0; + if (!one_robot_mode) { + if (robot_id == 1) { + short tmp = dest_zone; + if (start_zone < dest_zone) { + tmp = start_zone; + } + if (tmp > ZONE_WASHING_3B) { + tmp = ZONE_WASHING_3B; + } + robot1_lock_zone = tmp; + } else { + short tmp = dest_zone; + if (start_zone > dest_zone) { + tmp = start_zone; + } + if (tmp < ZONE_WASHING_2B) { + tmp = ZONE_WASHING_2B; + } + robot2_lock_zone = tmp; + } + } + // первым делом добавляем команду опустить траверсу code->code[cmd_index++] = ROBOT_CMD_DOWN(); @@ -409,6 +473,37 @@ void create_operation(struct robot_code *code, const short barrel_id, const shor } } + // теперь обновляем LOCK-зону + 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-зону нельзя двигать к началу линии + if (tmp > robot1_lock_zone) { + code->code[cmd_index++] = ROBOT_CMD_SET_LOCK_ZONE(dest_zone); + } + } else { + // ставим lock-зону только если она ближе к началу линии + short tmp = dest_zone; + + // lock-зона этого робота не может выходить за промывку 3А, потому что это не имеет смысла + if (tmp < ZONE_WASHING_2B) { + tmp = ZONE_WASHING_2B; + } + + // в любом случае lock-зону нельзя двигать к концу линии + if (tmp < robot2_lock_zone) { + code->code[cmd_index++] = ROBOT_CMD_SET_LOCK_ZONE(dest_zone); + } + } + } + code->code[cmd_index++] = ROBOT_CMD_UP_WITH_BARREL(); // теперь надо определиться с тем, сколько ждать скапывания @@ -526,16 +621,19 @@ void create_operation(struct robot_code *code, const short barrel_id, const shor } if (!one_robot_mode) { - if (robot_id == 1) { + if (robot_id == 2) { if (dest_zone >= ZONE_GALVANIZING_1) { // из промывки 2б он перекладывал, пусть едет в промывку 2а + code->code[cmd_index++] = ROBOT_CMD_SET_LOCK_ZONE(ZONE_WASHING_2A); code->code[cmd_index++] = ROBOT_CMD_MOVE_TO_ZONE(ZONE_WASHING_2A); } else if (dest_zone < ZONE_DEGREASING) { + code->code[cmd_index++] = ROBOT_CMD_SET_LOCK_ZONE(ZONE_DEGREASING); code->code[cmd_index++] = ROBOT_CMD_MOVE_TO_ZONE(ZONE_DEGREASING); } - } else if (robot_id == 2) { + } else if (robot_id == 1) { if (dest_zone <= ZONE_WASHING_3A) { // чтобы из этой зоны можно было переложить барабан первому роботу + code->code[cmd_index++] = ROBOT_CMD_SET_LOCK_ZONE(ZONE_WASHING_4A); code->code[cmd_index++] = ROBOT_CMD_MOVE_TO_ZONE(ZONE_WASHING_4A); } } diff --git a/utils.h b/utils.h index 2be5e39..13dc6ae 100644 --- a/utils.h +++ b/utils.h @@ -184,6 +184,15 @@ extern short hla_time_washing_2; extern char hla_robot1_en; extern char hla_robot2_en; + +// пауза для автоматического режима, при паузе идут таймера, но команды не могут быть выданы +extern char auto_mode_pause; + +// разрешение на работу планировщика +extern char scheduler_en; + +// сигнал инициализации планировщика, должен быть установлен по фронту разрешения на работу +extern char scheduler_start_signal; #else #define barrels ((struct barrel*)barrels_array) #define robot1 (*((struct robot_regs*)&robot1_reg_mx)) @@ -213,6 +222,8 @@ extern char hla_robot2_en; #define hla_robot1_en _c_hla_robot1_en #define hla_robot2_en _c_hla_robot2_en + +#define auto_mode_pause _c_auto_mode_pause #endif @@ -225,6 +236,7 @@ extern char hla_robot2_en; #define ROBOT_CMD_DOWN_code (short)0x5000 #define ROBOT_CMD_WAIT_code (short)0x6000 #define ROBOT_CMD_TMR_SET_code (short)0x7000 +#define ROBOT_CMD_SET_LOCK_ZONE_code (short)0x8000 #define ROBOT_CMD_CORRECT_AXIS_code (short)0x9000 #define ROBOT_CMD_INC_ZONE_code (short)0xA000 @@ -259,6 +271,7 @@ extern char hla_robot2_en; #define ROBOT_CMD_WAIT(time) ((ROBOT_CMD_WAIT_code) | (short)(time & 0x0FFF)) #define ROBOT_CMD_TMR_SET(time) ((ROBOT_CMD_TMR_SET_code) | (short)(time & 0x0FFF)) +#define ROBOT_CMD_SET_LOCK_ZONE(zone) ((ROBOT_CMD_SET_LOCK_ZONE_code) | (short)(zone & 0x00FF)) #define ROBOT_CMD_CORRECT_AXIS(axis) ((short)(ROBOT_CMD_CORRECT_AXIS_code) | (short)axis) #define ROBOT_CMD_INC_ZONE(arg) ((ROBOT_CMD_INC_ZONE_code) | (short)(arg)) @@ -326,7 +339,7 @@ extern char hla_robot2_en; char zone_is_busy(short zone); -short can_move(struct barrel* bar); +short can_move(struct barrel *bar, char robot_id); short get_operation_priority(short barrel_id); char remove_barrel_from_zone(short zone);