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

Добавление полноценного режима двух роботов

Browse Source
This commit is contained in:
VladislavOstapov 2023-01-02 00:20:23 +03:00
parent 33b705c1ef
commit d5c7c0e645
7 changed files with 328 additions and 121 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

6
CMakeLists.txt
View File

@ -1,10 +1,10 @@
cmake_minimum_required(VERSION 3.21) cmake_minimum_required(VERSION 3.21)
project(sdp_sheduler C CXX) project(sdp-scheduler C CXX)
set(CMAKE_C_STANDARD 17) set(CMAKE_C_STANDARD 17)
set(CMAKE_CXX_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) add_definitions(-DEMULATOR=1)

59
emulator.cpp
View File

@ -43,9 +43,11 @@ short hla_time_washing_2 = 20;
char hla_robot1_en = 1; 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 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; short etching_zone = 0, galvanizing_zone = 0;
@ -151,7 +153,31 @@ static void showAll() {
image_init(); image_init();
image_draw_borders(); 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++) { 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_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 // 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"); printf("\nInvalid address/ Address not supported \n");
exit(-1); 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() { int main() {
open_socket(); open_socket();
@ -240,6 +246,9 @@ int main() {
const char* message = nullptr; const char* message = nullptr;
while (true) { while (true) {
_scheduler_software_timer = 1; _scheduler_software_timer = 1;
button_load = 1;
robot_main(); robot_main();
scheduler_main(); scheduler_main();
send_str("\033c"); send_str("\033c");

2
emulator.h
View File

@ -50,8 +50,6 @@ extern short robot1_lock_zone;
extern short robot2_lock_zone; extern short robot2_lock_zone;
extern char _scheduler_en;
#ifdef __cplusplus #ifdef __cplusplus
} }
#endif #endif

9
robot.cpp
View File

@ -108,6 +108,15 @@ static void emulate_robot(robot_code &code, robot_regs& r, char robot_id) {
code.PC++; code.PC++;
break; 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: 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;

122
scheduler.c
View File

@ -11,7 +11,6 @@ struct scheduler_task {
short scheduler_find_task(const struct scheduler_task* tasks, const short curr_pos) { short scheduler_find_task(const struct scheduler_task* tasks, const short curr_pos) {
// TODO добавить поддержку ночного режима и режима двух роботов
// для начала надо найти максимальный приоритет у операций // для начала надо найти максимальный приоритет у операций
short max_priority = -1; short max_priority = -1;
for (short i = 0; i < BARRELS_COUNT; i++) { 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; short left = -1, right = -1;
for (short i = 0; i < BARRELS_COUNT; i++) { for (short i = 0; i < BARRELS_COUNT; i++) {
short target = tasks[i].start_zone; // фактическая зона откуда тащить барабан short target = tasks[i].start_zone; // фактическая зона откуда тащить барабан
if (tasks[i].dest_zone == -2) { if (tasks[i].dest_zone < 0) {
target = 18;
} else if (tasks[i].dest_zone < 0) {
continue; continue;
} }
@ -104,9 +101,34 @@ void schedule_one_robot(const struct scheduler_task* tasks, const struct robot_r
void scheduler_main() void scheduler_main()
#endif #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 (button_load) {
if (!zone_is_busy(1)) { if (!zone_is_busy(1)) {
for (int i = 0; i < BARRELS_COUNT; i++) { for (int i = 0; i < BARRELS_COUNT; i++) {
@ -145,37 +167,95 @@ void scheduler_main()
remove_barrel_from_zone(22); remove_barrel_from_zone(22);
} }
// таймер, применяется ко всем существующим барабанам if (!auto_mode_pause) {
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 ((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];
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 (robot_id != 0) {
for (short i = 0; i < BARRELS_COUNT; i++) { for (short i = 0; i < BARRELS_COUNT; i++) {
// для каждой задачи: // для каждой задачи:
tasks[i].start_zone = barrels[i].zone; tasks[i].start_zone = barrels[i].zone;
// определяем можно ли ее выполнить и что вообще нужно выполнить // определяем можно ли ее выполнить и что вообще нужно выполнить
tasks[i].dest_zone = can_move(barrels + i); tasks[i].dest_zone = can_move(barrels + i, robot_id);
if (tasks[i].dest_zone >= 0) { if (tasks[i].dest_zone >= 0) {
tasks[i].priority = get_operation_priority(i); tasks[i].priority = get_operation_priority(i);
} }
} }
if (one_robot_mode) { if (robot_id == 1) {
// режим одного робота
if (hla_robot1_en && robot1_code.PC < 0) {
schedule_one_robot(tasks, &robot1, &robot1_code, 1); schedule_one_robot(tasks, &robot1, &robot1_code, 1);
} else if (robot2_code.PC < 0 && hla_robot2_en) { } else {
schedule_one_robot(tasks, &robot2, &robot2_code, 2); schedule_one_robot(tasks, &robot2, &robot2_code, 2);
} }
}
} else { } 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);
}
}
}
} }
} }
} }

206
utils.c
View File

@ -9,6 +9,10 @@
#include "main.h" #include "main.h"
#endif #endif
#define LOCK_ZONE_BORDER 1
char zone_is_busy(short zone) { char zone_is_busy(short zone) {
for (short i = 0; i < BARRELS_COUNT; i++) { for (short i = 0; i < BARRELS_COUNT; i++) {
if (barrels[i].flags.is_exist && barrels[i].zone == zone) { if (barrels[i].flags.is_exist && barrels[i].zone == zone) {
@ -22,7 +26,11 @@ char zone_is_busy(short zone) {
// TODO обновить метод для работы с двумя роботами // TODO обновить метод для работы с двумя роботами
// вернет можно ли ехать и главное куда ехать, если можно (нельзя если вернулось значение < 0) // вернет можно ли ехать и главное куда ехать, если можно (нельзя если вернулось значение < 0)
// -1 вернет что перемещать нельзя // -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) { if (!bar->flags.is_exist) {
return -1; return -1;
@ -36,33 +44,28 @@ short can_move(struct barrel* bar) {
return -2; return -2;
} }
#ifdef EMULATOR
if (!one_robot_mode) {
printf("WARMING: нет проверки того, что для перемещения барабана не мешает второй робот\n");
}
#endif
// дальше нужно проверить, что можно передвигать бочку // дальше нужно проверить, что можно передвигать бочку
short dest_zone = -1;
switch (bar->zone) { switch (bar->zone) {
case ZONE_LOAD_2: case ZONE_LOAD_2:
// загрузка 2, только в нее можно грузить новые барабаны, нужно обезжиривание // загрузка 2, только в нее можно грузить новые барабаны, нужно обезжиривание
if (!zone_is_busy(ZONE_DEGREASING)) { if (!zone_is_busy(ZONE_DEGREASING)) {
return ZONE_DEGREASING; dest_zone = ZONE_DEGREASING;
} }
break; break;
case ZONE_DEGREASING: case ZONE_DEGREASING:
// обезжиривание, нужна промывка 1А // обезжиривание, нужна промывка 1А
if (!zone_is_busy(ZONE_WASHING_1A)) { if (!zone_is_busy(ZONE_WASHING_1A)) {
return ZONE_WASHING_1A; dest_zone = ZONE_WASHING_1A;
} }
break; break;
case ZONE_WASHING_1A: case ZONE_WASHING_1A:
// промывка 1А, нужна промывка 1Б // промывка 1А, нужна промывка 1Б
if (!zone_is_busy(ZONE_WASHING_1B)) { if (!zone_is_busy(ZONE_WASHING_1B)) {
return ZONE_WASHING_1B; dest_zone = ZONE_WASHING_1B;
} }
break; break;
@ -72,7 +75,7 @@ short can_move(struct barrel* bar) {
break; break;
} }
if (!zone_is_busy((short)(ZONE_ETCHING_1 + etching_zone))) { 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; break;
@ -80,14 +83,14 @@ short can_move(struct barrel* bar) {
case ZONE_ETCHING_2: case ZONE_ETCHING_2:
// травление, нужна промывка 2А // травление, нужна промывка 2А
if (!zone_is_busy(ZONE_WASHING_2A)) { if (!zone_is_busy(ZONE_WASHING_2A)) {
return ZONE_WASHING_2A; dest_zone = ZONE_WASHING_2A;
} }
break; break;
case ZONE_WASHING_2A: case ZONE_WASHING_2A:
// промывка 2А, нужна промывка 2Б // промывка 2А, нужна промывка 2Б
if (!zone_is_busy(ZONE_WASHING_2B)) { if (!zone_is_busy(ZONE_WASHING_2B)) {
return ZONE_WASHING_2B; dest_zone = ZONE_WASHING_2B;
} }
break; break;
@ -97,7 +100,7 @@ short can_move(struct barrel* bar) {
break; break;
} }
if (!zone_is_busy((short)(ZONE_GALVANIZING_1 + galvanizing_zone))) { 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; break;
@ -119,7 +122,7 @@ short can_move(struct barrel* bar) {
} }
} }
if (count < 3) { if (count < 3) {
return ZONE_WASHING_3A; dest_zone = ZONE_WASHING_3A;
} }
} }
break; break;
@ -127,14 +130,14 @@ short can_move(struct barrel* bar) {
case ZONE_WASHING_3A: case ZONE_WASHING_3A:
// промывка 3А, перекладываем в промывку 3Б // промывка 3А, перекладываем в промывку 3Б
if (!zone_is_busy(ZONE_WASHING_3B)) { if (!zone_is_busy(ZONE_WASHING_3B)) {
return ZONE_WASHING_3B; dest_zone = ZONE_WASHING_3B;
} }
break; break;
case ZONE_WASHING_3B: case ZONE_WASHING_3B:
// это перед пассивацией, требует свободную промывку 4А и на всякий случай свободную пассивацию // это перед пассивацией, требует свободную промывку 4А и на всякий случай свободную пассивацию
if (!zone_is_busy(ZONE_PASSIVATION) && !zone_is_busy(ZONE_WASHING_4A)) { if (!zone_is_busy(ZONE_PASSIVATION) && !zone_is_busy(ZONE_WASHING_4A)) {
return ZONE_PASSIVATION; dest_zone = ZONE_PASSIVATION;
} }
// это атомарная операция, по идее вносить барабан в пассивацию нельзя // это атомарная операция, по идее вносить барабан в пассивацию нельзя
break; break;
@ -143,21 +146,21 @@ short can_move(struct barrel* bar) {
// процесс пассивации, нужна промывка 4A // процесс пассивации, нужна промывка 4A
// чисто теоретически сюда никогда не попадем, но если вдруг выстрелит пусть будет // чисто теоретически сюда никогда не попадем, но если вдруг выстрелит пусть будет
if (!zone_is_busy(ZONE_WASHING_4A)) { if (!zone_is_busy(ZONE_WASHING_4A)) {
return ZONE_WASHING_4A; dest_zone = ZONE_WASHING_4A;
} }
break; break;
case ZONE_WASHING_4A: case ZONE_WASHING_4A:
// промывка 4А, перекладываем в промывку 4Б // промывка 4А, перекладываем в промывку 4Б
if (!zone_is_busy(ZONE_WASHING_4B)) { if (!zone_is_busy(ZONE_WASHING_4B)) {
return ZONE_WASHING_4B; dest_zone = ZONE_WASHING_4B;
} }
break; break;
case ZONE_WASHING_4B: case ZONE_WASHING_4B:
// процесс пассивации, нужна промывка 4B (зона 21) (потому что сейчас я в 4A) // процесс пассивации, нужна промывка 4B (зона 21) (потому что сейчас я в 4A)
if (!zone_is_busy(ZONE_UNLOAD)) { if (!zone_is_busy(ZONE_UNLOAD)) {
return ZONE_UNLOAD; dest_zone = ZONE_UNLOAD;
} }
break; break;
@ -166,12 +169,41 @@ short can_move(struct barrel* bar) {
if (one_robot_mode && button_unload) { if (one_robot_mode && button_unload) {
// нужно промывку загрузку 0 // нужно промывку загрузку 0
if (!zone_is_busy(ZONE_LOAD_1)) { if (!zone_is_busy(ZONE_LOAD_1)) {
return ZONE_LOAD_1; dest_zone = ZONE_LOAD_1;
} }
} }
break; 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; continue;
} }
if (barrels[i].flags.is_exist && barrels[i].zone >= ZONE_GALVANIZING_1 && barrels[i].zone <= ZONE_GALVANIZING_8) { 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; is_not_one = 1;
// чем больше у барабана время ожидания тем меньше у него число // чем больше у барабана время ожидания тем меньше у него число
if (barrels[i].software_timer < barrels[barrel_id].software_timer) { 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 (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) { if (barrels[i].software_timer < barrels[barrel_id].software_timer) {
return 0; // у этого барабана больше время ожидания (число меньше), значит приоритет 0 return 0; // у этого барабана больше время ожидания (число меньше), значит приоритет 0
} else { } 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); printf(" set barrel timer %d secs\n", cmd_arg);
break; break;
case ROBOT_CMD_SET_LOCK_ZONE_code:
printf(" set lock zone %d\n", cmd_arg);
break;
case ROBOT_CMD_CORRECT_AXIS_code: case ROBOT_CMD_CORRECT_AXIS_code:
if (cmd_arg == ROBOT_AXIS_X) { if (cmd_arg == ROBOT_AXIS_X) {
printf(" correct axis: X\n"); 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 зоне мы никогда не закончим, за нее не беспокоится) // NOTE первой командой на любую транзакцию должна стоять команда опустить траверсу (в 22 зоне мы никогда не закончим, за нее не беспокоится)
если режим двух роботов:
если команда для робота 1:
robot1_lock_zone = min(зона изъятия, зона назначения)
иначе:
robot2_lock_zone = max(зона изъятия, зона назначения)
* опустить траверсу * опустить траверсу
если зона изъятия != промывка 3б если зона изъятия != промывка 3б:
если текущая зона != зона изъятия если текущая зона != зона изъятия:
если зона изъятия == 22 если зона изъятия == 22:
* встать на смещенную * встать на смещенную
* поднять траверсу * поднять траверсу
* съебать в 21 зону * уехать в 22 зону
* встать на смещенную * встать на смещенную
* опустить траверсу не до конца * опустить траверсу
* съебать в 22 зону * встать в точную
иначе иначе:
* съебать в зону изъятия * уехать в зону изъятия
* поднять траверсу с барабаном если режим двух роботов:
если зона изъятия != 22 и зона изъятия != 1: * установить новую lock-зону
* поднять траверсу
если зона изъятия != 22 и зона изъятия >= 2:
* ждать скапывания (зависит от зоны) * ждать скапывания (зависит от зоны)
* ехать в зону назначения * ехать в зону назначения
если зона назначения == 22 если зона назначения == 22:
* опустить траверсу не до конца с барабаном * опустить траверсу
* съебать в 21 зону * уехать в 21 зону
если ЭНКОДЕРЫ СТАРЫЕ (по умолчанию) если ЭНКОДЕРЫ СТАРЫЕ (по умолчанию):
* поднять траверсу * поднять траверсу
* опустить траверсу * опустить траверсу
иначе
* опустить траверсу с барабаном если зона назначения != 22 и зона назначения != 0:
если зона назначения != 0
* установить время ожидания барабана (зависит от зоны) * установить время ожидания барабана (зависит от зоны)
иначе иначе:
если текущая зона != промывка 3б если текущая зона != промывка 3б:
* съебать в промывку 3б * уехать в промывку 3б
* поднять траверсу с барабаном * поднять траверсу
* съебать в пассивацию * уехать в пассивацию
* опустить траверсу с барабаном * опустить траверсу
* поставить время ожидания барабана в <время пассивации> * ждать <время пассивации>
* поднять траверсу с барабаном * поднять траверсу
* съебать в зону промывка 4а * уехать в зону промывка 4а
* опустить траверсу с барабаном * опустить траверсу
* установить время ожидания барабана (для промывки 4а) * установить время ожидания барабана (для промывки 4а)
*/ */
void create_operation(struct robot_code *code, const short barrel_id, const short start_zone, const short dest_zone, 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; code->barrel_id = barrel_id;
short cmd_index = 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_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(); 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(); 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 (!one_robot_mode) {
if (robot_id == 1) { if (robot_id == 2) {
if (dest_zone >= ZONE_GALVANIZING_1) { if (dest_zone >= ZONE_GALVANIZING_1) {
// из промывки 2б он перекладывал, пусть едет в промывку 2а // из промывки 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); code->code[cmd_index++] = ROBOT_CMD_MOVE_TO_ZONE(ZONE_WASHING_2A);
} else if (dest_zone < ZONE_DEGREASING) { } 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); 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) { 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); code->code[cmd_index++] = ROBOT_CMD_MOVE_TO_ZONE(ZONE_WASHING_4A);
} }
} }

15
utils.h
View File

@ -184,6 +184,15 @@ extern short hla_time_washing_2;
extern char hla_robot1_en; extern char hla_robot1_en;
extern char hla_robot2_en; extern char hla_robot2_en;
// пауза для автоматического режима, при паузе идут таймера, но команды не могут быть выданы
extern char auto_mode_pause;
// разрешение на работу планировщика
extern char scheduler_en;
// сигнал инициализации планировщика, должен быть установлен по фронту разрешения на работу
extern char scheduler_start_signal;
#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))
@ -213,6 +222,8 @@ extern char hla_robot2_en;
#define hla_robot1_en _c_hla_robot1_en #define hla_robot1_en _c_hla_robot1_en
#define hla_robot2_en _c_hla_robot2_en #define hla_robot2_en _c_hla_robot2_en
#define auto_mode_pause _c_auto_mode_pause
#endif #endif
@ -225,6 +236,7 @@ extern char hla_robot2_en;
#define ROBOT_CMD_DOWN_code (short)0x5000 #define ROBOT_CMD_DOWN_code (short)0x5000
#define ROBOT_CMD_WAIT_code (short)0x6000 #define ROBOT_CMD_WAIT_code (short)0x6000
#define ROBOT_CMD_TMR_SET_code (short)0x7000 #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_CORRECT_AXIS_code (short)0x9000
#define ROBOT_CMD_INC_ZONE_code (short)0xA000 #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_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_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_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_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); 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); short get_operation_priority(short barrel_id);
char remove_barrel_from_zone(short zone); char remove_barrel_from_zone(short zone);