VladislavOstapov/sdp-scheduler
Archived
1
0
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity

Исправление команды коррекции, потенциально убраны ошибки преобразования robot code, исправлен баг с зависшим роботом

Browse Source
This commit is contained in:
VladislavOstapov 2023-01-29 13:04:06 +03:00
parent d90b31cc7a
commit 353aad9bec
5 changed files with 105 additions and 46 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

2
emulator.cpp
View File

@ -20,6 +20,7 @@ struct robot_regs robot1;
struct robot_regs robot2; struct robot_regs robot2;
char _scheduler_software_timer = 0; char _scheduler_software_timer = 0;
short scheduler_correction_stage = 0;
// кнопка загрузки в зоне 0, означает что барабан надо изъять из этой загрузки (а перед этим создать) // кнопка загрузки в зоне 0, означает что барабан надо изъять из этой загрузки (а перед этим создать)
char button_load = 0; char button_load = 0;
@ -323,6 +324,7 @@ int main() {
robot1.dx.current_zone = 5; robot1.dx.current_zone = 5;
robot2.mx.correct_sensor = false; robot2.mx.correct_sensor = false;
robot2.dx.current_zone = 3; robot2.dx.current_zone = 3;
hla_correct_command = 1;
#else #else
robot1.mx.correct_sensor = true; robot1.mx.correct_sensor = true;
robot2.mx.correct_sensor = true; robot2.mx.correct_sensor = true;

4
robot.cpp
View File

@ -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)); 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: 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: case ROBOT_CMD_CORRECT_AXIS_code:
if (cmd_arg == ROBOT_AXIS_X) { if (cmd_arg == ROBOT_AXIS_X) {
std::cout << "robot " << robot_id << " correct axis X..." << std::endl; 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; r.mx.correct_status = true;
} else if (cmd_arg == ROBOT_AXIS_Z) { } else if (cmd_arg == ROBOT_AXIS_Z) {
std::cout << "robot " << robot_id << " correct axis Z..." << std::endl; std::cout << "robot " << robot_id << " correct axis Z..." << std::endl;

56
scheduler.c
View File

@ -98,6 +98,10 @@ void scheduler_main()
#endif #endif
{ {
if (scheduler_start_signal) { if (scheduler_start_signal) {
scheduler_correction_stage = 0;
robot1_code.PC = -1;
robot2_code.PC = -1;
for (short i = 0; i < BARRELS_COUNT; i++) { for (short i = 0; i < BARRELS_COUNT; i++) {
// после рестарта планировщика надо всем барабаном убрать робота, // после рестарта планировщика надо всем барабаном убрать робота,
// всех кто сверху удалить, // всех кто сверху удалить,
@ -176,25 +180,73 @@ void scheduler_main()
} }
if (hla_correct_command) { if (hla_correct_command) {
if (hla_robot2_en && (!robot2.mx.correct_sensor || !robot2.mz.correct_status)) { if (one_robot_mode) {
if (hla_robot2_en) {
if (robot2_code.PC < 0) { if (robot2_code.PC < 0) {
robot2_code.barrel_id = -1; robot2_code.barrel_id = -1;
robot2_code.code[0] = ROBOT_CMD_CORRECT_AXIS(ROBOT_AXIS_Z); 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[1] = ROBOT_CMD_CORRECT_AXIS(ROBOT_AXIS_X);
robot2_code.code[2] = ROBOT_CMD_END(); robot2_code.code[2] = ROBOT_CMD_END();
robot2_code.PC = 0; robot2_code.PC = 0;
hla_correct_command = 0;
} }
} else if (hla_robot1_en && (!robot1.mx.correct_sensor || !robot1.mz.correct_status)) { } else {
if (robot1_code.PC < 0) { if (robot1_code.PC < 0) {
robot1_code.barrel_id = -1; robot1_code.barrel_id = -1;
robot1_code.code[0] = ROBOT_CMD_CORRECT_AXIS(ROBOT_AXIS_Z); 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[1] = ROBOT_CMD_CORRECT_AXIS(ROBOT_AXIS_X);
robot1_code.code[2] = ROBOT_CMD_END(); robot1_code.code[2] = ROBOT_CMD_END();
robot1_code.PC = 0; robot1_code.PC = 0;
hla_correct_command = 0;
}
} }
} else { } 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; hla_correct_command = 0;
} }
}
} else if (!auto_mode_pause) { } else if (!auto_mode_pause) {
if ((hla_robot1_en && robot1_code.PC < 0) || (hla_robot2_en && robot2_code.PC < 0)) { if ((hla_robot1_en && robot1_code.PC < 0) || (hla_robot2_en && robot2_code.PC < 0)) {
struct scheduler_task tasks[BARRELS_COUNT]; struct scheduler_task tasks[BARRELS_COUNT];

2
utils.c
View File

@ -366,7 +366,7 @@ void debug_print_robot_code(const struct robot_code *code, const short robot_id,
break; break;
} }
switch ((short)(code->code[i] & (short)ROBOT_CMD_MASK)) { switch (code->code[i] & ROBOT_CMD_MASK) {
case ROBOT_CMD_MOVE_TO_ZONE_code: case ROBOT_CMD_MOVE_TO_ZONE_code:
if (cmd_arg & ROBOT_ZONE_PARKING) { if (cmd_arg & ROBOT_ZONE_PARKING) {
dprintf(fd, " move to parking (with barrel: %d)\n", (cmd_arg & ROBOT_WITH_BARREL) != 0); dprintf(fd, " move to parking (with barrel: %d)\n", (cmd_arg & ROBOT_WITH_BARREL) != 0);

59
utils.h
View File

@ -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 char scheduler_start_signal;
extern short scheduler_correction_stage;
#else #else
#define barrels ((struct barrel*)barrels_array) #define barrels ((struct barrel*)barrels_array)
#define robot1 (*((struct robot_regs*)&robot1_reg_mx)) #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_night_mode _c_hla_night_mode
#define hla_correct_command _c_hla_correct_command #define hla_correct_command _c_hla_correct_command
#define scheduler_correction_stage _c_scheduler_correction_stage
#endif #endif
extern const short NIGHT_ZONES[9]; extern const short NIGHT_ZONES[9];
#define ROBOT_CMD_MASK (short)0xF000 #define ROBOT_CMD_MASK 0xF000U
#define ROBOT_CMD_END_code (short)0x0000 #define ROBOT_CMD_END_code 0x0000U
#define ROBOT_CMD_MOVE_TO_ZONE_code (short)0x1000 #define ROBOT_CMD_MOVE_TO_ZONE_code 0x1000U
#define ROBOT_CMD_MOVE_OFF_code (short)0x2000 #define ROBOT_CMD_MOVE_OFF_code 0x2000U
#define ROBOT_CMD_MOVE_ACCURATE_code (short)0x3000 #define ROBOT_CMD_MOVE_ACCURATE_code 0x3000U
#define ROBOT_CMD_UP_code (short)0x4000 #define ROBOT_CMD_UP_code 0x4000U
#define ROBOT_CMD_DOWN_code (short)0x5000 #define ROBOT_CMD_DOWN_code 0x5000U
#define ROBOT_CMD_WAIT_code (short)0x6000 #define ROBOT_CMD_WAIT_code 0x6000U
#define ROBOT_CMD_TMR_SET_code (short)0x7000 #define ROBOT_CMD_TMR_SET_code 0x7000U
#define ROBOT_CMD_SET_LOCK_ZONE_code (short)0x8000 #define ROBOT_CMD_SET_LOCK_ZONE_code 0x8000U
#define ROBOT_CMD_CORRECT_AXIS_code (short)0x9000 #define ROBOT_CMD_CORRECT_AXIS_code 0x9000U
#define ROBOT_CMD_INC_ZONE_code (short)0xA000 #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_ETCH 0x0400U
#define ROBOT_ZONE_GAL 0x0200 #define ROBOT_ZONE_GAL 0x0200U
#define ROBOT_ZONE_PARKING 0x0100 #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_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(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) | (short)(ROBOT_WITH_BARREL) | (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() ((short)(ROBOT_CMD_MOVE_TO_ZONE_code) | (short)(ROBOT_ZONE_PARKING)) #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_OFF() (ROBOT_CMD_MOVE_OFF_code)
#define ROBOT_CMD_MOVE_ACCURATE() (ROBOT_CMD_MOVE_ACCURATE_code) #define ROBOT_CMD_MOVE_ACCURATE() (ROBOT_CMD_MOVE_ACCURATE_code)
#define ROBOT_CMD_UP() (ROBOT_CMD_UP_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() (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_WAIT(time) (ROBOT_CMD_WAIT_code | (unsigned short)(time & 0x0FFF))
#define ROBOT_CMD_TMR_SET(time) ((ROBOT_CMD_TMR_SET_code) | (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) | (short)(zone & 0x00FF)) #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_CORRECT_AXIS(axis) (ROBOT_CMD_CORRECT_AXIS_code | (unsigned short)axis)
#define ROBOT_CMD_INC_ZONE(arg) ((ROBOT_CMD_INC_ZONE_code) | (short)(arg)) #define ROBOT_CMD_INC_ZONE(arg) (ROBOT_CMD_INC_ZONE_code | (unsigned short)(arg))
/* ======================== DISABLED ZONES DEFS ======================== */ /* ======================== DISABLED ZONES DEFS ======================== */