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

Исправление команды коррекции, потенциально убраны ошибки преобразования robot code, исправлен баг с зависшим роботом

Browse Source
This commit is contained in:
VladislavOstapov
2023-01-29 13:04:06 +03:00
parent d90b31cc7a
commit 353aad9bec
5 changed files with 105 additions and 46 deletions
Download Patch File Download Diff File Expand all files Collapse all files

84
scheduler.c
View File

@@ -98,6 +98,10 @@ void scheduler_main()
#endif
{
if (scheduler_start_signal) {
scheduler_correction_stage = 0;
robot1_code.PC = -1;
robot2_code.PC = -1;
for (short i = 0; i < BARRELS_COUNT; i++) {
// после рестарта планировщика надо всем барабаном убрать робота,
// всех кто сверху удалить,
@@ -176,24 +180,72 @@ void scheduler_main()
}
if (hla_correct_command) {
if (hla_robot2_en && (!robot2.mx.correct_sensor || !robot2.mz.correct_status)) {
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;
}
} else if (hla_robot1_en && (!robot1.mx.correct_sensor || !robot1.mz.correct_status)) {
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;
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 {
hla_correct_command = 0;
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 ((hla_robot1_en && robot1_code.PC < 0) || (hla_robot2_en && robot2_code.PC < 0)) {