290 lines
8.6 KiB
C++
290 lines
8.6 KiB
C++
#include "scheduler.hpp"
|
|
#include "types.hpp"
|
|
#include "hal/gpio.hpp"
|
|
#include "hal/framebuffer.hpp"
|
|
#include "hal/encoder.hpp"
|
|
#include "control/pid.hpp"
|
|
#include "control/imu.hpp"
|
|
#include "control/motor.hpp"
|
|
#include "control/servo.hpp"
|
|
#include "vision/camera.hpp"
|
|
#include "vision/preprocess.hpp"
|
|
#include "vision/search.hpp"
|
|
#include "vision/element.hpp"
|
|
#include "strategy/deviation.hpp"
|
|
#include "strategy/speed.hpp"
|
|
#include "debug/draw.hpp"
|
|
#include "debug/config.hpp"
|
|
|
|
#include <atomic>
|
|
#include <chrono>
|
|
#include <thread>
|
|
#include <mutex>
|
|
#include <cstdio>
|
|
#include <sys/timerfd.h>
|
|
#include <sys/epoll.h>
|
|
#include <unistd.h>
|
|
|
|
// =========================================================
|
|
// 全局控制对象
|
|
// =========================================================
|
|
static YawTracker g_imu;
|
|
static Encoder* g_enc_l = nullptr;
|
|
static Encoder* g_enc_r = nullptr;
|
|
static PID g_pid_speed_l(0.6f, 0.2f, 0.0f, 0.0f, PID_INCREMENTAL, 100.0f, 50.0f);
|
|
static PID g_pid_speed_r(0.6f, 0.2f, 0.0f, 0.0f, PID_INCREMENTAL, 100.0f, 50.0f);
|
|
|
|
static float g_vision_yaw = 0.0f;
|
|
static float g_speed_l = 0.0f;
|
|
static float g_speed_r = 0.0f;
|
|
static float g_base_speed = 20.0f;
|
|
static float g_user_speed = 11.0f;
|
|
|
|
static std::atomic<bool> g_sched_running{false};
|
|
std::atomic<int> g_print_flag{0};
|
|
static int g_timer_fd = -1;
|
|
static int g_epoll_fd = -1;
|
|
static std::thread g_sched_thread;
|
|
static std::mutex g_vision_mutex;
|
|
static int g_tick = 0;
|
|
static int g_frame_cnt = 0;
|
|
static int g_bad_frame = 0;
|
|
static int g_empty_cnt = 0;
|
|
static FrameBuffer* g_fb_ptr = nullptr;
|
|
|
|
// =========================================================
|
|
// 5ms 回调
|
|
// =========================================================
|
|
|
|
static void sched_5ms(float dt)
|
|
{
|
|
if (g_enc_l) g_enc_l->update();
|
|
if (g_enc_r) g_enc_r->update();
|
|
g_imu.update(dt);
|
|
|
|
g_speed_l = g_enc_l ? g_enc_l->getRPS() : 0;
|
|
g_speed_r = g_enc_r ? g_enc_r->getRPS() : 0;
|
|
}
|
|
|
|
static void sched_10ms(float dt)
|
|
{
|
|
// 1. 视觉处理
|
|
cv::Mat bgr = camera_capture();
|
|
if (!bgr.empty())
|
|
{
|
|
std::lock_guard<std::mutex> lock(g_vision_mutex);
|
|
preprocess_run(bgr);
|
|
|
|
SearchResult sr;
|
|
search_run(sr);
|
|
|
|
TrackInfo info;
|
|
element_recognize(sr, info);
|
|
|
|
fit_midline(sr.lines);
|
|
info.deviation = calc_deviation(sr.lines);
|
|
info.base_speed = calc_base_speed(info);
|
|
info.curvature = info.deviation;
|
|
|
|
// 十字路口锁定直行
|
|
if (element_is_cross())
|
|
info.deviation = 0.0f;
|
|
|
|
g_vision_yaw = info.deviation;
|
|
g_base_speed = info.base_speed;
|
|
info.line_valid = (sr.lines.left_valid + sr.lines.right_valid) >= 1;
|
|
|
|
bool line_ok = (sr.lines.left_valid || sr.lines.right_valid);
|
|
if (!line_ok) ++g_bad_frame;
|
|
|
|
// LCD 预览 — 原始摄像头画面 + 车道线
|
|
if (read_config_flag(CFG_SHOW_IMG, false) && g_fb_ptr)
|
|
{
|
|
cv::Mat rgb;
|
|
cv::cvtColor(bgr, rgb, cv::COLOR_BGR2RGB);
|
|
cv::Mat disp;
|
|
cv::resize(rgb, disp, cv::Size(g_fb_ptr->width(), g_fb_ptr->height()));
|
|
|
|
int fw = g_fb_ptr->width();
|
|
int fh = g_fb_ptr->height();
|
|
float sx = (float)fw / IMAGE_WIDTH;
|
|
float sy = (float)fh / IMAGE_HEIGHT;
|
|
|
|
for (int y = 0; y < IPM_ROW_COUNT; ++y)
|
|
{
|
|
int dy = (int)(y * sy);
|
|
if (sr.lines.left[y] > 0)
|
|
cv::line(disp, cv::Point((int)(sr.lines.left[y] * sx), dy),
|
|
cv::Point((int)(sr.lines.left[y] * sx), dy), cv::Scalar(255,0,0), 5);
|
|
if (sr.lines.right[y] > 0)
|
|
cv::line(disp, cv::Point((int)(sr.lines.right[y] * sx), dy),
|
|
cv::Point((int)(sr.lines.right[y] * sx), dy), cv::Scalar(0,255,0), 5);
|
|
if (sr.lines.mid[y] > 0)
|
|
cv::line(disp, cv::Point((int)(sr.lines.mid[y] * sx), dy),
|
|
cv::Point((int)(sr.lines.mid[y] * sx), dy), cv::Scalar(255,255,0), 5);
|
|
}
|
|
|
|
// 帧计数闪烁点 — 验证屏幕在刷新
|
|
static int blink = 0;
|
|
blink = (blink + 1) % 20;
|
|
cv::circle(disp, cv::Point(10, fh - 10), 5,
|
|
blink < 10 ? cv::Scalar(255,255,255) : cv::Scalar(0,0,0), -1);
|
|
|
|
g_fb_ptr->write(disp.data, g_fb_ptr->width(), g_fb_ptr->height());
|
|
}
|
|
|
|
// 截帧到文件
|
|
if (read_config_flag(CFG_SAVE_IMG, false))
|
|
{
|
|
static int save_cnt = 0;
|
|
char fname[64];
|
|
snprintf(fname, sizeof(fname), "./capture_%04d.jpg", save_cnt++);
|
|
cv::imwrite(fname, bgr);
|
|
printf("[SAVE] %s (%dx%d)\n", fname, bgr.cols, bgr.rows);
|
|
}
|
|
|
|
++g_frame_cnt;
|
|
}
|
|
else
|
|
{
|
|
++g_empty_cnt;
|
|
}
|
|
|
|
// 2. 舵机转向
|
|
servo_set_angle(g_vision_yaw);
|
|
|
|
// 3. 差速辅助
|
|
float steer = g_vision_yaw;
|
|
float curve_factor = (std::abs(g_vision_yaw) < 0.1f) ? 1.0f : 0.8f;
|
|
float base_spd = g_user_speed * curve_factor;
|
|
float diff = steer * 0.15f * g_user_speed;
|
|
float pwm_l = base_spd + diff;
|
|
float pwm_r = base_spd - diff;
|
|
|
|
motor_set_pwm(MOTOR_LEFT, pwm_l);
|
|
motor_set_pwm(MOTOR_RIGHT, pwm_r);
|
|
}
|
|
|
|
static void sched_1s()
|
|
{
|
|
g_print_flag.store(1);
|
|
printf("[FPS] 视觉: %d 丢线: %d 空帧: %d\n",
|
|
g_frame_cnt, g_bad_frame, g_empty_cnt);
|
|
g_frame_cnt = 0;
|
|
g_bad_frame = 0;
|
|
g_empty_cnt = 0;
|
|
|
|
double speed_val = read_config_double(CFG_SPEED, 11.0);
|
|
g_user_speed = static_cast<float>(speed_val);
|
|
|
|
if (read_config_flag(CFG_START, false))
|
|
{
|
|
double kp = read_config_double(CFG_SERVO_KP, 3.5);
|
|
double ki = read_config_double(CFG_SERVO_KI, 0.3);
|
|
double kd = read_config_double(CFG_SERVO_KD, 0.2);
|
|
servo_set_pid(kp, ki, kd);
|
|
|
|
double deadband = read_config_double(CFG_DEADBAND, 0.03);
|
|
double gain = read_config_double(CFG_STEER_GAIN, 1.5);
|
|
servo_set_deadband(static_cast<float>(deadband));
|
|
servo_set_steer_gain(static_cast<float>(gain));
|
|
|
|
double mkp = read_config_double(CFG_MOTOR_KP, 0.6);
|
|
double mki = read_config_double(CFG_MOTOR_KI, 0.2);
|
|
double mkd = read_config_double(CFG_MOTOR_KD, 0.0);
|
|
g_pid_speed_l.setPID(mkp, mki, mkd);
|
|
g_pid_speed_r.setPID(mkp, mki, mkd);
|
|
}
|
|
}
|
|
|
|
static void scheduler_loop()
|
|
{
|
|
epoll_event ev;
|
|
while (g_sched_running.load())
|
|
{
|
|
int n = epoll_wait(g_epoll_fd, &ev, 1, 10);
|
|
if (n <= 0) continue;
|
|
|
|
uint64 expirations;
|
|
read(g_timer_fd, &expirations, sizeof(expirations));
|
|
|
|
for (uint64 i = 0; i < expirations; ++i)
|
|
{
|
|
float dt = 0.005f;
|
|
|
|
sched_5ms(dt);
|
|
|
|
if (g_tick % 2 == 0) sched_10ms(dt);
|
|
|
|
if (g_tick % 200 == 0) sched_1s();
|
|
|
|
++g_tick;
|
|
}
|
|
}
|
|
}
|
|
|
|
// =========================================================
|
|
// 初始化
|
|
// =========================================================
|
|
|
|
bool scheduler_init(FrameBuffer* fb)
|
|
{
|
|
g_fb_ptr = fb;
|
|
|
|
motor_init();
|
|
servo_init();
|
|
servo_center();
|
|
motor_enable(true);
|
|
|
|
g_enc_l = new Encoder(ENCODER_LEFT_CHANNEL, ENCODER_LEFT_DIR_GPIO, 1);
|
|
g_enc_r = new Encoder(ENCODER_RIGHT_CHANNEL, ENCODER_RIGHT_DIR_GPIO, -1);
|
|
|
|
g_imu.begin(IMU_DEVICE, IMU_BAUDRATE);
|
|
|
|
preprocess_init();
|
|
element_init();
|
|
speed_strategy_reset();
|
|
|
|
g_timer_fd = timerfd_create(CLOCK_MONOTONIC, 0);
|
|
if (g_timer_fd < 0) { printf("[SCHED] timerfd create failed\n"); return false; }
|
|
|
|
itimerspec its;
|
|
its.it_interval.tv_sec = 0;
|
|
its.it_interval.tv_nsec = 5 * 1000 * 1000; // 5ms
|
|
its.it_value = its.it_interval;
|
|
timerfd_settime(g_timer_fd, 0, &its, nullptr);
|
|
|
|
g_epoll_fd = epoll_create1(0);
|
|
epoll_event ev;
|
|
ev.events = EPOLLIN;
|
|
ev.data.fd = g_timer_fd;
|
|
epoll_ctl(g_epoll_fd, EPOLL_CTL_ADD, g_timer_fd, &ev);
|
|
|
|
printf("[SCHED] 调度器初始化完成 (5ms)\n");
|
|
return true;
|
|
}
|
|
|
|
void scheduler_start()
|
|
{
|
|
g_sched_running.store(true);
|
|
g_sched_thread = std::thread(scheduler_loop);
|
|
printf("[SCHED] 调度器启动\n");
|
|
}
|
|
|
|
void scheduler_stop()
|
|
{
|
|
g_sched_running.store(false);
|
|
if (g_sched_thread.joinable())
|
|
g_sched_thread.join();
|
|
|
|
motor_stop_all();
|
|
servo_center();
|
|
|
|
if (g_enc_l) { delete g_enc_l; g_enc_l = nullptr; }
|
|
if (g_enc_r) { delete g_enc_r; g_enc_r = nullptr; }
|
|
|
|
close(g_timer_fd);
|
|
close(g_epoll_fd);
|
|
printf("[SCHED] 调度器停止\n");
|
|
}
|