main.cpp - master - Logrus/CopterControl

main.cpp
97 строк4 KB
Nikolay Nosorev
Initial commit
04 окт 2025, 16:17
04 окт 2025, 16:1799ef28c
#include "pch.h"
#include <iomanip> 
//
#include "config.h"
#include "quadcopter.h"
#include "controller.h"
#include "trajectory.h"
#include "optimizer.h"

int main() {
	initConfig(); // загружает config.yaml → заполняет глобальную переменную cfg
    // Создание планировщика траектории
    MinimumSnapTrajectory planner(cfg.waypoints, true, false);
    std::cout << "planner created\n";// << std::endl;
    double T_sim = planner.get_total_time();
    std::cout << "Estimated T_sim: " << T_sim << " seconds\n";// << std::endl;
    //Вывод параметров траектории
    TrajectoryPoint tp;
    int n_print_steps = static_cast<int>(cfg.waypoints.size() * 3 - 1);
    for (int i = 0; i < n_print_steps; ++i) {
        double t = i * (T_sim + cfg.dt) / (n_print_steps - 1); // linspace аналог

        tp = planner.evaluate(t);
        Eigen::Vector3d pos = tp.pos;
        Eigen::Vector3d vel = tp.vel;
        Eigen::Vector3d acc = tp.acc;
        double yaw = tp.psi;

        std::cout << "t=" << std::fixed << std::setprecision(2) << t
                  << " | pos=[" << pos.x() << ", " << pos.y() << ", " << pos.z() << "]"
                  << " | vel=[" << vel.x() << ", " << vel.y() << ", " << vel.z() << "]"
                  << " | acc=[" << acc.x() << ", " << acc.y() << ", " << acc.z() << "]"
                  << " | yaw=" << std::setprecision(2) << yaw
                  << std::endl;
    }
    // Инициализация квадрокоптера и контроллера
    Quadcopter quad;
	    // Подготовка начальных параметров
    Params initial_params{
        cfg.Kp_pos_xy, cfg.Kp_pos_z, cfg.Ki_pos_z,
        cfg.Kp_vel_xy, cfg.Kd_vel_xy, cfg.Kp_vel_z, cfg.Kd_vel_z,
        cfg.kp_phi, cfg.kp_theta, cfg.kp_psi,
        cfg.kd_phi, cfg.kd_theta, cfg.kd_psi
    };	 
	 // std::vector<double> vparams=cfg.toVectorParams();
	 // for(double p: vparams) std::cout<<p<<" ";std::cout<<"\n";
	// Params p = Params::fromVector(vparams);
	// std::cout<<p.Kp_pos_xy<<" "<<p.Kp_pos_z<<" "<<p.Ki_pos_z<<" "<<p.Kp_vel_xy<<" "<<p.Kd_vel_xy<<" "<<p.Kp_vel_z<<" "<<p.Kd_vel_z<<" "<<p.kp_phi<<" "<<p.kp_theta<<" "<<p. kp_psi<<" "<<p.kd_phi<<" "<<p.kd_theta<<" "<<p. kd_psi<<"\n";
    // Controller controller(Params::fromVector(vparams),cfg.dt);
    Controller controller(initial_params,cfg.dt);

    // Симуляция
    int step_count = 0;
    for (double t = 0; t <= T_sim; t += cfg.dt) {
        // Получение желаемых значений
        tp = planner.evaluate(t);
        Eigen::Vector3d pos_des = tp.pos;
        Eigen::Vector3d vel_des = tp.vel;
        Eigen::Vector3d acc_des = tp.acc;
        double psi_des = tp.psi;
        
        // Вычисление управления
        double F_total;
        Eigen::Vector3d M;
        controller.compute_control(
            quad.pos, quad.vel, quad.angles, quad.ang_vel,
            pos_des, vel_des, acc_des, psi_des,
            F_total, M
        );
        
        // Расчет скоростей моторов
        Eigen::Vector4d motor_speeds = controller.calculate_motor_speeds(F_total, M);
        quad.w1 = motor_speeds.x();
        quad.w2 = motor_speeds.y();
        quad.w3 = motor_speeds.z();
        quad.w4 = motor_speeds.w();
        
        // Обновление состояния квадрокоптера
        quad.update_state(F_total, M, cfg.dt);
        
        // Вывод каждые 50 шагов
        if (step_count % 50 == 0) {
            std::cout << "Time: " << t << " s | Current pos: [" 
                      << quad.pos.transpose() << "] | Desired pos: [" 
                      << pos_des.transpose() << "] | W: ["
                      << quad.w1 << ", " << quad.w2 << ", " 
                      << quad.w3 << ", " << quad.w4 << "]" << '\n';//std::endl;
        }
        step_count++;
    }
	PIDOptimizer optimizer=PIDOptimizer(planner,cfg.dt);
	std::cout<<"PID optimizer created\n";
	struct Params optimized=optimizer.optimize_BOBYQA(initial_params);
	std::cout<<"\nOptimized parameters "<<optimized.Kp_pos_xy<<" "<<optimized.Kp_pos_z<<" "<<optimized.Ki_pos_z<<" "<<optimized.Kp_vel_xy<<" "<<optimized.Kd_vel_xy<<" "<<optimized.Kp_vel_z<<" "<<optimized.Kd_vel_z<<" "<<optimized.kp_phi<<" "<<optimized.kp_theta<<" "<<optimized.kp_psi<<" "<<optimized.kd_phi<<" "<<optimized.kd_theta<<" "<<optimized.kd_psi;
    
    return 0;
}