896 lines
34 KiB
C
896 lines
34 KiB
C
//
|
||
// Created by Владислав Остапов on 13.12.2022.
|
||
//
|
||
|
||
#ifdef EMULATOR
|
||
#include <stdio.h>
|
||
#include "emulator.h"
|
||
#else
|
||
#include "utils.h"
|
||
#endif
|
||
|
||
|
||
#define LOCK_ZONE_BORDER 1
|
||
|
||
|
||
const short NIGHT_ZONES[9] = {
|
||
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
|
||
};
|
||
|
||
|
||
char zone_is_busy(short zone) {
|
||
for (short i = 0; i < BARRELS_COUNT; i++) {
|
||
if (barrels[i].flags.is_exist && barrels[i].zone == zone && !barrels[i].flags.is_up) {
|
||
return 1;
|
||
}
|
||
}
|
||
return 0;
|
||
}
|
||
|
||
|
||
short get_first_night_zone() {
|
||
short min_normal_barrel = -1;
|
||
for (short i = 0; i < BARRELS_COUNT; i++) {
|
||
// ищем все не ночные, не пустые, и которые внизу барабаны
|
||
if (barrels[i].flags.is_exist && !barrels[i].flags.is_empty && !barrels[i].flags.is_night && !barrels[i].flags.is_up) {
|
||
// нужен барабан, у которого нет
|
||
if (min_normal_barrel < 0 || barrels[i].zone < min_normal_barrel) {
|
||
min_normal_barrel = barrels[i].zone;
|
||
}
|
||
}
|
||
}
|
||
|
||
// всего зон, куда можно сныкать барабаны (всего 9 мест: загрузка 1 и 8 промывок)
|
||
for (short nz = 0; nz < 9; nz++) {
|
||
char found = 0;
|
||
for (short i = 0; i < BARRELS_COUNT; i++) {
|
||
if (barrels[i].flags.is_exist && barrels[i].zone == NIGHT_ZONES[nz] && barrels[i].flags.is_night) {
|
||
found = 1;
|
||
}
|
||
if (NIGHT_ZONES[nz] == ZONE_LOAD_1 && barrels[i].flags.is_exist && barrels[i].zone == NIGHT_ZONES[nz]) {
|
||
found = 1;
|
||
}
|
||
}
|
||
|
||
if (found == 0) {
|
||
if (min_normal_barrel < 0 || NIGHT_ZONES[nz] < min_normal_barrel) {
|
||
return NIGHT_ZONES[nz];
|
||
} else {
|
||
break;
|
||
}
|
||
}
|
||
}
|
||
|
||
return -1;
|
||
}
|
||
|
||
|
||
short _get_dest_zone(struct barrel *bar, char robot_id) {
|
||
if (bar->flags.is_night) {
|
||
if (one_robot_mode) {
|
||
return ZONE_LOAD_1;
|
||
} else {
|
||
if (robot_id == ROBOT_1) {
|
||
// робот 1 может выкидывать барабаны только до зоны обмена
|
||
if (bar->zone >= hla_exchange_zone) {
|
||
return hla_exchange_zone;
|
||
} else {
|
||
return -1;
|
||
}
|
||
} else if (robot_id == ROBOT_2) {
|
||
// робот 2 всегда кидает барабаны в загрузку 1, но только дальше зоны обмена он не имеет права брать
|
||
if (bar->zone <= hla_exchange_zone) {
|
||
return ZONE_LOAD_1;
|
||
} else {
|
||
return -1;
|
||
}
|
||
} else {
|
||
return ZONE_LOAD_1;
|
||
}
|
||
}
|
||
}
|
||
|
||
// если это зона обмена и барабан пустой
|
||
if (bar->zone == hla_exchange_zone && bar->flags.is_empty) {
|
||
if (hla_night_mode) {
|
||
return get_first_night_zone();
|
||
} else {
|
||
return ZONE_LOAD_1;
|
||
}
|
||
}
|
||
|
||
|
||
switch (bar->zone) {
|
||
case ZONE_LOAD_2:
|
||
// загрузка 2, только в нее можно грузить новые барабаны, по умолчанию следующая зона - обезжиривание
|
||
// при повторном цинковании это кислота
|
||
if (hla_zinc_again) {
|
||
if (etching_zone < 0) {
|
||
return -1;
|
||
} else {
|
||
return (short)(ZONE_ETCHING_1 + etching_zone);
|
||
}
|
||
} else {
|
||
return ZONE_DEGREASING;
|
||
}
|
||
|
||
case ZONE_DEGREASING:
|
||
// обезжиривание, нужна промывка 1А
|
||
return ZONE_WASHING_1A;
|
||
|
||
case ZONE_WASHING_1A:
|
||
// промывка 1А, нужна промывка 1Б
|
||
return ZONE_WASHING_1B;
|
||
|
||
case ZONE_WASHING_1B:
|
||
// промывка 1Б, нужно травление (зоны 5-6)
|
||
if (etching_zone < 0) {
|
||
return -1;
|
||
}
|
||
return (short)(ZONE_ETCHING_1 + etching_zone);
|
||
|
||
case ZONE_ETCHING_1:
|
||
case ZONE_ETCHING_2:
|
||
// травление, нужна промывка 2А
|
||
return ZONE_WASHING_2A;
|
||
|
||
case ZONE_WASHING_2A:
|
||
// промывка 2А, нужна промывка 2Б
|
||
return ZONE_WASHING_2B;
|
||
|
||
case ZONE_WASHING_2B:
|
||
// промывка 2Б, нужно цинкование (зоны 9-16)
|
||
if (galvanizing_zone < 0) {
|
||
return -1;
|
||
}
|
||
return (short)(ZONE_GALVANIZING_1 + galvanizing_zone);
|
||
|
||
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) {
|
||
return ZONE_WASHING_3A;
|
||
}
|
||
}
|
||
return -1;
|
||
|
||
case ZONE_WASHING_3A:
|
||
// промывка 3А, перекладываем в промывку 3Б
|
||
return ZONE_WASHING_3B;
|
||
|
||
case ZONE_WASHING_3B:
|
||
// это перед пассивацией, требует свободную промывку 4А и на всякий случай свободную пассивацию
|
||
if (!zone_is_busy(ZONE_WASHING_4A)) {
|
||
return ZONE_PASSIVATION;
|
||
}
|
||
// это атомарная операция, по идее вносить барабан в пассивацию нельзя
|
||
return -1;
|
||
|
||
case ZONE_PASSIVATION:
|
||
// процесс пассивации, нужна промывка 4A
|
||
// чисто теоретически сюда никогда не попадем, но если вдруг выстрелит пусть будет
|
||
return ZONE_WASHING_4A;
|
||
|
||
case ZONE_WASHING_4A:
|
||
// промывка 4А, перекладываем в промывку 4Б
|
||
return ZONE_WASHING_4B;
|
||
|
||
case ZONE_WASHING_4B:
|
||
// последняя промывка в процессе
|
||
return ZONE_UNLOAD;
|
||
|
||
case ZONE_UNLOAD:
|
||
// последняя промывка, нужно разрешение на выгрузку
|
||
if (button_unload) {
|
||
if (hla_night_mode) {
|
||
short nz = get_first_night_zone();
|
||
if (one_robot_mode) {
|
||
if (nz < 0) {
|
||
return hla_exchange_zone;
|
||
} else {
|
||
return nz;
|
||
}
|
||
} else {
|
||
if (nz < hla_exchange_zone) {
|
||
return hla_exchange_zone;
|
||
} else {
|
||
return nz;
|
||
}
|
||
}
|
||
} else {
|
||
if (one_robot_mode) {
|
||
return ZONE_LOAD_1;
|
||
} else {
|
||
return hla_exchange_zone;
|
||
}
|
||
}
|
||
}
|
||
break;
|
||
}
|
||
|
||
return -1;
|
||
}
|
||
|
||
|
||
short can_move(struct barrel *bar, char robot_id) {
|
||
if (robot_id != ROBOT_NONE && robot_id != ROBOT_1 && robot_id != ROBOT_2) {
|
||
return -3;
|
||
}
|
||
// сразу отсекаем варианты, при которых невозможно переместить барабан
|
||
if (!bar->flags.is_exist) {
|
||
return -4;
|
||
}
|
||
|
||
if (bar->flags.robot != 0) {
|
||
return -5;
|
||
}
|
||
|
||
// проверка ночного режима
|
||
if (hla_night_mode) {
|
||
if (bar->flags.is_night) {
|
||
return -1;
|
||
}
|
||
}
|
||
|
||
// тут просчет примерного времени, в течении которого можно доехать до барабана
|
||
// !учитывается только с привязкой к роботу
|
||
if (!bar->flags.is_empty && robot_id != ROBOT_NONE) {
|
||
short approximate_time;
|
||
if (robot_id == 1) {
|
||
approximate_time = bar->zone - robot1.dx.current_zone;
|
||
} else {
|
||
approximate_time = bar->zone - robot2.dx.current_zone;
|
||
}
|
||
|
||
// abs(approximate_time)
|
||
if (approximate_time < 0) {
|
||
approximate_time = -approximate_time;
|
||
}
|
||
|
||
#ifndef EMULATOR
|
||
if (approximate_time > 1) {
|
||
approximate_time = ((approximate_time - 1) * 3) + 4;
|
||
} else if (approximate_time == 1) {
|
||
approximate_time = 5;
|
||
}
|
||
#endif
|
||
|
||
if (bar->software_timer > approximate_time) {
|
||
return -2;
|
||
}
|
||
}
|
||
|
||
// дальше нужно проверить, что можно передвигать бочку
|
||
short dest_zone = _get_dest_zone(bar, robot_id);
|
||
|
||
if (zone_is_busy(dest_zone)) {
|
||
return -1;
|
||
}
|
||
|
||
// учитывать границы только в режиме двух роботов и с привязкой к роботу
|
||
if (!one_robot_mode && dest_zone >= 0 && robot_id != ROBOT_NONE) {
|
||
if (robot_id == 1) {
|
||
// если робот 1, то это старый, который ближе к концу линии.
|
||
// Ему нельзя ехать если хоть одна из зон <= max(r2_pos, r2_lock) + кол-во пограничных зон
|
||
short border = robot2.dx.current_zone;
|
||
if (border < robot2_lock_zone) {
|
||
border = robot2_lock_zone;
|
||
}
|
||
if (border < ZONE_ETCHING_2) {
|
||
border = ZONE_ETCHING_2;
|
||
}
|
||
|
||
border += LOCK_ZONE_BORDER;
|
||
|
||
if (bar->zone <= border || dest_zone <= border) {
|
||
dest_zone = -6;
|
||
}
|
||
} else {
|
||
// если робот 2, то это новый, который ближе к началу линии.
|
||
// Ему нельзя ехать если хоть одна из зон >= max(r2_pos, r2_lock) - кол-во пограничных зон
|
||
short border = robot1.dx.current_zone;
|
||
if (border > robot1_lock_zone) {
|
||
border = robot1_lock_zone;
|
||
}
|
||
if (border > ZONE_PASSIVATION) {
|
||
border = ZONE_PASSIVATION;
|
||
}
|
||
|
||
border -= LOCK_ZONE_BORDER;
|
||
|
||
if (bar->zone >= border || dest_zone >= border) {
|
||
dest_zone = -6;
|
||
}
|
||
}
|
||
}
|
||
|
||
return dest_zone;
|
||
}
|
||
|
||
|
||
short get_operation_priority(short barrel_id) {
|
||
// если барабан пустой и не в ночном режиме, приоритет ему 2
|
||
if (barrels[barrel_id].flags.is_empty && !barrels[barrel_id].flags.is_night) {
|
||
return 2;
|
||
}
|
||
|
||
// если барабан в ночном режиме, то приоритет 1 самому левому, остальным 0
|
||
if (barrels[barrel_id].flags.is_night) {
|
||
short min_zone = barrels[barrel_id].zone;
|
||
|
||
for (short i = 0; i < BARRELS_COUNT; i++) {
|
||
if (i == barrel_id) {
|
||
continue;
|
||
}
|
||
if (barrels[i].flags.is_exist && barrels[i].flags.is_night && barrels[i].flags.robot == 0 && !barrels[i].flags.is_up) {
|
||
if (can_move(barrels + i, ROBOT_NONE) >= 0) {
|
||
// чем больше у барабана время ожидания тем меньше у него число
|
||
if (barrels[i].zone < min_zone) {
|
||
min_zone = barrels[i].zone;
|
||
}
|
||
}
|
||
}
|
||
}
|
||
|
||
return (min_zone == barrels[barrel_id].zone) ? 1 : 0;
|
||
}
|
||
|
||
// сделать приоритет на барабан, который больше всего ждет
|
||
if (barrels[barrel_id].zone >= ZONE_GALVANIZING_1 && barrels[barrel_id].zone <= ZONE_GALVANIZING_8) {
|
||
// теперь надо выяснить, есть ли барабаны с большим временем ожидания
|
||
// тут возможны несколько случаев:
|
||
// 1) когда барабан один такой (больше в цинковании нет барабанов чтобы их изъять),
|
||
// 2) когда барабанов несколько, соответственно если есть барабан с наибольшим временем, то надо ему дать приоритет 1, а остальным 0,
|
||
|
||
char is_not_one = 0;
|
||
char is_with_max_time = 1;
|
||
|
||
for (short i = 0; i < BARRELS_COUNT; i++) {
|
||
if (i == 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, ROBOT_NONE) >= 0) {
|
||
is_not_one = 1;
|
||
// чем больше у барабана время ожидания тем меньше у него число
|
||
if (barrels[i].software_timer < barrels[barrel_id].software_timer) {
|
||
is_with_max_time = 0;
|
||
break;
|
||
}
|
||
}
|
||
}
|
||
}
|
||
|
||
if (is_not_one == 0) {
|
||
return 1;
|
||
}
|
||
return is_with_max_time;
|
||
}
|
||
|
||
|
||
// теперь то же самое, только для травления (только тут задача проще потому что травления всего 2 зоны, и надо только один барабан если он есть)
|
||
if (barrels[barrel_id].zone >= ZONE_ETCHING_1 && barrels[barrel_id].zone <= ZONE_ETCHING_2) {
|
||
|
||
// если в травлении барабан лежит больше 40 минут, ему присвоить высокий приоритет
|
||
if (barrels[barrel_id].software_timer < (-1 * 60 * 40)) {
|
||
return 2;
|
||
}
|
||
|
||
for (short i = 0; i < BARRELS_COUNT; i++) {
|
||
if (i == barrel_id) {
|
||
continue;
|
||
}
|
||
|
||
if (barrels[i].flags.is_exist && barrels[i].zone >= ZONE_ETCHING_1 && barrels[i].zone <= ZONE_ETCHING_2) {
|
||
if (can_move(barrels + i, ROBOT_NONE) >= 0) {
|
||
if (barrels[i].software_timer < barrels[barrel_id].software_timer) {
|
||
return 0; // у этого барабана больше время ожидания (число меньше), значит приоритет 0
|
||
} else {
|
||
return 1; // поскольку у нашего барабана максимальное время ожидания
|
||
}
|
||
}
|
||
}
|
||
}
|
||
// барабан не найден, приоритет ему 1
|
||
return 1;
|
||
}
|
||
|
||
return 1; // 1 - нормальный приоритет
|
||
}
|
||
|
||
|
||
char remove_barrel_from_zone(short zone) {
|
||
for (short i = 0; i < 10; i++) {
|
||
if (barrels[i].flags.is_exist && barrels[i].flags.robot == 0 && barrels[i].zone == zone) {
|
||
barrels[i].flags.is_exist = 0;
|
||
return 1;
|
||
}
|
||
}
|
||
return 0;
|
||
}
|
||
|
||
|
||
#ifdef EMULATOR
|
||
void debug_print_robot_code(const struct robot_code *code, const short robot_id, int fd) {
|
||
dprintf(fd, "Code for R%d, B%d:\n", robot_id, code->barrel_id);
|
||
for (int i = 0; i < 16; i++) {
|
||
const short cmd_arg = (short)(code->code[i] & (short)(~ROBOT_CMD_MASK));
|
||
|
||
if (code->PC == i) {
|
||
dprintf(fd, "==>%2d 0x%04X", i, code->code[i] & 0xFFFF);
|
||
} else {
|
||
dprintf(fd, " %2d 0x%04X", i, code->code[i] & 0xFFFF);
|
||
}
|
||
|
||
if ((code->code[i] & ROBOT_CMD_MASK) == ROBOT_CMD_END_code) {
|
||
dprintf(fd, " END\n");
|
||
break;
|
||
}
|
||
|
||
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);
|
||
} else {
|
||
dprintf(fd, " move to zone %d (with barrel: %d)\n",
|
||
cmd_arg & (~ROBOT_WITH_BARREL), (cmd_arg & ROBOT_WITH_BARREL) != 0);
|
||
}
|
||
break;
|
||
|
||
case ROBOT_CMD_MOVE_OFF_code:
|
||
dprintf(fd, " move to offset pos\n");
|
||
break;
|
||
|
||
case ROBOT_CMD_MOVE_ACCURATE_code:
|
||
dprintf(fd, " move to accurate pos\n");
|
||
break;
|
||
|
||
case ROBOT_CMD_UP_code:
|
||
dprintf(fd, " up (with barrel: %d)\n", (cmd_arg & ROBOT_WITH_BARREL) != 0);
|
||
break;
|
||
|
||
// в эмуляторе не важно где я, поэтому тут обе команды вниз обрабатываются одинаково
|
||
case ROBOT_CMD_DOWN_code:
|
||
dprintf(fd, " down (with barrel: %d)\n", (cmd_arg & ROBOT_WITH_BARREL) != 0);
|
||
break;
|
||
|
||
case ROBOT_CMD_WAIT_code:
|
||
dprintf(fd, " wait %d secs\n", cmd_arg);
|
||
break;
|
||
|
||
case ROBOT_CMD_TMR_SET_code:
|
||
dprintf(fd, " set barrel timer %d secs\n", cmd_arg);
|
||
break;
|
||
|
||
case ROBOT_CMD_SET_LOCK_ZONE_code:
|
||
dprintf(fd, " set lock zone %d\n", cmd_arg);
|
||
break;
|
||
|
||
case ROBOT_CMD_CORRECT_AXIS_code:
|
||
if (cmd_arg == ROBOT_AXIS_X) {
|
||
dprintf(fd, " correct axis: X\n");
|
||
} else if (cmd_arg == ROBOT_AXIS_Z) {
|
||
dprintf(fd, " correct axis: Z\n");
|
||
} else {
|
||
dprintf(fd, " correct axis: INVALID (%d)\n", cmd_arg);
|
||
}
|
||
break;
|
||
|
||
case ROBOT_CMD_INC_ZONE_code:
|
||
if (cmd_arg == ROBOT_ZONE_GAL) {
|
||
dprintf(fd, " increment zone: galvanic\n");
|
||
} else if (cmd_arg == ROBOT_ZONE_ETCH) {
|
||
dprintf(fd, " increment zone: etching\n");
|
||
} else {
|
||
dprintf(fd, " increment zone: INVALID (0x%4X)\n", cmd_arg);
|
||
}
|
||
break;
|
||
|
||
default:
|
||
dprintf(fd, " UNKNOWN: 0x%04X\n", code->code[i] & 0xFFFF);
|
||
}
|
||
}
|
||
}
|
||
#endif
|
||
|
||
|
||
void create_operation(short barrel_id, const short start_zone, const short dest_zone, const short robot_id) {
|
||
short current_zone;
|
||
struct robot_code *code;
|
||
if (robot_id == ROBOT_1) {
|
||
current_zone = robot1.dx.current_zone;
|
||
code = &robot1_code;
|
||
} else if (robot_id == ROBOT_2) {
|
||
current_zone = robot2.dx.current_zone;
|
||
code = &robot2_code;
|
||
} else {
|
||
return;
|
||
}
|
||
|
||
// создаем код транзакции, пока обычный
|
||
if (barrel_id >= BARRELS_COUNT) {
|
||
barrel_id = -1;
|
||
}
|
||
code->barrel_id = barrel_id;
|
||
|
||
if (barrel_id >= 0) {
|
||
barrels[barrel_id].flags.robot = robot_id;
|
||
if (hla_night_mode && barrels[barrel_id].flags.is_empty && dest_zone == get_first_night_zone()) {
|
||
barrels[barrel_id].flags.is_night = -1;
|
||
}
|
||
|
||
// сброс флага повторного цинкования во время создания задания
|
||
if (start_zone == ZONE_LOAD_2) {
|
||
hla_zinc_again = 0;
|
||
}
|
||
}
|
||
|
||
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_PASSIVATION) {
|
||
tmp = ZONE_PASSIVATION;
|
||
}
|
||
robot1_lock_zone = tmp;
|
||
} else {
|
||
short tmp = dest_zone;
|
||
if (start_zone > dest_zone) {
|
||
tmp = start_zone;
|
||
}
|
||
if (tmp < ZONE_ETCHING_2) {
|
||
tmp = ZONE_ETCHING_2;
|
||
}
|
||
robot2_lock_zone = tmp;
|
||
}
|
||
}
|
||
|
||
// первым делом добавляем команду опустить траверсу
|
||
code->code[cmd_index++] = ROBOT_CMD_DOWN();
|
||
|
||
if (dest_zone == ZONE_PASSIVATION) {
|
||
// пассивация, тут все просто
|
||
if (current_zone != start_zone) {
|
||
code->code[cmd_index++] = ROBOT_CMD_MOVE_TO_ZONE(ZONE_WASHING_3B);
|
||
}
|
||
if (!one_robot_mode) {
|
||
code->code[cmd_index++] = ROBOT_CMD_SET_LOCK_ZONE(ZONE_PASSIVATION);
|
||
}
|
||
code->code[cmd_index++] = ROBOT_CMD_UP_WITH_BARREL();
|
||
code->code[cmd_index++] = ROBOT_CMD_WAIT(hla_time_washing_2);
|
||
code->code[cmd_index++] = ROBOT_CMD_MOVE_TO_ZONE_WITH_BARREL(ZONE_PASSIVATION);
|
||
code->code[cmd_index++] = ROBOT_CMD_DOWN_WITH_BARREL();
|
||
|
||
// NOTE таймер робота работает точнее чем таймер барабанов
|
||
code->code[cmd_index++] = ROBOT_CMD_TMR_SET(barrels[barrel_id].time_passivation);
|
||
// code->code[cmd_index++] = ROBOT_CMD_WAIT(barrels[barrel_id].time_passivation);
|
||
|
||
code->code[cmd_index++] = ROBOT_CMD_UP_WITH_BARREL();
|
||
code->code[cmd_index++] = ROBOT_CMD_WAIT(hla_time_digging);
|
||
code->code[cmd_index++] = ROBOT_CMD_MOVE_TO_ZONE_WITH_BARREL(ZONE_WASHING_4A);
|
||
code->code[cmd_index++] = ROBOT_CMD_DOWN_WITH_BARREL();
|
||
|
||
code->code[cmd_index++] = ROBOT_CMD_TMR_SET(barrels[barrel_id].time_washing_4a);
|
||
} else {
|
||
// любой другой случай
|
||
if (current_zone != start_zone) {
|
||
if (start_zone != 22) {
|
||
code->code[cmd_index++] = ROBOT_CMD_MOVE_TO_ZONE(start_zone);
|
||
} else {
|
||
code->code[cmd_index++] = ROBOT_CMD_MOVE_OFF();
|
||
code->code[cmd_index++] = ROBOT_CMD_UP();
|
||
code->code[cmd_index++] = ROBOT_CMD_MOVE_TO_ZONE(22);
|
||
code->code[cmd_index++] = ROBOT_CMD_MOVE_OFF();
|
||
code->code[cmd_index++] = ROBOT_CMD_DOWN();
|
||
code->code[cmd_index++] = ROBOT_CMD_MOVE_ACCURATE();
|
||
}
|
||
}
|
||
|
||
// теперь обновляем LOCK-зону
|
||
if (!one_robot_mode) {
|
||
if (robot_id == 1) {
|
||
// ставим lock-зону только если она ближе к концу линии
|
||
short tmp = dest_zone;
|
||
|
||
// lock-зона этого робота должна быть такой, чтобы была свободна промывка 3а
|
||
if (tmp > ZONE_PASSIVATION) {
|
||
tmp = ZONE_PASSIVATION;
|
||
}
|
||
|
||
// в любом случае lock-зону нельзя двигать к началу линии
|
||
if (tmp > robot1_lock_zone) {
|
||
code->code[cmd_index++] = ROBOT_CMD_SET_LOCK_ZONE(tmp);
|
||
}
|
||
} else {
|
||
// ставим lock-зону только если она ближе к началу линии
|
||
short tmp = dest_zone;
|
||
|
||
// lock-зона этого робота не может выходить за промывку 3А, потому что это не имеет смысла
|
||
if (tmp < ZONE_ETCHING_2) {
|
||
tmp = ZONE_ETCHING_2;
|
||
}
|
||
|
||
// в любом случае lock-зону нельзя двигать к концу линии
|
||
if (tmp < robot2_lock_zone) {
|
||
code->code[cmd_index++] = ROBOT_CMD_SET_LOCK_ZONE(tmp);
|
||
}
|
||
}
|
||
}
|
||
|
||
code->code[cmd_index++] = ROBOT_CMD_UP_WITH_BARREL();
|
||
|
||
// теперь надо определиться с тем, сколько ждать скапывания
|
||
switch (start_zone) {
|
||
case ZONE_DEGREASING:
|
||
case ZONE_ETCHING_1:
|
||
case ZONE_ETCHING_2:
|
||
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:
|
||
// время скапывания реактивов, оно не должно работать только в зоне обмена
|
||
if (start_zone != hla_exchange_zone) {
|
||
code->code[cmd_index++] = ROBOT_CMD_WAIT(hla_time_reagent);
|
||
}
|
||
break;
|
||
|
||
case ZONE_WASHING_1A:
|
||
case ZONE_WASHING_2A:
|
||
case ZONE_WASHING_3A:
|
||
case ZONE_WASHING_4A:
|
||
// время скапывания первого каскада промывок
|
||
code->code[cmd_index++] = ROBOT_CMD_WAIT(hla_time_washing_1);
|
||
break;
|
||
|
||
case ZONE_WASHING_1B:
|
||
case ZONE_WASHING_2B:
|
||
case ZONE_WASHING_3B:
|
||
case ZONE_WASHING_4B:
|
||
// время скапывания второго каскада промывок
|
||
code->code[cmd_index++] = ROBOT_CMD_WAIT(hla_time_washing_2);
|
||
break;
|
||
}
|
||
|
||
code->code[cmd_index++] = ROBOT_CMD_MOVE_TO_ZONE_WITH_BARREL(dest_zone);
|
||
|
||
if (barrel_id >= 0) {
|
||
if (!barrels[barrel_id].flags.is_empty) {
|
||
// инкремент зоны (если травление или цинкование)
|
||
if (dest_zone == ZONE_ETCHING_1 || dest_zone == ZONE_ETCHING_2) {
|
||
code->code[cmd_index++] = ROBOT_CMD_INC_ZONE(ROBOT_ZONE_ETCH);
|
||
} else if (dest_zone >= ZONE_GALVANIZING_1 && dest_zone <= ZONE_GALVANIZING_8) {
|
||
code->code[cmd_index++] = ROBOT_CMD_INC_ZONE(ROBOT_ZONE_GAL);
|
||
}
|
||
}
|
||
}
|
||
|
||
code->code[cmd_index++] = ROBOT_CMD_DOWN_WITH_BARREL();
|
||
|
||
if (dest_zone == 22) {
|
||
code->code[cmd_index++] = ROBOT_CMD_MOVE_TO_ZONE(21);
|
||
// NOTE старая механика не позволяет просто опустить траверсу до конца, для новой изменить поведение
|
||
code->code[cmd_index++] = ROBOT_CMD_UP();
|
||
code->code[cmd_index++] = ROBOT_CMD_DOWN();
|
||
} else {
|
||
// установка времени ожидания барабана
|
||
short tmp = -1;
|
||
if (barrel_id >= 0) {
|
||
if (!barrels[barrel_id].flags.is_empty) {
|
||
switch (dest_zone) {
|
||
case ZONE_DEGREASING:
|
||
tmp = barrels[barrel_id].time_degreasing;
|
||
break;
|
||
|
||
case ZONE_ETCHING_1:
|
||
case ZONE_ETCHING_2:
|
||
tmp = barrels[barrel_id].time_etching;
|
||
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:
|
||
tmp = barrels[barrel_id].time_galvanizing;
|
||
break;
|
||
|
||
case ZONE_WASHING_1A:
|
||
tmp = barrels[barrel_id].time_washing_1a;
|
||
break;
|
||
case ZONE_WASHING_2A:
|
||
tmp = barrels[barrel_id].time_washing_2a;
|
||
break;
|
||
case ZONE_WASHING_3A:
|
||
tmp = barrels[barrel_id].time_washing_3a;
|
||
break;
|
||
case ZONE_WASHING_4A:
|
||
tmp = barrels[barrel_id].time_washing_4a;
|
||
break;
|
||
|
||
case ZONE_WASHING_1B:
|
||
tmp = barrels[barrel_id].time_washing_1b;
|
||
break;
|
||
case ZONE_WASHING_2B:
|
||
tmp = barrels[barrel_id].time_washing_2b;
|
||
break;
|
||
case ZONE_WASHING_3B:
|
||
tmp = barrels[barrel_id].time_washing_3b;
|
||
break;
|
||
case ZONE_WASHING_4B:
|
||
tmp = barrels[barrel_id].time_washing_4b;
|
||
break;
|
||
}
|
||
}
|
||
}
|
||
|
||
if (tmp >= 0) {
|
||
if (tmp > 8000) {
|
||
tmp = 8000;
|
||
}
|
||
code->code[cmd_index++] = ROBOT_CMD_TMR_SET(tmp);
|
||
}
|
||
}
|
||
}
|
||
|
||
if (!one_robot_mode) {
|
||
if (robot_id == 2) {
|
||
// 2-му роботу нужно держать всегда свободной зону промывки 2б
|
||
if (dest_zone > ZONE_ETCHING_2) {
|
||
code->code[cmd_index++] = ROBOT_CMD_SET_LOCK_ZONE(ZONE_ETCHING_2);
|
||
code->code[cmd_index++] = ROBOT_CMD_MOVE_TO_ZONE(ZONE_ETCHING_2);
|
||
} else if (dest_zone < ZONE_WASHING_1A) {
|
||
// на случай если робот уехал в выгрузку или обезжир, надо его отогнать в зону промывки 1а
|
||
// code->code[cmd_index++] = ROBOT_CMD_SET_LOCK_ZONE(ZONE_WASHING_1A);
|
||
code->code[cmd_index++] = ROBOT_CMD_MOVE_TO_ZONE(ZONE_WASHING_1A);
|
||
}
|
||
} else if (robot_id == 1) {
|
||
// 2-му роботу нужно держать всегда свободной зону промывки 3а
|
||
if (dest_zone < ZONE_PASSIVATION) {
|
||
// чтобы из этой зоны можно было переложить барабан первому роботу
|
||
code->code[cmd_index++] = ROBOT_CMD_SET_LOCK_ZONE(ZONE_PASSIVATION);
|
||
code->code[cmd_index++] = ROBOT_CMD_MOVE_TO_ZONE(ZONE_PASSIVATION);
|
||
}
|
||
}
|
||
} else {
|
||
// автоматический отгоняем робота в зону 3, чтоб не мешался
|
||
if (dest_zone < ZONE_WASHING_1A) {
|
||
code->code[cmd_index++] = ROBOT_CMD_MOVE_TO_ZONE(ZONE_WASHING_1A);
|
||
}
|
||
}
|
||
|
||
code->code[cmd_index++] = ROBOT_CMD_END();
|
||
#ifdef EMULATOR
|
||
printf("INFO: code length is %d\n", cmd_index);
|
||
debug_print_robot_code(code, robot_id, 0);
|
||
#endif
|
||
code->PC = 0;
|
||
}
|
||
|
||
|
||
char is_accessible_zone(short zone) {
|
||
if (zone == ROBOT_ZONE_ETCH) {
|
||
if (etching_zone < 0) {
|
||
return 0;
|
||
}
|
||
|
||
return (hla_disabled_zones & (DISABLED_ETCH_1 << etching_zone)) == 0;
|
||
} else if (zone == ROBOT_ZONE_GAL) {
|
||
if (galvanizing_zone < 0) {
|
||
return 0;
|
||
}
|
||
|
||
return (hla_disabled_zones & (DISABLED_GAL_1 << galvanizing_zone)) == 0;
|
||
} else {
|
||
// неверный аргумент
|
||
return 0;
|
||
}
|
||
}
|
||
|
||
|
||
char increment_zone(short zone) {
|
||
if (zone == ROBOT_ZONE_ETCH) {
|
||
if ((hla_disabled_zones & DISABLED_ETCH) == DISABLED_ETCH) {
|
||
etching_zone = -1;
|
||
return 0;
|
||
} else {
|
||
if ((hla_disabled_zones & DISABLED_ETCH) == 0) {
|
||
// если обе зоны активны
|
||
if (etching_zone < 0) {
|
||
etching_zone = 0;
|
||
} else {
|
||
if (etching_zone == 0) {
|
||
etching_zone = 1;
|
||
} else {
|
||
etching_zone = 0;
|
||
}
|
||
}
|
||
} else {
|
||
// если только одна зона
|
||
if (hla_disabled_zones & DISABLED_ETCH_1) {
|
||
etching_zone = 1;
|
||
} else {
|
||
etching_zone = 0;
|
||
}
|
||
}
|
||
|
||
return 1;
|
||
}
|
||
} else if (zone == ROBOT_ZONE_GAL) {
|
||
// с зонами цинкования все несколько сложнее...
|
||
// сначала надо посчитать кол-во включенных зон
|
||
short enabled_zones = 0;
|
||
for (short z = DISABLED_GAL_1; z & DISABLED_GAL; z <<= 1) {
|
||
if ((hla_disabled_zones & z) == 0) {
|
||
enabled_zones++;
|
||
}
|
||
}
|
||
|
||
if (enabled_zones == 0) {
|
||
// таких зон нет...
|
||
galvanizing_zone = -1;
|
||
return 0;
|
||
} else if (enabled_zones == 1) {
|
||
// зона одна, ее только найти надо и установить
|
||
for (short z = 0; z < 8; z++) {
|
||
if ((hla_disabled_zones & (DISABLED_GAL_1 << z)) == 0) {
|
||
galvanizing_zone = z;
|
||
break;
|
||
}
|
||
}
|
||
} else {
|
||
// зон несколько, поэтому берем текущую, пробегаемся по битам и ищем другую
|
||
// всего может быть 7 зон, куда мы еще посмотрим
|
||
short bit_id = galvanizing_zone + 1;
|
||
if (galvanizing_zone < 0) {
|
||
bit_id = 0;
|
||
}
|
||
for (short i = 0; i < 7 + (galvanizing_zone < 0 ? 1 : 0); i++) {
|
||
if (bit_id >= 8) {
|
||
bit_id = 0;
|
||
}
|
||
if ((hla_disabled_zones & (DISABLED_GAL_1 << bit_id)) == 0) {
|
||
// свободная зона
|
||
galvanizing_zone = bit_id;
|
||
break;
|
||
}
|
||
|
||
bit_id++;
|
||
}
|
||
}
|
||
|
||
return 1;
|
||
} else {
|
||
// неверный аргумент
|
||
return 0;
|
||
}
|
||
}
|