Загрузка данных
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));
}
}