Готов механизм транзакций на роботе с пачкой команд
This commit is contained in:
235
robot.cpp
235
robot.cpp
@@ -19,7 +19,7 @@ static short get_barrel(char robot_id) {
|
||||
}
|
||||
|
||||
// true означает что движение закончено
|
||||
static bool robot_move(robot_regs& r, int target, char robot_id) {
|
||||
static bool robot_move(robot_regs& r, short target, char robot_id) {
|
||||
// после перемещения мы явно в точной позиции
|
||||
if (robot_id == 1) {
|
||||
robot1_offset_pos = false;
|
||||
@@ -39,167 +39,21 @@ static bool robot_move(robot_regs& r, int target, char robot_id) {
|
||||
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) {
|
||||
static void emulate_robot(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);
|
||||
const short cmd_arg = code.code[code.PC] & (~ROBOT_CMD_MASK);
|
||||
|
||||
switch (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++;
|
||||
if (robot_move(r, cmd_arg & (~ROBOT_WITH_BARREL), robot_id)) {
|
||||
code.PC++;
|
||||
}
|
||||
if (cmd_arg & ROBOT_WITH_BARREL && code.barrel_id >= 0) {
|
||||
barrels[code.barrel_id].zone = r.dx.current_zone;
|
||||
}
|
||||
break;
|
||||
|
||||
@@ -209,38 +63,42 @@ static void emulate_robot_v2(robot_code* code, robot_regs& r, char robot_id) {
|
||||
} else {
|
||||
robot2_offset_pos = true;
|
||||
}
|
||||
code->PC++;
|
||||
code.PC++;
|
||||
break;
|
||||
|
||||
case ROBOT_CMD_UP:
|
||||
if (code->barrel_id >= 0) {
|
||||
barrels[code->barrel_id].flags.is_up = true;
|
||||
if (code.barrel_id >= 0 && cmd_arg) {
|
||||
// не давать ехать перед тем как истечет таймер
|
||||
if (barrels[code.barrel_id].software_timer > 0) {
|
||||
break;
|
||||
}
|
||||
barrels[code.barrel_id].flags.is_up = true;
|
||||
r.mz.is_up = 1;
|
||||
}
|
||||
code->PC++;
|
||||
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;
|
||||
if (code.barrel_id >= 0 && cmd_arg) {
|
||||
barrels[code.barrel_id].flags.is_up = false;
|
||||
r.mz.is_up = 0;
|
||||
}
|
||||
code->PC++;
|
||||
code.PC++;
|
||||
break;
|
||||
|
||||
case ROBOT_CMD_WAIT:
|
||||
std::cout << "robot " << robot_id << " wait " << cmd_arg << " secs..." << std::endl;
|
||||
code->PC++;
|
||||
code.PC++;
|
||||
break;
|
||||
|
||||
case ROBOT_CMD_TMR_SET:
|
||||
if (code->barrel_id >= 0) {
|
||||
barrels[code->barrel_id].software_timer = code->code[code->PC + 1];
|
||||
if (code.barrel_id >= 0) {
|
||||
barrels[code.barrel_id].software_timer = code.code[code.PC + 1];
|
||||
r.mz.is_up = 0;
|
||||
}
|
||||
code->PC += 2;
|
||||
code.PC += 2;
|
||||
break;
|
||||
|
||||
case ROBOT_CMD_SET_LOCK_ZONE:
|
||||
@@ -249,31 +107,46 @@ static void emulate_robot_v2(robot_code* code, robot_regs& r, char robot_id) {
|
||||
} else {
|
||||
robot2_lock_zone = cmd_arg;
|
||||
}
|
||||
code->PC += 2;
|
||||
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++;
|
||||
case ROBOT_CMD_CORRECT_AXIS:
|
||||
if (cmd_arg == ROBOT_AXIS_X) {
|
||||
std::cout << "robot " << robot_id << " correct axis X..." << std::endl;
|
||||
r.dz.current_zone = 0;
|
||||
r.mx.correct_status = true;
|
||||
} else if (cmd_arg == ROBOT_AXIS_Z) {
|
||||
std::cout << "robot " << robot_id << " correct axis Z..." << std::endl;
|
||||
r.mz.is_up = true;
|
||||
r.mz.correct_status = true;
|
||||
} else {
|
||||
std::cout << "ERROR: R" << robot_id << " AXIS CORRECT - INCORRECT ARGUMENT VALUE " << cmd_arg << std::endl;
|
||||
}
|
||||
|
||||
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++;
|
||||
case ROBOT_CMD_INC_ZONE:
|
||||
if (cmd_arg == ROBOT_ZONE_ETCH) {
|
||||
std::cout << "robot " << robot_id << " increment etching..." << std::endl;
|
||||
etching_zone = (etching_zone + 1) & 1;
|
||||
} else if (cmd_arg == ROBOT_ZONE_GAL) {
|
||||
std::cout << "robot " << robot_id << " increment galvanic..." << std::endl;
|
||||
galvanizing_zone = (galvanizing_zone + 1) & 0x07;
|
||||
} else {
|
||||
std::cout << "ERROR: R" << robot_id << " INCREMENT ZONE - INCORRECT ARGUMENT VALUE " << cmd_arg << std::endl;
|
||||
}
|
||||
code.PC++;
|
||||
break;
|
||||
|
||||
case ROBOT_CMD_END:
|
||||
default:
|
||||
code->PC = -1;
|
||||
code.PC = -1;
|
||||
}
|
||||
}
|
||||
|
||||
|
||||
void robot_main() {
|
||||
emulate_robot(robot1_cmd, robot1, 1);
|
||||
emulate_robot(robot2_cmd, robot2, 2);
|
||||
emulate_robot(robot1_code, robot1, 1);
|
||||
emulate_robot(robot2_code, robot2, 2);
|
||||
}
|
||||
|
||||
Reference in New Issue
Block a user