Загрузка данных


void motionTask(void * parameter) {
    const float TIMER_FREQ = 100000.0f; // 100 кГц

    while (true) {
        if (!nextSeg.hasNewData) {
            if (bufferTail == bufferHead) { isVectorMoving = false; }
            vTaskDelay(pdMS_TO_TICKS(1)); 
            continue;
        }

        // Забираем геометрическую цель из ОЗУ УП
        float nX = nextSeg.x; float nY = nextSeg.y; float nZ = nextSeg.z;
        float feed = nextSeg.f; float accel = nextSeg.a;// зачем нам тут нужно ускорение?
        nextSeg.hasNewData = false; // Мгновенно освобождаем сокет для следующего кадра Look-Ahead!

        // Применяем лазерную поправку провиса станины фермы
        CorrectionPoint corr = getCorrection(nX);
        float realMachineY = nY + corr.y_offset;
        float realMachineZ = nZ + corr.z_offset;

        // ВЫЧИСЛЯЕМ АБСОЛЮТНЫЕ ЦЕЛИ В ЦЕЛЫХ ЧИСЛАХ (ШШАГАХ МОТОРОВ)
        long targetStepsX = nX * cfg.stepsPerMmX;
        long targetStepsY = realMachineY * cfg.stepsPerMmY;
        long targetStepsZ = realMachineZ * cfg.stepsPerMmZ;

        // Считаем чистую, неделимую дельту шагов, которую ОБЯЗАНА сделать каждая ось
        long totalDeltaX = targetStepsX - plannerStepsX;
        long totalDeltaY = targetStepsY - plannerStepsY;
        long totalDeltaZ = targetStepsZ - plannerStepsZ;

        long absDeltaX = labs(totalDeltaX);
        long absDeltaY = labs(totalDeltaY);
        long absDeltaZ = labs(totalDeltaZ);

        // Находим ведущую ось (максимальное количество шагов в этом кадре)
        long maxSegmentSteps = absDeltaX;
        if (absDeltaY > maxSegmentSteps) maxSegmentSteps = absDeltaY;
        if (absDeltaZ > maxSegmentSteps) maxSegmentSteps = absDeltaZ;

        if (maxSegmentSteps == 0) { continue; } // Кадр пустой, пропускаем

        // ОПРЕДЕЛЕНИЕ СКОРОСТЕЙ НА СТЫКАХ КАДРОВ (Look-Ahead Blending)
        float v_max = feed / 60.0f; // для чего feed делить на 60?
        float v_min = cfg.minVectorSpeed;// и что за minVectorSpeed откуда он взялся и для чего нужен?

        // Входная скорость оси — это то, на чем закончил прошлый кадр
        float v_start = lastFrameEndSpeedX; // Для упрощения считаем по ведущему вектору
        if (v_start < v_min) v_start = v_min;// для чего эта проверка и почему общая скорость когда для точной работы нужны отделные скорости каждой оси?

        // Выходная скорость стыка для следующего кадра.
        // Смотрим наперед: если траектория УП продолжается — держим скорость. 
        // Если это финал программы или резкий поворот 90 градусов — гасим скорость до minSpeed.
        float v_end = v_min;
        extern int currentCommandIndex;
        extern int totalLoadedCommands;
        if (currentCommandIndex < totalLoadedCommands) {
            v_end = v_max; // Look-Ahead: сопрягаем стык без остановки портала
        }// в чем вообще логика v_end и зачем он нужен? надо бежать по программе в поиске команд движения и вычленять скорости для всех осей

        // РАСЧЕТ ТРАПЕЦИИ УСКОРЕНИЯ СТРОГО В ЦЕЛЫХ ШАГАХ МОТОРА
        float currentMachineStepsPerMm = cfg.stepsPerMmX;
        
        // Шаги разгона
        long stepsAccel = ((v_max * v_max - v_start * v_start) / (2.0f * accel)) * currentMachineStepsPerMm; //по этому коду бы тоже хотелось развернуюую справку что зачем и почему
        // Шаги торможения
        long stepsDecel = ((v_max * v_max - v_end * v_end) / (2.0f * accel)) * currentMachineStepsPerMm;
        long stepsCruise = 0;

        // Проверка лимита отрезка (пересчет трапеции в треугольник, если не успеваем разогнаться)
        if (stepsAccel + stepsDecel > maxSegmentSteps) {
            float halfSteps = maxSegmentSteps / 2.0f;
            stepsAccel = halfSteps;
            stepsDecel = maxSegmentSteps - stepsAccel;// какой смысл в этой логике? ведь тут stepsAccel всегда будет == stepsDecel
            stepsCruise = 0;
            // Пересчитываем пиковую скорость треугольника
            v_max = sqrt(v_start * v_start + 2.0f * accel * ((float)stepsAccel / currentMachineStepsPerMm));
            // для чего пересчитывать эту скорость?
        } else {
            stepsCruise = maxSegmentSteps - (stepsAccel + stepsDecel);
        }

        // Локальные счетчики распределения шагов Брезенхема
        long sX = 0, sY = 0, sZ = 0;
        long errX = maxSegmentSteps / 2;
        long errY = maxSegmentSteps / 2;
        long errZ = maxSegmentSteps / 2;

        // УПРАВЛЕНИЕ СКОРОСТЬЮ ТАКТОВ (ГЕНЕРАЦИЯ ОЧЕРЕДИ БУФЕРА)
        float currentVelocity = v_start;

        for (long step = 0; step < maxSegmentSteps; step++) {
            
            // Динамический пересчет текущей скорости на каждом шаге трапеции
            if (step < stepsAccel) {
                // Участок разгона: скорость растет линейно относительно пройденных шагов
                currentVelocity = sqrt(v_start * v_start + 2.0f * accel * ((float)step / currentMachineStepsPerMm));
            } 
            else if (step >= (stepsAccel + stepsCruise)) {
                // Участок торможения: скорость плавно падает до скорости стыка v_end
                long decelStep = step - (stepsAccel + stepsCruise);
                currentVelocity = sqrt(v_max * v_max - 2.0f * accel * ((float)decelStep / currentMachineStepsPerMm));
            } 
            else {
                // Маршевый участок
                currentVelocity = v_max;
            }

            if (currentVelocity < v_min) currentVelocity = v_min;
            
            // Применяем динамический Feedrate Override от ползунка PyQt6
            float finalVelocity = currentVelocity * cfg.feedMultiplier;
            if (finalVelocity < 0.1f) finalVelocity = 0.1f;//зачем это ограничение?

            // КЛЮЧЕВАЯ ФОРМУЛА: Переводим текущую скорость в количество тиков задержки таймера ЧПУ!
            // delayTicks = Сколько тиков по 10 мкс нужно выждать между импульсами шага
            float stepsPerSec = finalVelocity * currentMachineStepsPerMm;
            uint16_t requiredDelayTicks = (uint16_t)(TIMER_FREQ / stepsPerSec);
            if (requiredDelayTicks < 1) requiredDelayTicks = 1; // Защита от перегрузки

            // ЛИНЕЙНАЯ ЦЕЛОЧИСЛЕННАЯ ИНТЕРПОЛЯЦИЯ БРЕЗЕНХЕМА (Без float!)
            StepCmd cmd = {0, 0, 0, 0, 0, 0, 0, requiredDelayTicks};

            // Ось X
            errX -= absDeltaX;
            if (errX < 0) {
                errX += maxSegmentSteps;
                cmd.stepX = 1;
                cmd.dirX = (totalDeltaX > 0) ? 1 : 0;
                sX += (totalDeltaX > 0) ? 1 : -1;
            }

            // Ось Y
            errY -= absDeltaY;
            if (errY < 0) {
                errY += maxSegmentSteps;
                cmd.stepY = 1;
                cmd.dirY = (totalDeltaY > 0) ? 1 : 0;
                sY += (totalDeltaY > 0) ? 1 : -1;
            }

            // Ось Z
            errZ -= absDeltaZ;
            if (errZ < 0) {
                errZ += maxSegmentSteps;
                cmd.stepZ = 1;
                cmd.dirZ = (totalDeltaZ > 0) ? 1 : 0;
                sZ += (totalDeltaZ > 0) ? 1 : -1;
            } //и как разные оси с разными скоростяи и ускорениями в один момент будут вести себя в этом случае?

            // Укладываем готовый шаг в кольцевой буфер
            uint32_t nextHead = (bufferHead + 1) % STEP_BUFFER_SIZE;
            // Если буфер полон — легально уступаем квант времени операционной системе
            while (nextHead == bufferTail) { vTaskDelay(pdMS_TO_TICKS(1)); }

            cmd.valid = 1;
            stepRingBuffer[bufferHead] = cmd;
            bufferHead = nextHead; // Сдвигаем указатель записи для таймера
        }

        // Запоминаем финишную скорость для бесшовного стыка со следующим кадром Look-Ahead
        lastFrameEndSpeedX = v_end;

        // ЖЕСТКАЯ МАТЕМАТИЧЕСКАЯ ФИКСАЦИЯ БАЗЫ (Расхождения координат НЕВОЗМОЖНЫ ФИЗИЧЕСКИ!)
        plannerStepsX += totalDeltaX;
        plannerStepsY += totalDeltaY;
        plannerStepsZ += totalDeltaZ;// это зачем если целочисленная математика не может ошибаться?

        vTaskDelay(pdMS_TO_TICKS(1));
    }
}