280 lines
8.2 KiB
C++
280 lines
8.2 KiB
C++
//
|
||
// Created by Владислав Остапов on 05.11.2022.
|
||
//
|
||
|
||
#include "robot.h"
|
||
#include <iostream>
|
||
|
||
bool robot1_offset_pos = false;
|
||
bool robot2_offset_pos = false;
|
||
|
||
|
||
static short get_barrel(char robot_id) {
|
||
for (short i = 0; i < BARRELS_COUNT; i++) {
|
||
if (barrels[i].flags.robot == robot_id) {
|
||
return i;
|
||
}
|
||
}
|
||
return -1;
|
||
}
|
||
|
||
// true означает что движение закончено
|
||
static bool robot_move(robot_regs& r, int target, char robot_id) {
|
||
// после перемещения мы явно в точной позиции
|
||
if (robot_id == 1) {
|
||
robot1_offset_pos = false;
|
||
} else {
|
||
robot2_offset_pos = false;
|
||
}
|
||
|
||
if (r.dx.current_zone == target) {
|
||
return true;
|
||
}
|
||
|
||
if (r.dx.current_zone < target) {
|
||
r.dx.current_zone++;
|
||
} else {
|
||
r.dx.current_zone--;
|
||
}
|
||
return false;
|
||
}
|
||
|
||
static void emulate_robot(robot_cmd& cmd, robot_regs& r, char robot_id) {
|
||
auto barrel = get_barrel(robot_id);
|
||
switch (cmd.cmd) {
|
||
case 1:
|
||
// команда просто уехать
|
||
switch (cmd.step) {
|
||
case 0:
|
||
// двигаемся в сторону цели
|
||
if (robot_move(r, cmd.args[0], robot_id)) {
|
||
cmd.step++;
|
||
}
|
||
break;
|
||
|
||
default:
|
||
cmd.cmd = 0;
|
||
cmd.step = 0;
|
||
}
|
||
break;
|
||
|
||
case 2:
|
||
// команда взять барабан и увезти его куда положено
|
||
switch (cmd.step) {
|
||
case 0:
|
||
// двигаемся в сторону барабана
|
||
if (robot_move(r, cmd.args[0], robot_id)) {
|
||
cmd.step++;
|
||
}
|
||
break;
|
||
|
||
case 1:
|
||
// поднимаем траверсу
|
||
r.mz.is_up = 1;
|
||
if (barrel != -1) {
|
||
barrels[barrel].flags.is_up = 1;
|
||
}
|
||
cmd.step++;
|
||
break;
|
||
|
||
case 2:
|
||
// двигаемся в сторону выгрузки
|
||
if (robot_move(r, cmd.args[1], robot_id)) {
|
||
cmd.step++;
|
||
}
|
||
break;
|
||
|
||
case 3:
|
||
// опускаем траверсу
|
||
r.mz.is_up = 0;
|
||
if (barrel != -1) {
|
||
barrels[barrel].flags.is_up = 0;
|
||
}
|
||
// ну и тут же конец
|
||
|
||
default:
|
||
cmd.cmd = 0;
|
||
cmd.step = 0;
|
||
}
|
||
break;
|
||
|
||
case 3:
|
||
// команда пассивация
|
||
switch (cmd.step) {
|
||
case 0:
|
||
// двигаемся в 18 зону
|
||
if (robot_move(r, 18, robot_id)) {
|
||
cmd.step++;
|
||
}
|
||
break;
|
||
|
||
case 1:
|
||
// поднимаем траверсу
|
||
r.mz.is_up = 1;
|
||
if (barrel != -1) {
|
||
barrels[barrel].flags.is_up = 1;
|
||
}
|
||
cmd.step++;
|
||
break;
|
||
|
||
case 2:
|
||
// двигаемся в 19 зону
|
||
if (robot_move(r, 19, robot_id)) {
|
||
cmd.step++;
|
||
}
|
||
break;
|
||
|
||
case 3:
|
||
// опускаем траверсу
|
||
r.mz.is_up = 0;
|
||
if (barrel != -1) {
|
||
barrels[barrel].flags.is_up = 0;
|
||
barrels[barrel].software_timer = barrels[barrel].time_passivation;
|
||
}
|
||
cmd.step++;
|
||
break;
|
||
|
||
case 4:
|
||
// ждем пока барабан отстоит таймер в пассивации
|
||
if (barrel != -1) {
|
||
if (barrels[barrel].software_timer <= 0) {
|
||
cmd.step++;
|
||
}
|
||
}
|
||
break;
|
||
|
||
case 5:
|
||
// поднимаем траверсу
|
||
r.mz.is_up = 1;
|
||
if (barrel != -1) {
|
||
barrels[barrel].flags.is_up = 1;
|
||
}
|
||
cmd.step++;
|
||
break;
|
||
|
||
case 6:
|
||
// двигаемся в 20 зону
|
||
if (robot_move(r, 20, robot_id)) {
|
||
cmd.step++;
|
||
}
|
||
break;
|
||
|
||
case 7:
|
||
// опускаем траверсу
|
||
r.mz.is_up = 0;
|
||
if (barrel != -1) {
|
||
barrels[barrel].flags.is_up = 0;
|
||
// и тут же ставим время промывки барабана
|
||
barrels[barrel].software_timer = barrels[barrel].time_washing_4a;
|
||
}
|
||
// ну и тут же конец
|
||
|
||
default:
|
||
cmd.cmd = 0;
|
||
cmd.step = 0;
|
||
}
|
||
break;
|
||
|
||
|
||
default:
|
||
cmd.cmd = 0;
|
||
cmd.step = 0;
|
||
}
|
||
|
||
if (r.mz.is_up) {
|
||
if (barrel != -1) {
|
||
barrels[barrel].zone = r.dx.current_zone;
|
||
}
|
||
}
|
||
}
|
||
|
||
|
||
static void emulate_robot_v2(robot_code* code, robot_regs& r, char robot_id) {
|
||
if (code->PC < 0) {
|
||
return;
|
||
}
|
||
const short cmd_arg = code->code[code->PC] & (~ROBOT_CMD_MASK);
|
||
|
||
switch (code->code[code->PC] & ROBOT_CMD_MASK) {
|
||
case ROBOT_CMD_MOVE_TO_ZONE:
|
||
// двигаемся в сторону цели
|
||
if (robot_move(r, cmd_arg, robot_id)) {
|
||
code->PC++;
|
||
}
|
||
break;
|
||
|
||
case ROBOT_CMD_MOVE_OFF:
|
||
if (robot_id == 1) {
|
||
robot1_offset_pos = true;
|
||
} else {
|
||
robot2_offset_pos = true;
|
||
}
|
||
code->PC++;
|
||
break;
|
||
|
||
case ROBOT_CMD_UP:
|
||
if (code->barrel_id >= 0) {
|
||
barrels[code->barrel_id].flags.is_up = true;
|
||
r.mz.is_up = 1;
|
||
}
|
||
code->PC++;
|
||
break;
|
||
|
||
// в эмуляторе не важно где я, поэтому тут обе команды вниз обрабатываются одинаково
|
||
case ROBOT_CMD_DOWN:
|
||
case ROBOT_CMD_DOWN_2:
|
||
if (code->barrel_id >= 0) {
|
||
barrels[code->barrel_id].flags.is_up = false;
|
||
r.mz.is_up = 0;
|
||
}
|
||
code->PC++;
|
||
break;
|
||
|
||
case ROBOT_CMD_WAIT:
|
||
std::cout << "robot " << robot_id << " wait " << cmd_arg << " secs..." << std::endl;
|
||
code->PC++;
|
||
break;
|
||
|
||
case ROBOT_CMD_TMR_SET:
|
||
if (code->barrel_id >= 0) {
|
||
barrels[code->barrel_id].software_timer = code->code[code->PC + 1];
|
||
r.mz.is_up = 0;
|
||
}
|
||
code->PC += 2;
|
||
break;
|
||
|
||
case ROBOT_CMD_SET_LOCK_ZONE:
|
||
if (robot_id == 1) {
|
||
robot1_lock_zone = cmd_arg;
|
||
} else {
|
||
robot2_lock_zone = cmd_arg;
|
||
}
|
||
code->PC += 2;
|
||
break;
|
||
|
||
case ROBOT_CMD_CORRECT_X:
|
||
std::cout << "robot " << robot_id << " correct axis X..." << std::endl;
|
||
r.dz.current_zone = 0;
|
||
r.mx.correct_status = true;
|
||
code->PC++;
|
||
break;
|
||
|
||
case ROBOT_CMD_CORRECT_Z:
|
||
std::cout << "robot " << robot_id << " correct axis Z..." << std::endl;
|
||
r.mz.is_up = true;
|
||
r.mz.correct_status = true;
|
||
code->PC++;
|
||
break;
|
||
|
||
case ROBOT_CMD_END:
|
||
default:
|
||
code->PC = -1;
|
||
}
|
||
}
|
||
|
||
|
||
void robot_main() {
|
||
emulate_robot(robot1_cmd, robot1, 1);
|
||
emulate_robot(robot2_cmd, robot2, 2);
|
||
}
|