0xFF

#ifndef MOTION_CONTROLLER_H
#define MOTION_CONTROLLER_H

#include <stdint.h>
#include <stdbool.h>

// Структура для хранения параметров движения
typedef struct {
    int32_t position;        // текущая позиция в шагах
    int32_t velocity;        // текущая скорость (шагов/сек)
    int32_t acceleration;    // текущее ускорение (шагов/сек^2)
    int32_t jerk;            // заданный рывок (шагов/сек^3)
    
    int32_t target_position; // целевая позиция
    int32_t max_velocity;    // максимальная скорость
    int32_t max_acceleration; // максимальное ускорение
    
    int32_t min_velocity;    // минимальная скорость (обычно 0)
} MotionParams;

// Структура для хранения временных точек профиля
typedef struct {
    uint32_t t1;  // время достижения макс. ускорения
    uint32_t t2;  // время начала снижения ускорения
    uint32_t t3;  // время достижения постоянной скорости
    uint32_t t4;  // время начала торможения
    uint32_t t5;  // время достижения макс. замедления
    uint32_t t6;  // время начала снижения замедления
    uint32_t t7;  // время остановки
    
    int32_t v1;   // скорость на момент t1
    int32_t v2;   // скорость на момент t2
    int32_t v3;   // скорость на момент t3 (мax_velocity)
    int32_t v5;   // скорость на момент t5
    int32_t v6;   // скорость на момент t6
    
    int32_t x1;   // положение на момент t1
    int32_t x2;   // положение на момент t2
    int32_t x3;   // положение на момент t3
    int32_t x4;   // положение на момент t4
    int32_t x5;   // положение на момент t5
    int32_t x6;   // положение на момент t6
    int32_t x7;   // положение на момент t7
    
    uint8_t profile_type;  // тип профиля движения
} MotionProfile;

// Константы для типов профиля
#define PROFILE_SHORT             0  // Короткий профиль (ни A, ни V не достигаются)
#define PROFILE_NO_MAX_VELOCITY   1  // Профиль без достижения максимальной скорости
#define PROFILE_NO_CONST_VELOCITY 2  // Профиль без постоянной скорости
#define PROFILE_FULL              3  // Полный профиль со всеми фазами

// Функция инициализации параметров движения
void initMotionParams(MotionParams *params, int32_t jerk, int32_t max_acceleration, 
                     int32_t max_velocity, int32_t min_velocity);

// Функция планирования профиля движения
bool planMotion(MotionParams *params, MotionProfile *profile, int32_t target_position);

// Функция вычисления параметров движения в момент времени t
void calculateMotionAt(const MotionProfile *profile, uint32_t t, 
                     int32_t *position, int32_t *velocity, int32_t *acceleration);

#endif /* MOTION_CONTROLLER_H */

#include "motion_controller.h"
#include <stdlib.h>  // для abs()

// Вспомогательная функция для вычисления квадратного корня (целочисленная версия)
// Используется алгоритм Ньютона
static uint32_t isqrt(uint32_t x) {
    if (x == 0) return 0;
    
    uint32_t res = x;
    uint32_t one = 1UL << 30;  // Второй старший бит
    
    // Ищем начальную точку
    while (one > x) one >>= 2;
    
    while (one != 0) {
        if (res >= one + x / one) 
            res = (res + x / res) / 2;
        one >>= 1;
    }
    
    return res;
}

// Вспомогательная функция для фиксированной точки (Q16.16)
typedef int32_t fixed_t;
#define FIXED_SHIFT 16
#define FIXED(x) ((fixed_t)((x) * (1 << FIXED_SHIFT)))
#define INT_FROM_FIXED(x) ((x) >> FIXED_SHIFT)
#define FIXED_MUL(a, b) (((int64_t)(a) * (b)) >> FIXED_SHIFT)
#define FIXED_DIV(a, b) (((int64_t)(a) << FIXED_SHIFT) / (b))

// Инициализация параметров движения
void initMotionParams(MotionParams *params, int32_t jerk, int32_t max_acceleration, 
                     int32_t max_velocity, int32_t min_velocity) {
    if (params == NULL) return;
    
    params->position = 0;
    params->velocity = 0;
    params->acceleration = 0;
    params->jerk = jerk;
    params->max_acceleration = max_acceleration;
    params->max_velocity = max_velocity;
    params->min_velocity = min_velocity;
    params->target_position = 0;
}

// Функция определения типа профиля движения
static uint8_t determineProfileType(int32_t distance, int32_t max_velocity, 
                                  int32_t max_acceleration, int32_t jerk) {
    uint32_t abs_distance = (uint32_t)abs(distance);
    
    // Преобразуем значения в fixed point для промежуточных вычислений
    fixed_t j_fixed = FIXED(jerk);
    fixed_t a_fixed = FIXED(max_acceleration);
    fixed_t v_fixed = FIXED(max_velocity);
    
    // Расстояние для достижения max_acceleration и снижения до 0
    fixed_t x_min_accel = FIXED_MUL(FIXED_MUL(FIXED_DIV(a_fixed, j_fixed), 
                                  FIXED_DIV(a_fixed, j_fixed)), a_fixed);
    
    // Расстояние для достижения max_velocity без поддержания max_acceleration
    fixed_t x_min_speed = FIXED_MUL(FIXED_MUL(v_fixed, v_fixed), 
                                 FIXED_DIV(j_fixed, FIXED_MUL(a_fixed, a_fixed)));
    
    // Расстояние для достижения max_velocity с поддержанием max_acceleration
    fixed_t d_accel = FIXED_DIV(FIXED_MUL(a_fixed, a_fixed), j_fixed);
    fixed_t v_at_accel = FIXED_DIV(FIXED_MUL(a_fixed, a_fixed), FIXED(2) * j_fixed);
    fixed_t x_full_speed = FIXED_MUL(v_at_accel, d_accel) + 
                          FIXED_MUL(v_fixed, FIXED_DIV((v_fixed - v_at_accel), a_fixed));
    
    if (abs_distance < INT_FROM_FIXED(x_min_accel))
        return PROFILE_SHORT;
    else if (abs_distance < INT_FROM_FIXED(x_min_speed))
        return PROFILE_NO_MAX_VELOCITY;
    else if (abs_distance < INT_FROM_FIXED(x_full_speed))
        return PROFILE_NO_CONST_VELOCITY;
    else
        return PROFILE_FULL;
}

// Расчет короткого профиля (ни макс. ускорение, ни макс. скорость не достигаются)
static bool calculateShortProfile(MotionProfile *profile, int32_t distance, int32_t jerk) {
    if (distance == 0) return false;
    
    int32_t dir = (distance > 0) ? 1 : -1;
    uint32_t abs_distance = (uint32_t)abs(distance);
    
    // Расчет времени до пиковой скорости (t_peak)
    fixed_t t_peak_squared = FIXED_DIV(FIXED(6) * FIXED(abs_distance), FIXED(jerk));
    uint32_t t_peak = isqrt(INT_FROM_FIXED(t_peak_squared));
    
    profile->t1 = t_peak / 2;
    profile->t2 = profile->t1;  // В коротком профиле нет фазы постоянного ускорения
    profile->t3 = t_peak;
    profile->t4 = profile->t3;  // В коротком профиле нет фазы постоянной скорости
    profile->t5 = profile->t4 + profile->t1;
    profile->t6 = profile->t5;  // В коротком профиле нет фазы постоянного замедления
    profile->t7 = 2 * t_peak;
    
    // Вычисляем скорости и позиции в ключевых точках
    int32_t j = jerk * dir;
    
    profile->v1 = (j * (int64_t)profile->t1 * profile->t1) / 2;
    profile->v3 = (j * (int64_t)t_peak * t_peak) / 4;  // Максимальная скорость
    
    profile->x1 = (j * (int64_t)profile->t1 * profile->t1 * profile->t1) / 6;
    profile->x3 = distance / 2;
    profile->x7 = distance;
    
    profile->profile_type = PROFILE_SHORT;
    return true;
}

// Расчет профиля без достижения макс. скорости (достигается только макс. ускорение)
static bool calculateNoMaxVelocityProfile(MotionProfile *profile, int32_t distance, 
                                        int32_t max_acceleration, int32_t jerk) {
    if (distance == 0) return false;
    
    int32_t dir = (distance > 0) ? 1 : -1;
    uint32_t abs_distance = (uint32_t)abs(distance);
    int32_t a = max_acceleration * dir;
    int32_t j = jerk * dir;
    
    // Время достижения макс. ускорения
    profile->t1 = max_acceleration / jerk;
    
    // Скорость и позиция в момент t1
    profile->v1 = (j * (int64_t)profile->t1 * profile->t1) / 2;
    profile->x1 = (j * (int64_t)profile->t1 * profile->t1 * profile->t1) / 6;
    
    // Находим t2 из условий симметричности профиля
    // Решаем уравнение: x7 = 2*(x1 + x2-x1 + x3-x2) = distance
    int64_t dt_squared = ((int64_t)profile->v1 * profile->v1 + 
                         2 * a * (abs_distance - 2 * profile->x1));
    int32_t dt = isqrt(dt_squared) - profile->v1;
    dt = dt / max_acceleration;
    
    profile->t2 = profile->t1 + dt;
    profile->v2 = profile->v1 + a * dt;
    profile->x2 = profile->x1 + (int64_t)profile->v1 * dt + (a * (int64_t)dt * dt) / 2;
    
    // Фаза 3: Снижение ускорения до нуля
    profile->t3 = profile->t2 + profile->t1;
    profile->v3 = profile->v2 + a * profile->t1 - (j * (int64_t)profile->t1 * profile->t1) / 2;
    profile->x3 = profile->x2 + (int64_t)profile->v2 * profile->t1 + 
                 (a * (int64_t)profile->t1 * profile->t1) / 2 - 
                 (j * (int64_t)profile->t1 * profile->t1 * profile->t1) / 6;
    
    // Фазы 4-7 симметричны фазам 3-1
    profile->t4 = profile->t3;  // Нет фазы постоянной скорости
    profile->x4 = profile->x3;
    
    profile->t5 = profile->t4 + profile->t1;
    profile->v5 = profile->v3 - (j * (int64_t)profile->t1 * profile->t1) / 2;
    profile->x5 = profile->x4 + (int64_t)profile->v3 * profile->t1 - 
                 (j * (int64_t)profile->t1 * profile->t1 * profile->t1) / 6;
    
    profile->t6 = profile->t5 + dt;
    profile->v6 = profile->v5 - a * dt;
    profile->x6 = profile->x5 + (int64_t)profile->v5 * dt - (a * (int64_t)dt * dt) / 2;
    
    profile->t7 = profile->t6 + profile->t1;
    profile->x7 = distance;
    
    profile->profile_type = PROFILE_NO_MAX_VELOCITY;
    return true;
}

// Расчет профиля без фазы постоянной скорости
static bool calculateNoConstVelocityProfile(MotionProfile *profile, int32_t distance, 
                                          int32_t max_velocity, int32_t max_acceleration, 
                                          int32_t jerk) {
    if (distance == 0) return false;
    
    int32_t dir = (distance > 0) ? 1 : -1;
    uint32_t abs_distance = (uint32_t)abs(distance);
    int32_t v = max_velocity * dir;
    int32_t a = max_acceleration * dir;
    int32_t j = jerk * dir;
    
    // Время достижения макс. ускорения
    profile->t1 = max_acceleration / jerk;
    
    // Скорость и позиция в момент t1
    profile->v1 = (j * (int64_t)profile->t1 * profile->t1) / 2;
    profile->x1 = (j * (int64_t)profile->t1 * profile->t1 * profile->t1) / 6;
    
    // Время движения с постоянным ускорением
    int32_t dt = (max_velocity - profile->v1) / max_acceleration;
    profile->t2 = profile->t1 + dt;
    profile->v2 = profile->v1 + a * dt;
    profile->x2 = profile->x1 + (int64_t)profile->v1 * dt + (a * (int64_t)dt * dt) / 2;
    
    // Время снижения ускорения до нуля
    profile->t3 = profile->t2 + profile->t1;
    profile->v3 = v;  // Максимальная скорость
    profile->x3 = profile->x2 + (int64_t)profile->v2 * profile->t1 + 
                 (a * (int64_t)profile->t1 * profile->t1) / 2 - 
                 (j * (int64_t)profile->t1 * profile->t1 * profile->t1) / 6;
    
    // Фаза 4: Отсутствует движение с постоянной скоростью
    profile->t4 = profile->t3;
    profile->x4 = profile->x3;
    
    // Оставшиеся фазы симметричны начальным
    profile->t5 = profile->t4 + profile->t1;
    profile->v5 = profile->v3 - (j * (int64_t)profile->t1 * profile->t1) / 2;
    profile->x5 = profile->x4 + (int64_t)profile->v3 * profile->t1 - 
                 (j * (int64_t)profile->t1 * profile->t1 * profile->t1) / 6;
    
    profile->t6 = profile->t5 + dt;
    profile->v6 = profile->v5 - a * dt;
    profile->x6 = profile->x5 + (int64_t)profile->v5 * dt - (a * (int64_t)dt * dt) / 2;
    
    profile->t7 = profile->t6 + profile->t1;
    profile->x7 = distance;
    
    profile->profile_type = PROFILE_NO_CONST_VELOCITY;
    return true;
}

// Расчет полного профиля движения со всеми фазами
static bool calculateFullProfile(MotionProfile *profile, int32_t distance, 
                               int32_t max_velocity, int32_t max_acceleration, 
                               int32_t jerk) {
    if (distance == 0) return false;
    
    int32_t dir = (distance > 0) ? 1 : -1;
    uint32_t abs_distance = (uint32_t)abs(distance);
    int32_t v = max_velocity * dir;
    int32_t a = max_acceleration * dir;
    int32_t j = jerk * dir;
    
    // Фаза 1: Увеличение ускорения до max_acceleration
    profile->t1 = max_acceleration / jerk;
    profile->v1 = (j * (int64_t)profile->t1 * profile->t1) / 2;
    profile->x1 = (j * (int64_t)profile->t1 * profile->t1 * profile->t1) / 6;
    
    // Фаза 2: Движение с постоянным ускорением
    int32_t dt = (max_velocity - profile->v1) / max_acceleration;
    profile->t2 = profile->t1 + dt;
    profile->v2 = profile->v1 + a * dt;
    profile->x2 = profile->x1 + (int64_t)profile->v1 * dt + (a * (int64_t)dt * dt) / 2;
    
    // Фаза 3: Снижение ускорения до нуля
    profile->t3 = profile->t2 + profile->t1;
    profile->v3 = v;  // Максимальная скорость
    profile->x3 = profile->x2 + (int64_t)profile->v2 * profile->t1 + 
                 (a * (int64_t)profile->t1 * profile->t1) / 2 - 
                 (j * (int64_t)profile->t1 * profile->t1 * profile->t1) / 6;
    
    // Фаза 4: Движение с постоянной скоростью
    // Вычисляем время движения с постоянной скоростью
    int32_t remaining_distance = abs_distance - 2 * profile->x3;
    int32_t const_velocity_time = remaining_distance / max_velocity;
    
    profile->t4 = profile->t3 + const_velocity_time;
    profile->x4 = profile->x3 + v * const_velocity_time;
    
    // Фазы 5-7 симметричны 3-1
    profile->t5 = profile->t4 + profile->t1;
    profile->v5 = profile->v3 - (j * (int64_t)profile->t1 * profile->t1) / 2;
    profile->x5 = profile->x4 + (int64_t)profile->v3 * profile->t1 - 
                 (j * (int64_t)profile->t1 * profile->t1 * profile->t1) / 6;
    
    profile->t6 = profile->t5 + dt;
    profile->v6 = profile->v5 - a * dt;
    profile->x6 = profile->x5 + (int64_t)profile->v5 * dt - (a * (int64_t)dt * dt) / 2;
    
    profile->t7 = profile->t6 + profile->t1;
    profile->x7 = distance;
    
    profile->profile_type = PROFILE_FULL;
    return true;
}

// Функция планирования профиля движения
bool planMotion(MotionParams *params, MotionProfile *profile, int32_t target_position) {
    if (params == NULL || profile == NULL) return false;
    
    int32_t distance = target_position - params->position;
    if (distance == 0) return false;  // Уже в целевой позиции
    
    // Определяем тип профиля
    uint8_t profile_type = determineProfileType(distance, params->max_velocity, 
                                             params->max_acceleration, params->jerk);
    
    bool result = false;
    
    // Вычисляем профиль движения в зависимости от его типа
    switch (profile_type) {
        case PROFILE_SHORT:
            result = calculateShortProfile(profile, distance, params->jerk);
            break;
        case PROFILE_NO_MAX_VELOCITY:
            result = calculateNoMaxVelocityProfile(profile, distance, 
                                                params->max_acceleration, params->jerk);
            break;
        case PROFILE_NO_CONST_VELOCITY:
            result = calculateNoConstVelocityProfile(profile, distance, params->max_velocity, 
                                                  params->max_acceleration, params->jerk);
            break;
        case PROFILE_FULL:
            result = calculateFullProfile(profile, distance, params->max_velocity, 
                                        params->max_acceleration, params->jerk);
            break;
        default:
            return false;
    }
    
    params->target_position = target_position;
    return result;
}

// Вычисление параметров движения в заданный момент времени
void calculateMotionAt(const MotionProfile *profile, uint32_t t, 
                     int32_t *position, int32_t *velocity, int32_t *acceleration) {
    if (profile == NULL || position == NULL || velocity == NULL || acceleration == NULL)
        return;
    
    int32_t pos = 0, vel = 0, acc = 0;
    int32_t dir = (profile->x7 > 0) ? 1 : -1;
    int32_t j = profile->x7 > 0 ? abs(profile->t1 * profile->t1) : -abs(profile->t1 * profile->t1);
    int32_t a_max = dir * abs(profile->v2 - profile->v1) / (profile->t2 - profile->t1);
    
    if (t >= profile->t7) {
        // После завершения движения
        pos = profile->x7;
        vel = 0;
        acc = 0;
    }
    else if (t < profile->t1) {
        // Фаза 1: Увеличение ускорения
        acc = j * t;
        vel = j * (int64_t)t * t / 2;
        pos = j * (int64_t)t * t * t / 6;
    }
    else if (t < profile->t2) {
        // Фаза 2: Движение с постоянным ускорением
        acc = a_max;
        vel = profile->v1 + acc * (t - profile->t1);
        pos = profile->x1 + profile->v1 * (t - profile->t1) + 
              acc * (int64_t)(t - profile->t1) * (t - profile->t1) / 2;
    }
    else if (t < profile->t3) {
        // Фаза 3: Снижение ускорения до нуля
        acc = a_max - j * (t - profile->t2);
        vel = profile->v2 + a_max * (t - profile->t2) - 
             j * (int64_t)(t - profile->t2) * (t - profile->t2) / 2;
        pos = profile->x2 + profile->v2 * (t - profile->t2) + 
             a_max * (int64_t)(t - profile->t2) * (t - profile->t2) / 2 - 
             j * (int64_t)(t - profile->t2) * (t - profile->t2) * (t - profile->t2) / 6;
    }
    else if (t < profile->t4) {
        // Фаза 4: Движение с постоянной скоростью
        acc = 0;
        vel = profile->v3;
        pos = profile->x3 + profile->v3 * (t - profile->t3);
    }
    else if (t < profile->t5) {
        // Фаза 5: Начало торможения
        acc = -j * (t - profile->t4);
        vel = profile->v3 - j * (int64_t)(t - profile->t4) * (t - profile->t4) / 2;
        pos = profile->x4 + profile->v3 * (t - profile->t4) - 
             j * (int64_t)(t - profile->t4) * (t - profile->t4) * (t - profile->t4) / 6;
    }
    else if (t < profile->t6) {
        // Фаза 6: Движение с постоянным замедлением
        acc = -a_max;
        vel = profile->v5 + acc * (t - profile->t5);
        pos = profile->x5 + profile->v5 * (t - profile->t5) + 
              acc * (int64_t)(t - profile->t5) * (t - profile->t5) / 2;
    }
    else {
        // Фаза 7: Снижение замедления
        acc = -a_max + j * (t - profile->t6);
        vel = profile->v6 - a_max * (t - profile->t6) + 
             j * (int64_t)(t - profile->t6) * (t - profile->t6) / 2;
        pos = profile->x6 + profile->v6 * (t - profile->t6) - 
             a_max * (int64_t)(t - profile->t6) * (t - profile->t6) / 2 + 
             j * (int64_t)(t - profile->t6) * (t - profile->t6) * (t - profile->t6) / 6;
    }
    
    *position = pos;
    *velocity = vel;
    *acceleration = acc;
}

Здесь он устал....

#include <stdio.h>
#include "motion_controller.h"

// Пример использования контроллера движения
int main(void) {
    // Инициализация параметров движения
    MotionParams params;
    MotionProfile profile;
    
    // Параметры для микроконтроллера (значения зависят от конкретной задачи)
    int32_t jerk = 100;              // шагов/сек^3
    int32_t max_acceleration = 500;  // шагов/сек^2
    int32_t max_velocity = 1000;     // шагов/сек
    int32_t min_velocity = 0;        // шагов/сек
    int32_t target_position = 5000;  // шагов
    
    // Инициализация
    initMotionParams(&params, jerk, max_acceleration, max_velocity, min_velocity);
    
    // Планирование движения
    if (!planMotion(&params, &profile, target_position)) {
        printf("Ошибка при планировании движения\n");
        return -1;
    }
    
    // Вывод информации о профиле движения
    printf("Тип профиля: %d\n", profile.profile_type);
    printf("Общее время движения: %u мс\n", profile.t7);
    printf("Максимальная достигаемая скорость: %d шагов/сек\n", profile.v3);
    
    // Можно также рассчитывать положение, скорость и ускорение в любой момент времени
    int32_t pos, vel, acc;
    uint32_