350 lines
16 KiB
C
350 lines
16 KiB
C
#ifdef EMULATOR
|
||
#include "emulator.h"
|
||
#endif
|
||
|
||
|
||
struct scheduler_task {
|
||
short start_zone; // стартовая зона
|
||
short dest_zone; // конечная зона
|
||
short priority; // приоритет, чем больше тем выше, по умолчанию 0
|
||
};
|
||
|
||
|
||
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 (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;
|
||
}
|
||
}
|
||
|
||
|
||
void schedule_one_robot(const struct scheduler_task* tasks, const struct robot_regs* r, struct robot_code* code, const short robot_id) {
|
||
// формируем список задач
|
||
short target_task = scheduler_find_task(tasks, r->dx.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);
|
||
}
|
||
}
|
||
|
||
|
||
#ifdef EMULATOR
|
||
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++) {
|
||
// после рестарта планировщика надо всем барабаном убрать робота,
|
||
// всех кто сверху удалить,
|
||
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 (hla_correct_command) {
|
||
if (one_robot_mode) {
|
||
switch (scheduler_correction_stage) {
|
||
case 0:
|
||
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;
|
||
|
||
scheduler_correction_stage++;
|
||
}
|
||
} 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_MOVE_TO_PARKING();
|
||
robot1_code.code[3] = ROBOT_CMD_END();
|
||
robot1_code.PC = 0;
|
||
|
||
scheduler_correction_stage++;
|
||
}
|
||
}
|
||
break;
|
||
|
||
case 1:
|
||
if (robot2_code.PC < 0 && robot1_code.PC < 0) {
|
||
scheduler_correction_stage++;
|
||
}
|
||
break;
|
||
|
||
default:
|
||
scheduler_correction_stage = 0;
|
||
hla_correct_command = 0;
|
||
}
|
||
} 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_MOVE_TO_PARKING();
|
||
robot1_code.code[3] = 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 (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 (!is_accessible_zone(ROBOT_ZONE_ETCH)) {
|
||
increment_zone(ROBOT_ZONE_ETCH);
|
||
}
|
||
if (!is_accessible_zone(ROBOT_ZONE_GAL)) {
|
||
increment_zone(ROBOT_ZONE_GAL);
|
||
}
|
||
|
||
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;
|
||
if (hla_robot1_en && robot1_code.PC < 0) {
|
||
robot_id = ROBOT_1;
|
||
} else if (hla_robot2_en && robot2_code.PC < 0) {
|
||
robot_id = ROBOT_2;
|
||
}
|
||
|
||
if (robot_id != ROBOT_NONE) {
|
||
if ((robot_id == ROBOT_1 && robot1.dx.current_zone < ZONE_WASHING_1A) ||
|
||
(robot_id == ROBOT_2 && robot2.dx.current_zone < ZONE_WASHING_1A)) {
|
||
if (robot_id == 1) {
|
||
robot1_code.barrel_id = -1;
|
||
robot1_code.code[0] = ROBOT_CMD_DOWN();
|
||
robot1_code.code[1] = ROBOT_CMD_MOVE_TO_ZONE(ZONE_WASHING_1A);
|
||
robot1_code.code[2] = ROBOT_CMD_END();
|
||
robot1_code.PC = 0;
|
||
} else {
|
||
robot2_code.barrel_id = -1;
|
||
robot2_code.code[0] = ROBOT_CMD_DOWN();
|
||
robot2_code.code[1] = ROBOT_CMD_MOVE_TO_ZONE(ZONE_WASHING_1A);
|
||
robot2_code.code[2] = ROBOT_CMD_END();
|
||
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);
|
||
}
|
||
}
|
||
}
|
||
} else {
|
||
// а вот для режима двух роботов все интересно
|
||
// для каждого робота нужно получить свой список задач
|
||
// и надо еще сделать так, чтобы роботы не столкнулись
|
||
|
||
// суть этого предела в том, чтобы робот всегда был свободен ЗА зоной gal8,
|
||
// но при этом если зоной обмена выбрана промывка 3а, то за ней тоже, получается max(ZONE_G8, exchange_zone)
|
||
const short robot1_border = ZONE_GALVANIZING_8 > hla_exchange_zone ? ZONE_GALVANIZING_8 : hla_exchange_zone;
|
||
|
||
// суть этого предела в том, чтобы робот всегда держал свободной зону gal1,
|
||
// но при этом если зоной обмена выбрана промывка 2б, то за ней тоже, получается min(ZONE_G1, exchange_zone)
|
||
const short robot2_border = ZONE_GALVANIZING_1 < hla_exchange_zone ? ZONE_GALVANIZING_1 : hla_exchange_zone;
|
||
|
||
// логика для того, чтобы роботы не столкнулись в начале
|
||
if (robot1.dx.current_zone < robot1_border) {
|
||
if (robot1_code.PC < 0) {
|
||
robot2_lock_zone = -1;
|
||
robot1_lock_zone = robot1_border;
|
||
robot1_code.barrel_id = -1;
|
||
robot1_code.code[0] = ROBOT_CMD_DOWN();
|
||
robot1_code.code[1] = ROBOT_CMD_MOVE_TO_ZONE(robot1_border + 2);
|
||
robot1_code.code[2] = ROBOT_CMD_END();
|
||
robot1_code.PC = 0;
|
||
}
|
||
} else if (robot2.dx.current_zone > robot2_border) {
|
||
// это уже граница робота 2
|
||
if (robot2_code.PC < 0) {
|
||
robot2_lock_zone = robot2_border - 2;
|
||
robot2_code.barrel_id = -1;
|
||
robot2_code.code[0] = ROBOT_CMD_DOWN();
|
||
robot2_code.code[1] = ROBOT_CMD_MOVE_TO_ZONE(robot2_border - 2);
|
||
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, ROBOT_1);
|
||
if (tasks[i].dest_zone >= 0) {
|
||
tasks[i].priority = get_operation_priority(i);
|
||
}
|
||
}
|
||
|
||
schedule_one_robot(tasks, &robot1, &robot1_code, ROBOT_1);
|
||
}
|
||
|
||
// и отдельно для второго (только если не в ночном режиме)
|
||
if (robot2_code.PC < 0) {
|
||
if (robot2.dx.current_zone < ZONE_WASHING_1A) {
|
||
// начальная позиция робота 1 - обезжир
|
||
robot2_lock_zone = ZONE_WASHING_1A;
|
||
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_WASHING_1A);
|
||
robot2_code.code[2] = ROBOT_CMD_END();
|
||
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);
|
||
}
|
||
}
|
||
}
|
||
}
|
||
}
|
||
}
|
||
}
|
||
}
|