From 353aad9bec9e82b687396e989e4d58e92cb2d459 Mon Sep 17 00:00:00 2001 From: VladislavOstapov Date: Sun, 29 Jan 2023 13:04:06 +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=D0=BC=D0=B0=D0=BD=D0=B4?= =?UTF-8?q?=D1=8B=20=D0=BA=D0=BE=D1=80=D1=80=D0=B5=D0=BA=D1=86=D0=B8=D0=B8?= =?UTF-8?q?,=20=D0=BF=D0=BE=D1=82=D0=B5=D0=BD=D1=86=D0=B8=D0=B0=D0=BB?= =?UTF-8?q?=D1=8C=D0=BD=D0=BE=20=D1=83=D0=B1=D1=80=D0=B0=D0=BD=D1=8B=20?= =?UTF-8?q?=D0=BE=D1=88=D0=B8=D0=B1=D0=BA=D0=B8=20=D0=BF=D1=80=D0=B5=D0=BE?= =?UTF-8?q?=D0=B1=D1=80=D0=B0=D0=B7=D0=BE=D0=B2=D0=B0=D0=BD=D0=B8=D1=8F=20?= =?UTF-8?q?robot=20code,=20=D0=B8=D1=81=D0=BF=D1=80=D0=B0=D0=B2=D0=BB?= =?UTF-8?q?=D0=B5=D0=BD=20=D0=B1=D0=B0=D0=B3=20=D1=81=20=D0=B7=D0=B0=D0=B2?= =?UTF-8?q?=D0=B8=D1=81=D1=88=D0=B8=D0=BC=20=D1=80=D0=BE=D0=B1=D0=BE=D1=82?= =?UTF-8?q?=D0=BE=D0=BC?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- emulator.cpp | 2 ++ robot.cpp | 4 +-- scheduler.c | 84 ++++++++++++++++++++++++++++++++++++++++++---------- utils.c | 2 +- utils.h | 59 +++++++++++++++++++----------------- 5 files changed, 105 insertions(+), 46 deletions(-) diff --git a/emulator.cpp b/emulator.cpp index 4d1ec73..f36aa2b 100644 --- a/emulator.cpp +++ b/emulator.cpp @@ -20,6 +20,7 @@ struct robot_regs robot1; struct robot_regs robot2; char _scheduler_software_timer = 0; +short scheduler_correction_stage = 0; // кнопка загрузки в зоне 0, означает что барабан надо изъять из этой загрузки (а перед этим создать) char button_load = 0; @@ -323,6 +324,7 @@ int main() { robot1.dx.current_zone = 5; robot2.mx.correct_sensor = false; robot2.dx.current_zone = 3; + hla_correct_command = 1; #else robot1.mx.correct_sensor = true; robot2.mx.correct_sensor = true; diff --git a/robot.cpp b/robot.cpp index 675db8d..4dfff89 100644 --- a/robot.cpp +++ b/robot.cpp @@ -59,7 +59,7 @@ static void emulate_robot(robot_code &code, robot_regs& r, char robot_id) { const auto cmd_arg = (short)(code.code[code.PC] & (~ROBOT_CMD_MASK)); - switch (code.code[code.PC] & (short)ROBOT_CMD_MASK) { + switch (code.code[code.PC] & ROBOT_CMD_MASK) { case ROBOT_CMD_MOVE_TO_ZONE_code: // двигаемся в сторону цели { @@ -149,7 +149,7 @@ static void emulate_robot(robot_code &code, robot_regs& r, char robot_id) { case ROBOT_CMD_CORRECT_AXIS_code: if (cmd_arg == ROBOT_AXIS_X) { std::cout << "robot " << robot_id << " correct axis X..." << std::endl; - r.dz.current_zone = 0; + r.dx.current_zone = 0; r.mx.correct_status = true; } else if (cmd_arg == ROBOT_AXIS_Z) { std::cout << "robot " << robot_id << " correct axis Z..." << std::endl; diff --git a/scheduler.c b/scheduler.c index 2174cd1..2a071e2 100644 --- a/scheduler.c +++ b/scheduler.c @@ -98,6 +98,10 @@ void scheduler_main() #endif { if (scheduler_start_signal) { + scheduler_correction_stage = 0; + robot1_code.PC = -1; + robot2_code.PC = -1; + for (short i = 0; i < BARRELS_COUNT; i++) { // после рестарта планировщика надо всем барабаном убрать робота, // всех кто сверху удалить, @@ -176,24 +180,72 @@ void scheduler_main() } if (hla_correct_command) { - if (hla_robot2_en && (!robot2.mx.correct_sensor || !robot2.mz.correct_status)) { - 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; - } - } else if (hla_robot1_en && (!robot1.mx.correct_sensor || !robot1.mz.correct_status)) { - 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; + 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 { - hla_correct_command = 0; + 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 ((hla_robot1_en && robot1_code.PC < 0) || (hla_robot2_en && robot2_code.PC < 0)) { diff --git a/utils.c b/utils.c index cc26719..b0a13f6 100644 --- a/utils.c +++ b/utils.c @@ -366,7 +366,7 @@ void debug_print_robot_code(const struct robot_code *code, const short robot_id, break; } - switch ((short)(code->code[i] & (short)ROBOT_CMD_MASK)) { + switch (code->code[i] & ROBOT_CMD_MASK) { case ROBOT_CMD_MOVE_TO_ZONE_code: if (cmd_arg & ROBOT_ZONE_PARKING) { dprintf(fd, " move to parking (with barrel: %d)\n", (cmd_arg & ROBOT_WITH_BARREL) != 0); diff --git a/utils.h b/utils.h index 368e971..d5f5457 100644 --- a/utils.h +++ b/utils.h @@ -146,7 +146,7 @@ struct robot_code { * * формат команды: (команда, старший байт) [младший байт, аргумент команды (если есть)] [слово, аргумент если команда требует] */ - short code[16]; // формат кода: [команды] <команда 0> + unsigned short code[16]; // формат кода: [команды] <команда 0> }; @@ -203,6 +203,9 @@ extern char scheduler_en; // сигнал инициализации планировщика, должен быть установлен по фронту разрешения на работу extern char scheduler_start_signal; + +extern short scheduler_correction_stage; + #else #define barrels ((struct barrel*)barrels_array) #define robot1 (*((struct robot_regs*)&robot1_reg_mx)) @@ -240,31 +243,33 @@ extern char scheduler_start_signal; #define hla_night_mode _c_hla_night_mode #define hla_correct_command _c_hla_correct_command +#define scheduler_correction_stage _c_scheduler_correction_stage + #endif extern const short NIGHT_ZONES[9]; -#define ROBOT_CMD_MASK (short)0xF000 -#define ROBOT_CMD_END_code (short)0x0000 -#define ROBOT_CMD_MOVE_TO_ZONE_code (short)0x1000 -#define ROBOT_CMD_MOVE_OFF_code (short)0x2000 -#define ROBOT_CMD_MOVE_ACCURATE_code (short)0x3000 -#define ROBOT_CMD_UP_code (short)0x4000 -#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 +#define ROBOT_CMD_MASK 0xF000U +#define ROBOT_CMD_END_code 0x0000U +#define ROBOT_CMD_MOVE_TO_ZONE_code 0x1000U +#define ROBOT_CMD_MOVE_OFF_code 0x2000U +#define ROBOT_CMD_MOVE_ACCURATE_code 0x3000U +#define ROBOT_CMD_UP_code 0x4000U +#define ROBOT_CMD_DOWN_code 0x5000U +#define ROBOT_CMD_WAIT_code 0x6000U +#define ROBOT_CMD_TMR_SET_code 0x7000U +#define ROBOT_CMD_SET_LOCK_ZONE_code 0x8000U +#define ROBOT_CMD_CORRECT_AXIS_code 0x9000U +#define ROBOT_CMD_INC_ZONE_code 0xA000U // перемещение с барабаном -#define ROBOT_WITH_BARREL 0x0800 +#define ROBOT_WITH_BARREL 0x0800U // опции зон -#define ROBOT_ZONE_ETCH 0x0400 -#define ROBOT_ZONE_GAL 0x0200 -#define ROBOT_ZONE_PARKING 0x0100 +#define ROBOT_ZONE_ETCH 0x0400U +#define ROBOT_ZONE_GAL 0x0200U +#define ROBOT_ZONE_PARKING 0x0100U // опции коррекции осей @@ -275,24 +280,24 @@ extern const short NIGHT_ZONES[9]; // макросы для генерации команд #define ROBOT_CMD_END() (ROBOT_CMD_END_code) -#define ROBOT_CMD_MOVE_TO_ZONE(zone) ((ROBOT_CMD_MOVE_TO_ZONE_code) | (short)(zone & 0x00FF)) -#define ROBOT_CMD_MOVE_TO_ZONE_WITH_BARREL(zone) ((ROBOT_CMD_MOVE_TO_ZONE_code) | (short)(ROBOT_WITH_BARREL) | (short)(zone & 0x00FF)) -#define ROBOT_CMD_MOVE_TO_PARKING() ((short)(ROBOT_CMD_MOVE_TO_ZONE_code) | (short)(ROBOT_ZONE_PARKING)) +#define ROBOT_CMD_MOVE_TO_ZONE(zone) (ROBOT_CMD_MOVE_TO_ZONE_code | (unsigned short)(zone & 0x00FF)) +#define ROBOT_CMD_MOVE_TO_ZONE_WITH_BARREL(zone) (ROBOT_CMD_MOVE_TO_ZONE_code | ROBOT_WITH_BARREL | (unsigned short)(zone & 0x00FF)) +#define ROBOT_CMD_MOVE_TO_PARKING() (ROBOT_CMD_MOVE_TO_ZONE_code | ROBOT_ZONE_PARKING) #define ROBOT_CMD_MOVE_OFF() (ROBOT_CMD_MOVE_OFF_code) #define ROBOT_CMD_MOVE_ACCURATE() (ROBOT_CMD_MOVE_ACCURATE_code) #define ROBOT_CMD_UP() (ROBOT_CMD_UP_code) -#define ROBOT_CMD_UP_WITH_BARREL() ((ROBOT_CMD_UP_code) | (short)(ROBOT_WITH_BARREL)) +#define ROBOT_CMD_UP_WITH_BARREL() (ROBOT_CMD_UP_code | ROBOT_WITH_BARREL) #define ROBOT_CMD_DOWN() (ROBOT_CMD_DOWN_code) -#define ROBOT_CMD_DOWN_WITH_BARREL() ((ROBOT_CMD_DOWN_code) | (short)(ROBOT_WITH_BARREL)) +#define ROBOT_CMD_DOWN_WITH_BARREL() (ROBOT_CMD_DOWN_code | ROBOT_WITH_BARREL) -#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_WAIT(time) (ROBOT_CMD_WAIT_code | (unsigned short)(time & 0x0FFF)) +#define ROBOT_CMD_TMR_SET(time) ((ROBOT_CMD_TMR_SET_code) | (unsigned short)(time & 0x0FFF)) +#define ROBOT_CMD_SET_LOCK_ZONE(zone) ((ROBOT_CMD_SET_LOCK_ZONE_code) | (unsigned 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)) +#define ROBOT_CMD_CORRECT_AXIS(axis) (ROBOT_CMD_CORRECT_AXIS_code | (unsigned short)axis) +#define ROBOT_CMD_INC_ZONE(arg) (ROBOT_CMD_INC_ZONE_code | (unsigned short)(arg)) /* ======================== DISABLED ZONES DEFS ======================== */