diff --git a/scheduler.c b/scheduler.c index ec0244b..48316b3 100644 --- a/scheduler.c +++ b/scheduler.c @@ -110,30 +110,6 @@ void scheduler_main() scheduler_start_signal = 0; } - if (hla_night_mode) { - robot1_lock_zone = -1; - robot2_lock_zone = -1; - -// // ставим всем барабанам которые в промывках флаг ночного барабана (кроме тех, что заняты роботами) -// for (short i = 0; i < BARRELS_COUNT; i++) { -// if (barrels[i].flags.is_exist && barrels[i].flags.robot == 0) { -// const short zones[] = { -// ZONE_LOAD_1, -// ZONE_WASHING_1A, ZONE_WASHING_1B, -// ZONE_WASHING_2A, ZONE_WASHING_2B, -// ZONE_WASHING_3A, ZONE_WASHING_3B, -// ZONE_WASHING_4A, ZONE_WASHING_4B -// }; -// for (short t = 0; t < 9; t++) { -// if (barrels[i].zone == zones[t]) { -// barrels[i].flags.is_night = -1; -// break; -// } -// } -// } -// } - } - if (scheduler_en) { // программный таймер, применяется ко всем существующим барабанам @@ -187,7 +163,27 @@ void scheduler_main() remove_barrel_from_zone(22); } - if (!auto_mode_pause) { + 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[0] = 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[0] = ROBOT_CMD_CORRECT_AXIS(ROBOT_AXIS_X); + robot1_code.code[2] = ROBOT_CMD_END(); + robot1_code.PC = 0; + } + } else { + hla_correct_command = 0; + } + } else 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]; @@ -282,7 +278,7 @@ void scheduler_main() } // и отдельно для второго (только если не в ночном режиме) - if (!hla_night_mode && robot2_code.PC < 0) { + if (robot2_code.PC < 0) { for (short i = 0; i < BARRELS_COUNT; i++) { // для каждой задачи: tasks[i].start_zone = barrels[i].zone;