https://pastein.ru/t/FX0
Загрузка данных
void motionTask(void * parameter) {
// Внутренняя фиксированная частота работы нашего двухфазного таймера (200 кГц)
const float TIMER_FREQ = 200000.0f;
// Локальные переменные потока для отслеживания шагов внутри кадра
long localStepsX = 0;
long localStepsY = 0;
long localStepsZ = 0;
while (true) {
// --- 4.1.1. ОПРОС И БЛОКИРОВКА МЕЖЪЯДЕРНОЙ ОЧЕРЕДИ ---
if (!nextSeg.hasNewData) {
// Если в кольцевом буфере физически кончились шаги, сигнализируем Ядру 0 в main.cpp
if (bufferTail == bufferHead) {
isPlannerBusy = false;
}
vTaskDelay(pdMS_TO_TICKS(1)); // Неблокирующее ожидание FreeRTOS (1 мс)
continue;
}
// --- 4.1.2. ИЗВЛЕЧЕНИЕ ПАРАМЕТРОВ КАДРА И СКОРОСТЕЙ (в мм/сек) ---
float nX = nextSeg.x;
float nY = nextSeg.y;
float nZ = nextSeg.z;
float accel = nextSeg.accel;
float v_max = nextSeg.v_frame; // Ограничение подачи кадра от CAM
float v_start = nextSeg.v_start; // Скорость входа от Look-Ahead Python
float v_end = nextSeg.v_end; // Скорость выхода на стыке угла от Look-Ahead Python
nextSeg.hasNewData = false; // Мгновенный Handshake: освобождаем ячейку для Ядра 0
// --- 4.1.3. ОПТИЧЕСКАЯ ЛАЗЕРНАЯ КОМПЕНСАЦИЯ ПРОВИСА ФЕРМЫ ---
CorrectionPoint corr = getCorrection(nX);
float realMachineY = nY + corr.y_offset;
float realMachineZ = nZ + corr.z_offset;
// --- 4.1.4. ПЕРЕВОД МИЛЛИМЕТРОВ В ЦЕЛОЧИСЛЕННЫЕ НАПРАВЛЕНИЯ ЧПУ ---
// Рассчитываем точные целевые шаги, которые требует CAM-модель
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;
}
// ====================================================================
// СТРУКТУРНЫЙ БЛОК: MULTI-SEGMENT VECTOR SPLITTING & VALIDATION (Core 1)
// ====================================================================
// Идеальные длины участков в мм
float s_accel_ideal = (v_max * v_max - v_start * v_start) / (2.0f * accel);
float s_decel_ideal = (v_max * v_max - v_end * v_end) / (2.0f * accel);
float dx_mm = (float)totalDeltaX / cfg.stepsPerMmX;
float dy_mm = (float)totalDeltaY / cfg.stepsPerMmY;
float dz_mm = (float)totalDeltaZ / cfg.stepsPerMmZ;
float totalLength_mm = sqrt(dx_mm * dx_mm + dy_mm * dy_mm + dz_mm * dz_mm);
// Объявляем массивы целочисленных дельт для каждого из трех сегментов
long dX[3] = {0, 0, 0}; // [0] - разгон, [1] - марш, [2] - торможение
long dY[3] = {0, 0, 0};
long dZ[3] = {0, 0, 0};
// Процентные доли сегментов в общем векторе
float pct_accel = 0.0f;
float pct_cruise = 0.0f;
if (s_accel_ideal + s_decel_ideal > totalLength_mm) {
// --- СЦЕНАРИЙ А: КОРОТКИЙ ОТРЕЗК (ТРЕУГОЛЬНИК, 2 сегмента) ---
float v_peak = sqrt((2.0f * accel * totalLength_mm + v_start * v_start + v_end * v_end) / 2.0f);
v_max = v_peak;
float s_accel_actual = (v_peak * v_peak - v_start * v_start) / (2.0f * accel);
pct_accel = s_accel_actual / totalLength_mm;
pct_cruise = 0.0f; // Марша нет
// Целочисленные дельты разгона
dX[0] = (long)(totalDeltaX * pct_accel);
dY[0] = (long)(totalDeltaY * pct_accel);
dZ[0] = (long)(totalDeltaZ * pct_accel);
// Марш равен нулю
dX[1] = 0; dY[1] = 0; dZ[1] = 0;
// Торможение — это строго остаток от исходного вектора
dX[2] = totalDeltaX - dX[0];
dY[2] = totalDeltaY - dY[0];
dZ[2] = totalDeltaZ - dZ[0];
}
else {
// --- СЦЕНАРИЙ Б: КЛАССИЧЕСКАЯ ТРАПЕЦИЯ (3 сегмента) ---
pct_accel = s_accel_ideal / totalLength_mm;
float s_cruise_ideal = totalLength_mm - (s_accel_ideal + s_decel_ideal);
pct_cruise = s_cruise_ideal / totalLength_mm;
// Дельты разгона
dX[0] = (long)(totalDeltaX * pct_accel);
dY[0] = (long)(totalDeltaY * pct_accel);
dZ[0] = (long)(totalDeltaZ * pct_accel);
// Дельты маршевого круиза
dX[1] = (long)(totalDeltaX * pct_cruise);
dY[1] = (long)(totalDeltaY * pct_cruise);
dZ[1] = (long)(totalDeltaZ * pct_cruise);
// Дельты торможения — строго вычитанием из общего баланса, поглощая округления float
dX[2] = totalDeltaX - dX[0] - dX[1];
dY[2] = totalDeltaY - dY[0] - dY[1];
dZ[2] = totalDeltaZ - dZ[0] - dZ[1];
}
// --- 4.2.3. ЖЕСТКАЯ ЦЕЛОЧИСЛЕННАЯ ВАЛИДАЦИЯ БАЛАНСА ШАГОВ ---
// Проверяем, что сумма всех дельт сегментов математически тождественна общему вектору
// (Это защита на уровне компилятора. Если баланс сошелся — мы гарантированно не пропустим микрошаг)
long checkSumX = dX[0] + dX[1] + dX[2];
long checkSumY = dY[0] + dY[1] + dY[2];
long checkSumZ = dZ[0] + dZ[1] + dZ[2];
if (checkSumX != totalDeltaX || checkSumY != totalDeltaY || checkSumZ != totalDeltaZ) {
// Если из-за краевых эффектов округления где-то потерялся 1 микрошаг,
// принудительно корректируем его в сегменте торможения [2]
if (checkSumX != totalDeltaX) dX[2] += (totalDeltaX - checkSumX);
if (checkSumY != totalDeltaY) dY[2] += (totalDeltaY - checkSumY);
if (checkSumZ != totalDeltaZ) dZ[2] += (totalDeltaZ - checkSumZ);
}
// ====================================================================
// ТЕПЕРЬ У НАС ЕСТЬ ЖЕСТКИЕ ЦЕЛОЧИСЛЕННЫЕ ЗАДАЧИ ДЛЯ БРЕЗЕНХЕМА:
// Сегмент 0 (Разгон): сделать шаги dX[0], dY[0], dZ[0]
// Сегмент 1 (Марш): сделать шаги dX[1], dY[1], dZ[1]
// Сегмент 2 (Торможение): сделать шаги dX[2], dY[2], dZ[2]
// СУММА ДЕЛЬТ СЕГМЕНТОВ ВСЕГДА СТРОГО РАВНА ИСХОДНОМУ ВЕКТОРУ!
// ====================================================================
// ====================================================================
// СТРУКТУРНЫЙ БЛОК: ЧИСТАЯ ПОТАКТОВАЯ ОТРАБОТКА ПОДВЕКТОРОВ (Core 1)
// ====================================================================
// Базовая ЧПУ-сетка частоты планирования — 100 кГц (1 такт = 10 микросекунд)
const float BASE_CNC_FREQ = 100000.0f;
// Текущая физическая скорость вектора в начале кадра (мм/сек)
float currentVelocity = v_start;
localStepsX = plannerStepsX;
localStepsY = plannerStepsY;
localStepsZ = plannerStepsZ;
// Поочередно прогоняем 3 рассчитанных подвектора (Разгон, Марш, Торможение)
for (uint8_t segIdx = 0; segIdx < 3; segIdx++) {
long segDeltaX = dX[segIdx];
long segDeltaY = dY[segIdx];
long segDeltaZ = dZ[segIdx];
long segAbsX = labs(segDeltaX);
long segAbsY = labs(segDeltaY);
long segAbsZ = labs(segDeltaZ);
// Определяем, какая физическая ось является Мастер-Осью для текущего подвектора
long totalSegSteps = segAbsX;
float masterStepsPerMm = cfg.stepsPerMmX; // Цена шага Мастер-Оси
if (segAbsY > totalSegSteps) {
totalSegSteps = segAbsY;
masterStepsPerMm = cfg.stepsPerMmY;
}
if (segAbsZ > totalSegSteps) {
totalSegSteps = segAbsZ;
masterStepsPerMm = cfg.stepsPerMmZ;
}
// Если сегмент пустой (например, марш в треугольнике), просто идем дальше
if (totalSegSteps == 0) continue;
// Инициализация накопителей ошибки Брезенхема для этого подвектора
long errX = totalSegSteps / 2;
long errY = totalSegSteps / 2;
long errZ = totalSegSteps / 2;
uint8_t dirBitX = (segDeltaX >= 0) ? 1 : 0;
uint8_t dirBitY = (segDeltaY >= 0) ? 1 : 0;
uint8_t dirBitZ = (segDeltaZ >= 0) ? 1 : 0;
// --- ХОДОВОЙ ЦИКЛ ГЕНЕРАЦИИ ТАКТОВ ПОДВЕКТОРА ---
// Цикл итерируется строго по физическому количеству импульсов, которые нужно выдать
for (long step = 0; step < totalSegSteps; step++) {
// Вычисляем истинное количество шагов Master-оси на 1 мм пространственного пути.
// totalDistanceMm — полная длина 3D вектора в мм (уже посчитана при приеме кадра).
// totalSegSteps — полное количество шагов Master-оси в текущем сегменте/кадре.
float effectiveMasterStepsPerMm = (float)totalSegSteps / totalLength_mm;
// Защита от околонулевых значений (если кадр пустой)
if (effectiveMasterStepsPerMm < 0.001f) effectiveMasterStepsPerMm = 1.0f;
// Считаем частоту Master-оси на основе её реального вклада в пространственную траекторию:
float masterFrequency = currentVelocity * effectiveMasterStepsPerMm;
// Защита от деления на ноль: частота не может быть ниже 1 Гц (1 шаг в секунду)
if (masterFrequency < 1.0f) masterFrequency = 1.0f;
// 2. ПЕРЕВОД ФИЗИЧЕСКОЙ ЧАСТОТЫ В ДИСКРЕТНЫЕ КВАНТЫ ЧПУ-ЛЕНТЫ
// Делим базовую частоту планирования сетки (BASE_CNC_FREQ = 100 000 Гц) на частоту шагов,
// дополнительно умноженную на глобальный множитель подачи pythonFeedrateOverride.
// Пример при 100% подаче (1.0f): 100 000 / (10 000 * 1.0) = 10 тактов паузы на этот шаг.
uint16_t delay100kHz = (uint16_t)(BASE_CNC_FREQ / (masterFrequency * pythonFeedrateOverride));
// Аппаратная защита таймера: задержка не может быть меньше 1 такта сетки (10 микросекунд).
// Это ограничивает максимальную частоту импульсов на отметке 100 кГц, предотвращая зависание ISR.
if (delay100kHz < 1) delay100kHz = 1;
// 3. РАСЧЕТ РЕАЛЬНОГО ФИЗИЧЕСКОГО ВРЕМЕНИ ДЛИТЕЛЬНОСТИ ТЕКУЩЕГО ШАГА
// Переводим полученные такты обратно во float-секунды для физических формул разгона/торможения.
// Пример: 10 тактов / 100 000 Гц = 0.0001 секунды (100 микросекунд) длится этот конкретный шаг.
float stepDurationSec = (float)delay100kHz / BASE_CNC_FREQ;
// ====================================================================
// ДИНАМИЧЕСКИЙ ПЕРЕСЧЕТ СКОРОСТИ И АДАПТИВНЫЙ БАРЬЕР БЕЗОПАСНОСТИ
// ====================================================================
// --- СЦЕНАРИЙ А: СЕГМЕНТ РАЗГОНА (segIdx == 0) ---
if (segIdx == 0) {
// Вычисляем теоретический прирост скорости за время текущего шага по формуле: V = a * t
// Пример: 500 мм/сек2 (ускорение) * 0.0001 сек (время шага) = 0.05 мм/сек прироста
float deltaV = accel * stepDurationSec;
// РАСЧЕТ ПРОГРАММНОГО БАРЬЕРА БЕЗОПАСНОСТИ (Anti-Jerk Guard):
// Прирост скорости за ОДИН дискретный шаг не имеет права превысить половину
// от всей разницы между скоростью входа в кадр и маршевой (круизной) скоростью.
// Это защищает тяжелый портал от ударного срыва роторов на сверхвысоких частотах таймера.
float maxSafeDeltaV = (v_max - v_start) / 2.0f;
// Нижний барьер-предохранитель, чтобы рампа не застряла на околонулевых скоростях
if (maxSafeDeltaV < 0.1f) maxSafeDeltaV = 0.1f;
// Если математический расчет выдал слишком агрессивный прыжок скорости — зажимаем его по лимиту
if (deltaV > maxSafeDeltaV) deltaV = maxSafeDeltaV;
// Физически наращиваем текущую скорость вектора на безопасную величину дельты
currentVelocity += deltaV;
// Срезаем скорость по верхней границе маршевой подачи кадра (ограничение G-кода / CAM)
if (currentVelocity > v_max) currentVelocity = v_max;
}
// --- СЦЕНАРИЙ Б: СЕГМЕНТ ТОРМОЖЕНИЯ (segIdx == 2) ---
else if (segIdx == 2) {
// Вычисляем теоретическое падение скорости за время шага: V = a * t
float deltaV = accel * stepDurationSec;
// Симметричный барьер безопасности для торможения перед стыком со следующим кадром.
// Защищает от мгновенного удара и «втыкания» в угол при переходе на новую траекторию.
float maxSafeDeltaV = (v_max - v_end) / 2.0f;
if (maxSafeDeltaV < 0.1f) maxSafeDeltaV = 0.1f;
if (deltaV > maxSafeDeltaV) deltaV = maxSafeDeltaV;
// Физически гасим текущую скорость вектора
currentVelocity -= deltaV;
// Срезаем скорость по нижней границе, заданной Python Look-Ahead планировщиком (скорость выхода)
if (currentVelocity < v_end) currentVelocity = v_end;
}
// --- СЦЕНАРИЙ В: СЕГМЕНТ МАРШЕВОГО КРУИЗА (segIdx == 1) ---
else {
// На маршевом участке скорость станка строго стабильна и равна максимальной подаче кадра
currentVelocity = v_max;
}
// ====================================================================
// БЛОК 3: ИЗОЛИРОВАННЫЙ ЦЕЛОЧИСЛЕННЫЙ АЛГОРИТМ БРЕЗЕНХЕМА (Core 1)
// ====================================================================
// Объявляем локальные битовые флаги-триггеры для этого конкретного шага.
// 1 = в этот такт нужно физически выдать импульс STEP, 0 = ось стоит на месте.
uint8_t issueX = 0;
uint8_t issueY = 0;
uint8_t issueZ = 0;
// --- ЛИНЕЙНАЯ ИНТЕРПОЛЯЦИЯ ОСИ X ---
// Вычитаем из накопителя ошибки Брезенхема (errX) модуль перемещения текущей оси (segAbsX)
errX -= segAbsX;
// Если ошибка упала ниже нуля — пороговое значение пересечено, пора делать физический шаг!
if (errX < 0) {
errX += totalSegSteps; // Возвращаем накопитель ошибки в рабочую зону ведущей оси
issueX = 1; // Взводим флаг генерации импульса STEP для прерывания
// Обновляем виртуальную координату планировщика в ОЗУ в зависимости от знака направления
localStepsX += (dirBitX == 1) ? 1 : -1;
}
// --- ЛИНЕЙНАЯ ИНТЕРПОЛЯЦИЯ ОСИ Y ---
// Математика полностью идентична оси X, но выполняется параллельно на независимых регистрах
errY -= segAbsY;
if (errY < 0) {
errY += totalSegSteps;
issueY = 1;
localStepsY += (dirBitY == 1) ? 1 : -1;
}
// --- ЛИНЕЙНАЯ ИНТЕРПОЛЯЦИЯ ОСИ Z ---
errZ -= segAbsZ;
if (errZ < 0) {
errZ += totalSegSteps;
issueZ = 1;
localStepsZ += (dirBitZ == 1) ? 1 : -1;
}
// ====================================================================
// БЛОК 4: ЗАПОЛНЕНИЕ БУФЕРА СЕТКИ ВРЕМЕНИ (Внутренний цикл паузы)
// ====================================================================
// Цикл крутится ровно delay100kHz раз, нарезая 10-микросекундные кванты времени.
// Если delay100kHz равен 10, то мы запишем 1 такт с импульсом и 9 тактов чистой паузы.
for (uint16_t t = 0; t < delay100kHz; t++) {
// 1. КОНТРОЛЬ ПЕРЕПОЛНЕНИЯ И ЗАЩИТА ОЧЕРЕДИ FREE ROOT
// Вычисляем виртуальный следующий индекс для головы кольцевого буфера
uint32_t nextHead = (bufferHead + 1) % STEP_BUFFER_SIZE;
// Если голова догнала хвост (буфер ОЗУ забит на все 40.96 мс вперед)
while (nextHead == bufferTail) {
// Уступаем квант времени операционной системе FreeRTOS на 1 мс.
// Прерывание таймера на Core 1 продолжит читать буфер и освободит место.
vTaskDelay(pdMS_TO_TICKS(1));
}
// 2. ИНИЦИАЛИЗАЦИЯ ЧИСТОЙ БИНАРНОЙ КОМАНДЫ ВРЕМЕНИ
// Создаем пустую микроструктуру (по умолчанию все шаги равны 0, valid = 0)
StepCmd cmd = { 0, 0, 0, 0, 0, 0, 0 };
// 3. ПОТАКТОВОЕ ФОРМИРОВАНИЕ ПЕРЕДНЕГО ФРОНТА (Только в первый квант)
// Физический импульс шага и биты направлений закладываются СТРОГО в самый первый такт (t == 0)
if (t == 0) {
// Подмешиваем флаги и направления, которые рассчитал Брезенхем в Блоке 3
if (issueX) { cmd.stepX = 1; cmd.dirX = dirBitX; }
if (issueY) { cmd.stepY = 1; cmd.dirY = dirBitY; }
if (issueZ) { cmd.stepZ = 1; cmd.dirZ = dirBitZ; }
}
// Во всех остальных итерациях (при t > 0) поля cmd.stepX/Y/Z остаются равными 0.
// Это и формирует необходимую временную паузу между шагами моторов!
// Взводим флаг готовности ячейки. Аппаратный таймер теперь имеет право её прочесть.
cmd.valid = 1;
// Физически записываем структуру в оперативную память кольцевого буфера
stepRingBuffer[bufferHead] = cmd;
// Сдвигаем индекс головы кольца вперед
bufferHead = nextHead;
}
} // Конец цикла по шагам текущего подвектора
} // Конец цикла по 3-м подвекторам
isVectorMoving = true; // Жестко взводим флаг физического старта движения!
// Фиксируем эталонную базу
plannerStepsX = targetStepsX;
plannerStepsY = targetStepsY;
plannerStepsZ = targetStepsZ;
vTaskDelay(pdMS_TO_TICKS(1));
}
}
