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-02-01 18:12:34 +03:00
parent dbc7143f01
commit c4ed3b76f1
1 changed files with 84 additions and 70 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

154
scheduler.c
View File

@ -114,7 +114,89 @@ void scheduler_main()
scheduler_start_signal = 0;
}
if (scheduler_en) {
if (hla_correct_command) {
if (one_robot_mode) {
switch (scheduler_correction_stage) {
case 0:
if (hla_robot2_en) {
if (robot2_code.PC < 0) {
robot2_code.barrel_id = -1;
robot2_code.code[0] = ROBOT_CMD_CORRECT_AXIS(ROBOT_AXIS_Z);
robot2_code.code[1] = ROBOT_CMD_CORRECT_AXIS(ROBOT_AXIS_X);
robot2_code.code[2] = ROBOT_CMD_END();
robot2_code.PC = 0;
scheduler_correction_stage++;
}
} else {
if (robot1_code.PC < 0) {
robot1_code.barrel_id = -1;
robot1_code.code[0] = ROBOT_CMD_CORRECT_AXIS(ROBOT_AXIS_Z);
robot1_code.code[1] = ROBOT_CMD_CORRECT_AXIS(ROBOT_AXIS_X);
robot1_code.code[2] = ROBOT_CMD_MOVE_TO_PARKING();
robot1_code.code[3] = ROBOT_CMD_END();
robot1_code.PC = 0;
scheduler_correction_stage++;
}
}
case 1:
if (robot2_code.PC < 0 && robot1_code.PC < 0) {
scheduler_correction_stage++;
}
break;
default:
scheduler_correction_stage = 0;
hla_correct_command = 0;
}
} else {
switch (scheduler_correction_stage) {
case 0:
// сначала робота 2 отправляем на коррекцию
if (robot2_code.PC < 0) {
robot2_code.barrel_id = -1;
robot2_code.code[0] = ROBOT_CMD_CORRECT_AXIS(ROBOT_AXIS_Z);
robot2_code.code[1] = ROBOT_CMD_CORRECT_AXIS(ROBOT_AXIS_X);
robot2_code.code[2] = ROBOT_CMD_END();
robot2_code.PC = 0;
scheduler_correction_stage++;
}
break;
case 1:
if (robot2_code.PC < 0) {
scheduler_correction_stage++;
}
break;
case 2:
// потом робот 1
if (robot1_code.PC < 0) {
robot1_code.barrel_id = -1;
robot1_code.code[0] = ROBOT_CMD_CORRECT_AXIS(ROBOT_AXIS_Z);
robot1_code.code[1] = ROBOT_CMD_CORRECT_AXIS(ROBOT_AXIS_X);
robot1_code.code[2] = ROBOT_CMD_MOVE_TO_PARKING();
robot1_code.code[3] = ROBOT_CMD_END();
robot1_code.PC = 0;
scheduler_correction_stage++;
}
break;
case 3:
if (robot1_code.PC < 0) {
scheduler_correction_stage++;
}
break;
default:
scheduler_correction_stage = 0;
hla_correct_command = 0;
}
}
} else if (scheduler_en) {
// программный таймер, применяется ко всем существующим барабанам
if (_scheduler_software_timer) {
@ -134,75 +216,7 @@ void scheduler_main()
increment_zone(ROBOT_ZONE_GAL);
}
if (hla_correct_command) {
if (one_robot_mode) {
if (hla_robot2_en) {
if (robot2_code.PC < 0) {
robot2_code.barrel_id = -1;
robot2_code.code[0] = ROBOT_CMD_CORRECT_AXIS(ROBOT_AXIS_Z);
robot2_code.code[1] = ROBOT_CMD_CORRECT_AXIS(ROBOT_AXIS_X);
robot2_code.code[2] = ROBOT_CMD_END();
robot2_code.PC = 0;
hla_correct_command = 0;
}
} else {
if (robot1_code.PC < 0) {
robot1_code.barrel_id = -1;
robot1_code.code[0] = ROBOT_CMD_CORRECT_AXIS(ROBOT_AXIS_Z);
robot1_code.code[1] = ROBOT_CMD_CORRECT_AXIS(ROBOT_AXIS_X);
robot1_code.code[2] = ROBOT_CMD_END();
robot1_code.PC = 0;
hla_correct_command = 0;
}
}
} else {
switch (scheduler_correction_stage) {
case 0:
// сначала робота 2 отправляем на коррекцию
if (robot2_code.PC < 0) {
robot2_code.barrel_id = -1;
robot2_code.code[0] = ROBOT_CMD_CORRECT_AXIS(ROBOT_AXIS_Z);
robot2_code.code[1] = ROBOT_CMD_CORRECT_AXIS(ROBOT_AXIS_X);
robot2_code.code[2] = ROBOT_CMD_END();
robot2_code.PC = 0;
scheduler_correction_stage++;
}
break;
case 1:
if (robot2_code.PC < 0) {
scheduler_correction_stage++;
}
break;
case 2:
// потом робот 1
if (robot1_code.PC < 0) {
robot1_code.barrel_id = -1;
robot1_code.code[0] = ROBOT_CMD_CORRECT_AXIS(ROBOT_AXIS_Z);
robot1_code.code[1] = ROBOT_CMD_CORRECT_AXIS(ROBOT_AXIS_X);
robot1_code.code[2] = ROBOT_CMD_END();
robot1_code.PC = 0;
scheduler_correction_stage++;
}
break;
case 3:
if (robot1_code.PC < 0) {
scheduler_correction_stage++;
}
break;
default:
scheduler_correction_stage = 0;
hla_correct_command = 0;
}
}
} else if (!auto_mode_pause) {
if (!auto_mode_pause) {
if ((hla_robot1_en && robot1_code.PC < 0) || (hla_robot2_en && robot2_code.PC < 0)) {
struct scheduler_task tasks[BARRELS_COUNT];