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
2022-12-02 22:29:36 +03:00
parent aa8d949323
commit 8f19d1b277
8 changed files with 461 additions and 120 deletions
Download Patch File Download Diff File Expand all files Collapse all files

111
robot.cpp
View File

@@ -3,6 +3,11 @@
//
#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++) {
@@ -14,20 +19,27 @@ static short get_barrel(char robot_id) {
}
// true означает что движение закончено
static bool robot_move(robot& r, int target, char robot_id) {
if (r.curr_zone == target) {
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.curr_zone < target) {
r.curr_zone++;
if (r.dx.current_zone < target) {
r.dx.current_zone++;
} else {
r.curr_zone--;
r.dx.current_zone--;
}
return false;
}
static void emulate_robot(robot_cmd& cmd, robot& r, char robot_id) {
static void emulate_robot(robot_cmd& cmd, robot_regs& r, char robot_id) {
auto barrel = get_barrel(robot_id);
switch (cmd.cmd) {
case 1:
@@ -171,11 +183,96 @@ static void emulate_robot(robot_cmd& cmd, robot& r, char robot_id) {
if (r.mz.is_up) {
if (barrel != -1) {
barrels[barrel].zone = r.curr_zone;
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);