初始提交：龙芯2K0300智能车卖家Demo完整代码

src/GPIO.cpp

@@ -0,0 +1,95 @@
+/*
+ * @Author: ilikara 3435193369@qq.com
+ * @Date: 2024-10-10 15:02:10
+ * @LastEditors: ilikara 3435193369@qq.com
+ * @LastEditTime: 2024-12-01 03:50:49
+ * @FilePath: /smartcar/src/GPIO.cpp
+ * @Description: 
+ * 
+ * Copyright (c) 2024 by ${git_name_email}, All Rights Reserved. 
+ */
+#include "GPIO.h"
+
+GPIO::GPIO(int gpioNum) : gpioNum(gpioNum), fd(-1)
+{
+    gpioPath = "/sys/class/gpio/gpio" + std::to_string(gpioNum);
+
+    writeToFile("/sys/class/gpio/export", std::to_string(gpioNum));
+
+    fd = open((gpioPath + "/value").c_str(), O_RDWR);
+    if (fd == -1)
+    {
+        throw std::runtime_error("Failed to open GPIO value file: " + std::string(strerror(errno)));
+    }
+}
+
+GPIO::~GPIO()
+{
+    if (fd != -1)
+    {
+        close(fd); // 关闭文件描述符
+    }
+}
+
+bool GPIO::setDirection(const std::string &direction)
+{
+    return writeToFile(gpioPath + "/direction", direction);
+}
+
+bool GPIO::setEdge(const std::string &edge)
+{
+    return writeToFile(gpioPath + "/edge", edge);
+}
+
+bool GPIO::setValue(bool value)
+{
+    if (fd == -1)
+    {
+        std::cerr << "GPIO file descriptor is invalid" << std::endl;
+        return false;
+    }
+
+    // 使用文件描述符写入 GPIO 值 ('1' 或 '0')
+    const char *val_str = value ? "1" : "0";
+    if (write(fd, val_str, 1) != 1)
+    {
+        std::cerr << "Failed to write GPIO value: " << strerror(errno) << std::endl;
+        return false;
+    }
+    return true;
+}
+
+bool GPIO::readValue()
+{
+    if (fd == -1)
+    {
+        std::cerr << "GPIO file descriptor is invalid" << std::endl;
+        return false;
+    }
+
+    char value;
+    lseek(fd, 0, SEEK_SET); // 重置文件偏移量
+    if (read(fd, &value, 1) != 1)
+    {
+        std::cerr << "Failed to read GPIO value: " << strerror(errno) << std::endl;
+        return false;
+    }
+    return value == '1'; // 如果读取的值为 '1'，则返回 true，否则返回 false
+}
+
+int GPIO::getFileDescriptor() const
+{
+    return fd;
+}
+
+bool GPIO::writeToFile(const std::string &path, const std::string &value)
+{
+    int fd = ::open(path.c_str(), O_WRONLY);
+    if (fd == -1)
+    {
+        return false;
+    }
+    ssize_t n = ::write(fd, value.c_str(), value.size());
+    ::close(fd);
+    return (n == static_cast<ssize_t>(value.size()));
+}

src/MotorController.cpp

@@ -0,0 +1,62 @@
+/*
+ * @Author: ilikara 3435193369@qq.com
+ * @Date: 2024-10-10 14:36:42
+ * @LastEditors: ilikara 3435193369@qq.com
+ * @LastEditTime: 2025-03-21 10:41:17
+ * @FilePath: /smartcar/src/MotorController.cpp
+ * @Description: 这是默认设置,请设置`customMade`, 打开koroFileHeader查看配置 进行设置: https://github.com/OBKoro1/koro1FileHeader/wiki/%E9%85%8D%E7%BD%AE
+ */
+#include "MotorController.h"
+
+MotorController::MotorController(int pwmchip, int pwmnum, int gpioNum, unsigned int period_ns,
+                                 double kp, double ki, double kd, double targetSpeed,
+                                 int encoder_pwmNum, int encoder_gpioNum, int encoder_dir_)
+    : pwmController(pwmchip, pwmnum), directionGPIO(gpioNum), pidController(kp, ki, kd, targetSpeed, INCREMENTAL, 80),
+      encoder(encoder_pwmNum, encoder_gpioNum), encoder_dir(encoder_dir_)
+{
+    pwmController.setPeriod(period_ns); // 设置 PWM 周期
+    directionGPIO.setDirection("out");
+    pwmController.enable(); // 启用 PWM
+}
+
+MotorController::~MotorController(void)
+{
+    pwmController.disable();
+}
+
+void MotorController::updateduty(double dutyCycle)
+{
+    int newduty = pwmController.readPeriod() * abs(dutyCycle) / 100.0;
+    if (newduty != pwmController.readDutyCycle())
+    {
+        pwmController.setDutyCycle(newduty);
+    }
+
+    // 根据 PID 输出设置 GPIO 的方向
+    if (dutyCycle > 0)
+    {
+        directionGPIO.setValue(1); // 正向
+    }
+    else
+    {
+        directionGPIO.setValue(0); // 反向
+    }
+    //std::cout << encoder.pulse_counter_update() << std::endl;
+}
+
+void MotorController::updateSpeed(void)
+{
+    double encoderReading = encoder.pulse_counter_update() * encoder_dir;
+    // std::cout << encoderReading << std::endl;
+    double output = pidController.update(encoderReading);
+    // int dutyCycle = static_cast<int>(output);
+
+    // 设置 PWM 占空比
+    updateduty(output);
+    std::cout << encoderReading << "  " << output << std::endl;
+}
+
+void MotorController::updateTarget(int speed)
+{
+    pidController.setTarget(speed);
+}

src/PIDController.cpp

@@ -0,0 +1,94 @@
+#include "PIDController.h"
+
+// 构造函数，初始化 PID 参数
+PIDController::PIDController(double kp, double ki, double kd, double target, PIDMode mode,
+                             double output_limit, double integral_limit)
+    : kp_(kp), ki_(ki), kd_(kd), target_(target),
+      prev_error_(0.0), prev_prev_error_(0.0), integral_(0.0),
+      mode_(mode), prev_output_(0.0), output_limit_(output_limit), integral_limit_(integral_limit) {}
+
+// 更新 PID 控制器
+double PIDController::update(double measured_value)
+{
+    // 计算误差
+    double error = target_ - measured_value;
+
+    if (mode_ == POSITION)
+    {
+        // 位置式 PID 计算
+        return positionPID(error);
+    }
+    else
+    {
+        // 增量式 PID 计算
+        return incrementalPID(error);
+    }
+}
+
+// 设置新的目标值
+void PIDController::setTarget(double target)
+{
+    target_ = target;
+}
+
+// 设置 PID 参数
+void PIDController::setPID(double kp, double ki, double kd)
+{
+    kp_ = kp;
+    ki_ = ki;
+    kd_ = kd;
+}
+
+// 设置 PID 控制模式（位置式或增量式）
+void PIDController::setMode(PIDMode mode)
+{
+    mode_ = mode;
+}
+
+// 设置输出和积分的限幅值
+void PIDController::setLimits(double output_limit, double integral_limit)
+{
+    output_limit_ = output_limit;
+    integral_limit_ = integral_limit;
+}
+
+// 位置式 PID 实现
+double PIDController::positionPID(double error)
+{
+    // 计算积分项，进行积分饱和处理
+    integral_ += error;
+    integral_ = std::clamp(integral_, -integral_limit_, integral_limit_);
+
+    // 计算微分项
+    double derivative = (error - prev_error_);
+
+    // 计算输出
+    double output = kp_ * error + ki_ * integral_ + kd_ * derivative;
+
+    // 输出饱和处理
+    output = std::clamp(output, -output_limit_, output_limit_);
+
+    // 存储当前误差，用于下次计算
+    prev_error_ = error;
+
+    return output;
+}
+
+// 增量式 PID 实现
+double PIDController::incrementalPID(double error)
+{
+    // 增量输出计算公式
+    double delta_output = kp_ * (error - prev_error_) + ki_ * error + kd_ * (error - 2 * prev_error_ + prev_prev_error_);
+
+    // 更新误差历史
+    prev_prev_error_ = prev_error_;
+    prev_error_ = error;
+
+    // 累加增量得到新的输出
+    prev_output_ += delta_output;
+
+    // 输出饱和处理
+    prev_output_ = std::clamp(prev_output_, -output_limit_, output_limit_);
+
+    return prev_output_;
+}

src/PwmController.cpp

@@ -0,0 +1,85 @@
+/*
+ * @Author: ilikara 3435193369@qq.com
+ * @Date: 2024-09-17 08:21:50
+ * @LastEditors: ilikara 3435193369@qq.com
+ * @LastEditTime: 2025-03-20 12:03:51
+ * @FilePath: /smartcar/src/PwmController.cpp
+ * @Description: 这是默认设置,请设置`customMade`, 打开koroFileHeader查看配置 进行设置: https://github.com/OBKoro1/koro1FileHeader/wiki/%E9%85%8D%E7%BD%AE
+ */
+#include "PwmController.h"
+#include <fcntl.h>
+#include <unistd.h>
+
+PwmController::PwmController(int pwmchip, int pwmnum, bool polarity)
+    : pwmchip(pwmchip), pwmnum(pwmnum)
+{
+    initialize();
+    pwmPath = "/sys/class/pwm/pwmchip" + std::to_string(pwmchip) + "/pwm" + std::to_string(pwmnum) + "/";
+    setPolarity(polarity);
+}
+
+PwmController::~PwmController()
+{
+    disable(); // 析构时自动禁用PWM
+}
+
+int PwmController::readPeriod()
+{
+    return period;
+}
+
+int PwmController::readDutyCycle()
+{
+    return duty_cycle;
+}
+
+// 初始化PWM设备
+bool PwmController::initialize()
+{
+    std::string exportPath = "/sys/class/pwm/pwmchip" + std::to_string(pwmchip) + "/export";
+    return writeToFile(exportPath, std::to_string(pwmnum));
+}
+
+// 启用PWM
+bool PwmController::enable()
+{
+    return writeToFile(pwmPath + "enable", std::to_string(1));
+}
+
+// 禁用PWM
+bool PwmController::disable()
+{
+    return writeToFile(pwmPath + "enable", std::to_string(0));
+}
+
+// 设置周期（以纳秒为单位）
+bool PwmController::setPeriod(unsigned int period_ns)
+{
+    period = period_ns;
+    return writeToFile(pwmPath + "period", std::to_string(period_ns));
+}
+
+// 设置低电平时间（以纳秒为单位）
+bool PwmController::setDutyCycle(unsigned int duty_cycle_ns)
+{
+    duty_cycle = duty_cycle_ns;
+    return writeToFile(pwmPath + "duty_cycle", std::to_string(duty_cycle_ns));
+}
+
+// 设置极性
+bool PwmController::setPolarity(bool polarity)
+{
+    return writeToFile(pwmPath + "polarity", polarity ? "normal" : "inversed");
+}
+
+bool PwmController::writeToFile(const std::string &path, const std::string &value)
+{
+    int fd = open(path.c_str(), O_WRONLY);
+    if (fd == -1)
+    {
+        return false;
+    }
+    ssize_t n = write(fd, value.c_str(), value.size());
+    close(fd);
+    return (n == static_cast<ssize_t>(value.size()));
+}

src/Timer.cpp

@@ -0,0 +1,36 @@
+#include "Timer.h"
+
+Timer::Timer(int interval_ms, std::function<void()> task)
+    : interval(interval_ms), task(task), running(false) {}
+
+Timer::~Timer()
+{
+    stop(); // Ensure the timer is stopped in the destructor
+}
+
+void Timer::start()
+{
+    running = true;
+    worker = std::thread(&Timer::run, this);
+}
+
+void Timer::stop()
+{
+    running = false;
+    if (worker.joinable())
+    {
+        worker.join();
+    }
+}
+
+void Timer::run()
+{
+    while (running)
+    {
+        std::this_thread::sleep_for(std::chrono::milliseconds(interval));
+        if (running)
+        {
+            std::thread(task).detach();
+        }
+    }
+}

src/camera.cpp

@@ -0,0 +1,284 @@
+#include "camera.h"
+
+cv::VideoCapture cap;
+
+double kp = 0;
+double ki = 0;
+double kd = 0;
+
+int screenWidth, screenHeight;
+int newWidth, newHeight;
+int fb;
+// 创建帧缓冲区
+uint16_t *fb_buffer;
+PwmController servo(1, 0);
+
+#define calc_scale 2
+
+int CameraInit(uint8_t camera_id, double dest_fps, int width, int height)
+{
+    servo.setPeriod(3000000);
+    servo.setDutyCycle(1500000);
+    servo.enable();
+
+    // 打开帧缓冲区设备
+    fb = open("/dev/fb0", O_RDWR);
+    if (fb == -1)
+    {
+        std::cerr << "无法打开帧缓冲区设备" << std::endl;
+        return -1;
+    }
+
+    // 获取帧缓冲区设备信息
+    struct fb_var_screeninfo vinfo;
+    if (ioctl(fb, FBIOGET_VSCREENINFO, &vinfo) == -1)
+    {
+        std::cerr << "无法获取帧缓冲区信息" << std::endl;
+        close(fb);
+        return -1;
+    }
+
+    // 动态设置屏幕分辨率
+    screenWidth = vinfo.xres;
+    screenHeight = vinfo.yres;
+
+    // 计算帧缓冲区大小
+    size_t fb_size = vinfo.yres_virtual * vinfo.xres_virtual * vinfo.bits_per_pixel / 8;
+
+    // 使用 mmap 映射帧缓冲区到内存
+    fb_buffer = (uint16_t *)mmap(NULL, fb_size, PROT_READ | PROT_WRITE, MAP_SHARED, fb, 0);
+    if (fb_buffer == MAP_FAILED)
+    {
+        std::cerr << "无法映射帧缓冲区到内存" << std::endl;
+        close(fb);
+        return -1;
+    }
+
+    // 打开默认摄像头（设备编号 0）
+    cap.open(0, cv::CAP_V4L2);
+    if (!cap.isOpened()) cap.open(0);
+
+    cap.set(cv::CAP_PROP_FRAME_WIDTH, 320);
+    cap.set(cv::CAP_PROP_FRAME_HEIGHT, 240);
+    cap.set(cv::CAP_PROP_FOURCC, cv::VideoWriter::fourcc('M', 'J', 'P', 'G'));
+
+    // 检查摄像头是否成功打开
+    if (!cap.isOpened())
+    {
+        printf("无法打开摄像头\n");
+        munmap(fb_buffer, fb_size);
+        close(fb);
+        return -1;
+    }
+
+    cap.set(cv::CAP_PROP_FRAME_WIDTH, width);                                  // 宽度
+    cap.set(cv::CAP_PROP_FRAME_HEIGHT, height);                                // 高度
+    cap.set(cv::CAP_PROP_FOURCC, cv::VideoWriter::fourcc('M', 'J', 'P', 'G')); // 视频流格式
+    cap.set(cv::CAP_PROP_AUTO_EXPOSURE, -1);                                   // 设置自动曝光
+
+    // 获取摄像头实际分辨率
+    int cameraWidth = cap.get(cv::CAP_PROP_FRAME_WIDTH);
+    int cameraHeight = cap.get(cv::CAP_PROP_FRAME_HEIGHT);
+    printf("摄像头分辨率: %d x %d\n", cameraWidth, cameraHeight);
+
+    // 计算 newWidth 和 newHeight，确保图像适应屏幕
+    double widthRatio = static_cast<double>(screenWidth) / cameraWidth;
+    double heightRatio = static_cast<double>(screenHeight) / cameraHeight;
+    double scale = std::min(widthRatio, heightRatio); // 选择较小的比例，确保图像不超出屏幕
+
+    newWidth = static_cast<int>(cameraWidth * scale);
+    newHeight = static_cast<int>(cameraHeight * scale);
+    printf("自适应分辨率: %d x %d\n", newWidth, newHeight);
+
+    // 计算帧率
+    double fps = cap.get(cv::CAP_PROP_FPS);
+    printf("Camera fps:%lf\n", fps);
+
+    line_tracking_width = newWidth / calc_scale;
+    line_tracking_height = newHeight / calc_scale;
+
+    // 计算每帧的延迟时间（ms）
+    return static_cast<int>(1000.0 / std::min(fps, dest_fps));
+}
+
+void cameraDeInit(void)
+{
+    cap.release();
+
+    // 获取帧缓冲区设备信息
+    struct fb_var_screeninfo vinfo;
+    if (ioctl(fb, FBIOGET_VSCREENINFO, &vinfo) == -1)
+    {
+        std::cerr << "无法获取帧缓冲区信息" << std::endl;
+    }
+    else
+    {
+        // 计算帧缓冲区大小
+        size_t fb_size = vinfo.yres_virtual * vinfo.xres_virtual * vinfo.bits_per_pixel / 8;
+
+        // 取消映射
+        munmap(fb_buffer, fb_size);
+    }
+
+    close(fb);
+}
+
+int saved_frame_count = 0;
+bool saveCameraImage(cv::Mat frame, const std::string &directory)
+{
+    if (frame.empty())
+    {
+        std::cerr << "Save Error: Frame is empty." << std::endl;
+        return 0;
+    }
+
+    // 构建文件名
+    std::ostringstream filename;
+    filename << directory << "/image_" << std::setw(5) << std::setfill('0') << saved_frame_count << ".jpg";
+
+    saved_frame_count++;
+    // 保存图像
+    return cv::imwrite(filename.str(), frame);
+}
+
+std::mutex frameMutex;
+cv::Mat pubframe;
+bool streamCaptureRunning;
+void streamCapture(void)
+{
+    cv::Mat frame;
+    while (streamCaptureRunning)
+    {
+        cap.read(frame);
+        frameMutex.lock();
+        pubframe = frame;
+        frameMutex.unlock();
+    }
+    return;
+}
+
+// ===================================================
+// PIDController ServoControl(P=1.0, I=0, D=2.0, target=0, 位置式, 输出限幅=1,250,000)
+// 输出单位: 百分之一脉宽周期 (÷100 × period_ns → ns)
+// 实际等效线性增益: Kp=1.0 起主导, I=0 无积分, D=2.0 微分量抑制过冲
+// ===================================================
+double g_steer_deviation = 0;  // 全局偏差, 供速度控制用
+
+PIDController ServoControl(1.0, 0.0, 2.0, 0.0, POSITION, 1250000);
+int CameraHandler(void)
+{
+    cv::Mat resizedFrame;
+
+    frameMutex.lock();
+    raw_frame = pubframe;
+    frameMutex.unlock();
+
+    if (raw_frame.empty())
+    {
+        printf("无法捕获图像\n");
+        return -1;
+    }
+
+    if (readFlag(saveImg_file))
+    {
+        if (saveCameraImage(raw_frame, "./image"))
+        {
+            printf("图像%d已保存\n", saved_frame_count);
+        }
+        else
+        {
+            printf("图像保存失败\n");
+            return -1;
+        }
+    }
+
+    // ── 1. 视觉巡线 ──────────────────────────────────
+    // image_main() 处理 raw_frame → 80×60 图
+    // 产出: left_line[60], right_line[60], mid_line[60] (EMA 滤波后)
+    { // 图像计算
+        image_main();
+    }
+
+    // ── 2. 舵机转向控制 ──────────────────────────────
+    // 仅在 start 文件为 1 时执行 (readFlag(start_file))
+    // 否则保持上一次的脉宽 (不做任何转向)
+    if (readFlag(start_file))
+    {
+        // 2a. 前瞻行号换算
+        // foresee 是 newWidth×newHeight (160×120) 坐标系下的行号
+        // calc_scale=2, 除以 2 得到 80×60 (line_tracking) 下的行号
+        int foresee = readDoubleFromFile(foresee_file);
+        int check_row = foresee / calc_scale;
+
+            // 2b. 单行偏差计算 (80×60 坐标系 → 160×120 像素偏差)
+        if (check_row >= 0 && check_row < line_tracking_height && mid_line[check_row] != 255)
+        {
+            double deviation = mid_line[check_row] * calc_scale - newWidth / 2;
+            double bias = readDoubleFromFile(center_bias_file);  // 中位偏置(像素), 正=偏右
+            deviation -= bias;
+            double norm = deviation / (newWidth / 2.0);  // 归一化到 ±1
+            g_steer_deviation = norm;                     // 给速度控制用
+
+            // 2c. 死区 (像素)
+            double deadband = readDoubleFromFile(deadband_file);
+            if (std::abs(deviation) < deadband)
+            {
+                servo.setDutyCycle(1500000);  // 中位 1.50ms
+            }
+            else
+            {
+                // 2d. 比例转向: 像素偏差 → 归一化 → 舵机脉宽
+                double steer_gain = readDoubleFromFile(steer_gain_file);
+                double norm = deviation / (newWidth / 2.0);  // 归一化到 ±1
+                double offset = norm * steer_gain * 300000;   // 半行程 0.30ms
+                double duty_ns = 1500000.0 + offset;
+                duty_ns = std::clamp(duty_ns, 1200000.0, 1800000.0);
+                servo.setDutyCycle(static_cast<unsigned int>(duty_ns));
+            }
+        }
+    }
+    else
+    {
+        // servo.setDutyCycle(1520000);
+    }
+
+    // 显示图片
+    if (readFlag(showImg_file))
+    {
+        cv::Mat fbImage(screenHeight, screenWidth, CV_8UC3, cv::Scalar(0, 0, 0));
+
+        // 缩放视频到新尺寸
+        cv::resize(track, resizedFrame, cv::Size(newWidth, newHeight));
+        // 将单通道的二值化图像转换为三通道的彩色图像
+        cv::Mat coloredResizedFrame;
+        cv::cvtColor(resizedFrame, coloredResizedFrame, cv::COLOR_GRAY2BGR); // 转换为彩色图像
+
+        // 将缩放后的图像居中放置在帧缓冲区图像中，填充黑色边框
+        fbImage.setTo(cv::Scalar(0, 0, 0)); // 清空缓冲区（填充黑色）
+        cv::Rect roi((screenWidth - newWidth) / 2, (screenHeight - newHeight) / 2, newWidth, newHeight);
+        coloredResizedFrame.copyTo(fbImage(roi));
+
+        // 绘制左右边界线和中线
+        int scaledLeftX, scaledRightX, scaledMidX, scaledY;
+
+        for (int y = 0; y < line_tracking_height; y++)
+        {
+            // 根据缩放比例调整X坐标
+            scaledLeftX = static_cast<int>(left_line[y] * calc_scale);
+            scaledRightX = static_cast<int>(right_line[y] * calc_scale);
+            scaledMidX = static_cast<int>(mid_line[y] * calc_scale);
+            scaledY = static_cast<int>(y * calc_scale);
+
+            // 绘制左边界（红）
+            cv::line(fbImage(roi), cv::Point(scaledLeftX, scaledY), cv::Point(scaledLeftX, scaledY), cv::Scalar(0, 0, 255), calc_scale);
+            // 绘制右边界（绿）
+            cv::line(fbImage(roi), cv::Point(scaledRightX, scaledY), cv::Point(scaledRightX, scaledY), cv::Scalar(0, 255, 0), calc_scale);
+            // 绘制中线  （蓝）
+            cv::line(fbImage(roi), cv::Point(scaledMidX, scaledY), cv::Point(scaledMidX, scaledY), cv::Scalar(255, 0, 0), calc_scale);
+        }
+
+        // 将帧缓冲区图像转换为RGB565格式
+        convertMatToRGB565(fbImage, fb_buffer, screenWidth, screenHeight);
+    }
+    return 0;
+}

src/control.cpp

@@ -0,0 +1,81 @@
+/*
+ * @Author: ilikara 3435193369@qq.com
+ * @Date: 2024-10-10 09:02:10
+ * @LastEditors: ilikara 3435193369@qq.com
+ * @LastEditTime: 2025-03-21 10:40:10
+ * @FilePath: /smartcar/src/control.cpp
+ * @Description:
+ *
+ * Copyright (c) 2024 by ${git_name_email}, All Rights Reserved.
+ */
+#include "control.h"
+
+#include "GPIO.h"
+extern double g_steer_deviation;  // camera.cpp 输出的归一化偏差
+
+MotorController *motorController[2] = {nullptr, nullptr};
+GPIO mortorEN(73);
+double mortor_kp = 1000;
+double mortor_ki = 300;
+double mortor_kd = 0;
+
+void ControlInit()
+{
+    mortorEN.setDirection("out");
+    mortorEN.setValue(1);
+
+    GPIO leftIn2(13);
+    leftIn2.setDirection("out");
+    leftIn2.setValue(1);
+
+    const int pwmchip[2] = {8, 8};
+    const int pwmnum[2] = {2, 1};
+    const int gpioNum[2] = {12, 13};
+    const int encoder_pwmchip[2] = {0, 3};
+    const int encoder_gpioNum[2] = {75, 72};
+    const int encoder_dir[2] = {1, -1};
+    const unsigned int period_ns = 50000; // 20 kHz
+
+    for (int i = 0; i < 2; ++i)
+    {
+        motorController[i] = new MotorController(pwmchip[i], pwmnum[i], gpioNum[i], period_ns,
+                                                 mortor_kp, mortor_ki, mortor_kd, 0,
+                                                 encoder_pwmchip[i], encoder_gpioNum[i], encoder_dir[i]);
+    }
+}
+
+void ControlMain()
+{
+    if (readFlag(start_file))
+    {
+        // 弯道减速: 偏差大→速度低, 最低 30%
+        double curve = 1.0 - std::abs(g_steer_deviation) * 0.7;
+        if (curve < 0.3) curve = 0.3;
+        double spd = target_speed * curve;
+
+        for (int i = 0; i < 2; ++i)
+        {
+            motorController[i]->updateduty(spd);
+        }
+        mortorEN.setValue(1);
+    }
+    else
+    {
+        for (int i = 0; i < 2; ++i)
+        {
+            motorController[i]->updateduty(0);
+        }
+        mortorEN.setValue(0);
+    }
+    return;
+}
+
+void ControlExit()
+{
+    for (int i = 0; i < 2; ++i)
+    {
+        delete motorController[i];
+        std::cout << "motor" << i << "deleted\n";
+    }
+    mortorEN.setValue(0);
+}

src/encoder.cpp

@@ -0,0 +1,82 @@
+/*
+ * @Author: ilikara 3435193369@qq.com
+ * @Date: 2024-10-11 06:19:57
+ * @LastEditors: ilikara 3435193369@qq.com
+ * @LastEditTime: 2024-12-01 03:54:06
+ * @FilePath: /smartcar/src/encoder.cpp
+ * @Description:
+ *
+ * Copyright (c) 2024 by ilikara 3435193369@qq.com, All Rights Reserved.
+ */
+#include "encoder.h"
+
+ENCODER::ENCODER(int pwmNum, int gpioNum) : base_addr(PWM_BASE_ADDR + pwmNum * PWM_OFFSET), directionGPIO(gpioNum)
+{
+    directionGPIO.setDirection("in");
+
+    control_buffer = map_register(base_addr + CONTROL_REG_OFFSET, PAGE_SIZE);
+    low_buffer = map_register(base_addr + LOW_BUFFER_OFFSET, PAGE_SIZE);
+    full_buffer = map_register(base_addr + FULL_BUFFER_OFFSET, PAGE_SIZE);
+
+    printf("Registers mapped successfully\n");
+
+    PWM_Init();
+}
+
+ENCODER::~ENCODER()
+{
+    directionGPIO.~GPIO();
+    munmap(control_buffer, PAGE_SIZE);
+    munmap(low_buffer, PAGE_SIZE);
+    munmap(full_buffer, PAGE_SIZE);
+}
+
+void *ENCODER::map_register(uint32_t physical_address, size_t size)
+{
+    int mem_fd = open("/dev/mem", O_RDWR | O_SYNC);
+    if (mem_fd == -1)
+    {
+        perror("Failed to open /dev/mem");
+        exit(EXIT_FAILURE);
+    }
+
+    void *mapped_addr = mmap(NULL, size, PROT_READ | PROT_WRITE, MAP_SHARED, mem_fd, physical_address & ~(PAGE_SIZE - 1));
+    if (mapped_addr == MAP_FAILED)
+    {
+        perror("Failed to map memory");
+        close(mem_fd);
+        exit(EXIT_FAILURE);
+    }
+
+    close(mem_fd);
+
+    return (void *)((uintptr_t)mapped_addr + (physical_address & (PAGE_SIZE - 1)));
+}
+
+void ENCODER::PWM_Init(void)
+{
+    uint32_t control_reg = 0;
+
+    control_reg |= CNTR_ENABLE_BIT;
+    control_reg |= MEASURE_PULSE_BIT;
+    control_reg |= INT_ENABLE_BIT;
+
+    REG_WRITE(control_buffer, control_reg);
+
+    printf("PWM initialized with control register: 0x%08X\n", control_reg);
+}
+
+void ENCODER::reset_counter(void)
+{
+    uint32_t control_reg = REG_READ(control_buffer);
+    control_reg |= COUNTER_RESET_BIT;
+    REG_WRITE(control_buffer, control_reg);
+}
+
+double ENCODER::pulse_counter_update(void)
+{
+    double value = 100000000.0 / REG_READ(full_buffer) / 1024.0 * (directionGPIO.readValue() * 2 - 1);
+    // reset_counter();
+    // printf("Encoder RPS: %8.1lf\n", 100000000.0 / REG_READ(full_buffer) / 1024.0 * (gpio_value * 2 - 1));
+    return value;
+}

src/frame_buffer.cpp

@@ -0,0 +1,20 @@
+#include "frame_buffer.h"
+
+// 将RGB转换为RGB565格式
+uint16_t convertRGBToRGB565(uint8_t r, uint8_t g, uint8_t b)
+{
+    return ((r & 0xF8) << 8) | ((g & 0xFC) << 3) | (b >> 3);
+}
+
+// 将Mat图像转换为RGB565格式
+void convertMatToRGB565(const cv::Mat &frame, uint16_t *buffer, int width, int height)
+{
+    for (int y = 0; y < height; y++)
+    {
+        for (int x = 0; x < width; x++)
+        {
+            cv::Vec3b color = frame.at<cv::Vec3b>(y, x);
+            buffer[y * width + x] = convertRGBToRGB565(color[2], color[1], color[0]);
+        }
+    }
+}

src/global.cpp

@@ -0,0 +1,37 @@
+#include "global.h"
+
+double target_speed;
+
+// 从文件读取双精度值
+double readDoubleFromFile(const std::string &filename)
+{
+    std::ifstream file(filename);
+    double value = 0.0;
+    if (file.is_open())
+    {
+        file >> value; // 读取文件中的值
+        file.close();
+    }
+    else
+    {
+        std::cerr << "Failed to open " << filename << std::endl;
+    }
+    return value;
+}
+
+// 从文件中读取标志
+bool readFlag(const std::string &filename)
+{
+    std::ifstream file(filename);
+    int flag = 0;
+    if (file.is_open())
+    {
+        file >> flag; // 读取文件中的更新标志
+        file.close();
+    }
+    else
+    {
+        std::cerr << "Failed to open " << filename << std::endl;
+    }
+    return flag;
+}

src/image_cv.cpp

@@ -0,0 +1,177 @@
+/*
+ * @Author: ilikara 3435193369@qq.com
+ * @Date: 2025-01-04 06:50:56
+ * @LastEditors: ilikara 3435193369@qq.com
+ * @LastEditTime: 2025-03-13 08:15:10
+ * @FilePath: /2k300_smartcar/src/image_cv.cpp
+ * @Description: 这是默认设置,请设置`customMade`, 打开koroFileHeader查看配置 进行设置: https://github.com/OBKoro1/koro1FileHeader/wiki/%E9%85%8D%E7%BD%AE
+ */
+#include "image_cv.h"
+
+cv::Mat raw_frame;
+cv::Mat grayFrame;
+cv::Mat binarizedFrame;
+cv::Mat morphologyExFrame;
+cv::Mat track;
+
+std::vector<int> left_line;              // 左边缘列号数组
+std::vector<int> right_line;             // 右边缘列号数组
+std::vector<int> mid_line;               // 中线列号数组
+std::vector<double> left_line_filtered;  // 中线列号数组
+std::vector<double> right_line_filtered; // 中线列号数组
+std::vector<double> mid_line_filtered;   // 中线列号数组
+
+int line_tracking_width;
+int line_tracking_height;
+
+cv::Mat image_binerize(cv::Mat &frame)
+{
+    cv::Mat output;
+    cv::Mat binarizedFrame;
+    cv::Mat hsvImage;
+    cv::cvtColor(frame, hsvImage, cv::COLOR_BGR2HSV);
+
+    std::vector<cv::Mat> hsvChannels;
+    cv::split(hsvImage, hsvChannels);
+
+    cv::threshold(hsvChannels[0], binarizedFrame, 0, 255, cv::THRESH_BINARY_INV | cv::THRESH_OTSU);
+    cv::threshold(hsvChannels[1], output, 0, 255, cv::THRESH_BINARY_INV | cv::THRESH_OTSU);
+
+    cv::bitwise_or(output, binarizedFrame, output);
+
+    return output;
+}
+
+cv::Mat find_road(cv::Mat &frame)
+{
+    cv::Mat kernel = cv::getStructuringElement(cv::MORPH_CROSS, cv::Size(2, 2));
+    cv::morphologyEx(binarizedFrame, morphologyExFrame, cv::MORPH_OPEN, kernel);
+
+    cv::Mat mask = cv::Mat::zeros(line_tracking_height + 2, line_tracking_width + 2, CV_8UC1);
+
+    cv::Point seedPoint(line_tracking_width / 2, line_tracking_height - 10);
+
+    cv::circle(morphologyExFrame, seedPoint, 10, 255, -1);
+
+    cv::Scalar newVal(128);
+
+    cv::Scalar loDiff = cv::Scalar(20);
+    cv::Scalar upDiff = cv::Scalar(20);
+
+    cv::floodFill(morphologyExFrame, mask, seedPoint, newVal, 0, loDiff, upDiff, 8);
+
+    cv::Mat outputImage = cv::Mat::zeros(line_tracking_width, line_tracking_height, CV_8UC1);
+
+    mask(cv::Rect(1, 1, line_tracking_width, line_tracking_height)).copyTo(outputImage);
+
+    return outputImage;
+}
+
+void image_main()
+{
+    cv::Mat resizedFrame;
+
+    cv::resize(raw_frame, resizedFrame, cv::Size(line_tracking_width, line_tracking_height));
+
+    binarizedFrame = image_binerize(resizedFrame);
+
+    track = find_road(binarizedFrame);
+
+    left_line.clear();
+    right_line.clear();
+    mid_line.clear();
+    left_line_filtered.clear();
+    right_line_filtered.clear();
+    mid_line_filtered.clear();
+
+    left_line.resize(line_tracking_height, -1);
+    right_line.resize(line_tracking_height, -1);
+    mid_line.resize(line_tracking_height, -1);
+    left_line_filtered.resize(line_tracking_height, -1);
+    right_line_filtered.resize(line_tracking_height, -1);
+    mid_line_filtered.resize(line_tracking_height, -1);
+
+    uchar(*IMG)[line_tracking_width] = reinterpret_cast<uchar(*)[line_tracking_width]>(track.data);
+
+    for (int i = 0; i < line_tracking_height; ++i)
+    {
+        int max_start = -1;
+        int max_end = -1;
+        int current_start = -1;
+        int current_length = 0;
+        int max_length = 0;
+
+        for (int j = 0; j < line_tracking_width; ++j)
+        {
+            if (IMG[i][j])
+            {
+                if (current_length == 0)
+                {
+                    current_start = j;
+                    current_length = 1;
+                }
+                else
+                {
+                    current_length++;
+                }
+                if (current_length >= max_length)
+                {
+                    max_length = current_length;
+                    max_start = current_start;
+                    max_end = j;
+                }
+            }
+            else
+            {
+                current_length = 0;
+                current_start = -1;
+            }
+        }
+        if (max_length > 0)
+        {
+            left_line[i] = max_start;
+            right_line[i] = max_end;
+        }
+        else
+        {
+            left_line[i] = -1;
+            right_line[i] = -1;
+        }
+    }
+
+    double a = 0.4;
+    for (int row = line_tracking_height - 1; row >= 10; --row)
+    {
+        if (left_line[row] == -1 && right_line[row] == -1)
+        {
+            mid_line[row] = mid_line[row + 1];
+            if (mid_line[row] > line_tracking_width / 2)
+            {
+                right_line[row] = line_tracking_width - 1;
+                left_line[row] = mid_line[row + 1];
+            }
+            else
+            {
+                left_line[row] = 0;
+                right_line[row] = mid_line[row + 1];
+            }
+        }
+        else
+        {
+            mid_line[row] = (left_line[row] + right_line[row]) / 2;
+        }
+        if (row == line_tracking_height - 1)
+        {
+            left_line_filtered[row] = left_line[row];
+            right_line_filtered[row] = right_line[row];
+            mid_line_filtered[row] = mid_line[row];
+        }
+        else
+        {
+            left_line_filtered[row] = a * left_line[row] + (1 - a) * left_line_filtered[row + 1];
+            right_line_filtered[row] = a * right_line[row] + (1 - a) * right_line_filtered[row + 1];
+            // mid_line_filtered[row] = a * mid_line[row] + (1 - a) * mid_line_filtered[row + 1];
+            mid_line_filtered[row] = (left_line_filtered[row] + right_line_filtered[row]) / 2.0;
+        }
+    }
+}

src/serial.cpp

@@ -0,0 +1,40 @@
+#include "serial.h"
+
+char vofa_buffer[64];
+
+bool vofa_justfloat(int CH_count)
+{
+    const unsigned char tail[4]{0x00, 0x00, 0x80, 0x7f};
+    std::string path = "/dev/ttyS0";
+    std::ofstream file(path);
+    if (!file.is_open())
+    {
+        std::cerr << "Error opening file: " << path << std::endl;
+        return false;
+    }
+    file.write((char *)vofa_buffer, CH_count * 4);
+    file.write((char *)tail, sizeof(tail));
+    file.close();
+    return true;
+}
+
+bool vofa_image(int IMG_ID, int IMG_SIZE, int IMG_WIDTH, int IMG_HEIGHT, ImgFormat IMG_FORMAT, char *image)
+{
+    int preFrame[7] = {0, 0, 0, 0, 0, 0x7F800000, 0x7F800000};
+    preFrame[0] = IMG_ID;     // 此ID用于标识不同图片通道
+    preFrame[1] = IMG_SIZE;   // 图片数据大小
+    preFrame[2] = IMG_WIDTH;  // 图片宽度
+    preFrame[3] = IMG_HEIGHT; // 图片高度
+    preFrame[4] = IMG_FORMAT; // 图片格式
+    std::string path = "/dev/ttyS0";
+    std::ofstream file(path);
+    if (!file.is_open())
+    {
+        std::cerr << "Error opening file: " << path << std::endl;
+        return false;
+    }
+    file.write((char *)preFrame, sizeof(preFrame));
+    file.write((char *)image, IMG_SIZE);
+    file.close();
+    return true;
+}

src/video.cpp

@@ -0,0 +1,20 @@
+#include "video.h"
+
+Video::Video(const std::string &filename, double fps) 
+    : timer(static_cast<int>(1000 / fps), std::bind(&Video::streamCapture, this)), cap(filename)
+{
+    streamCapture();
+}
+
+Video::~Video(void)
+{
+    timer.stop();
+}
+
+void Video::streamCapture(void) 
+{
+    frameMutex.lock();
+    cap.read(frame);
+    frameMutex.unlock();
+}
+    

src/vl53l0x.cpp

@@ -0,0 +1,72 @@
+#include "vl53l0x.h"
+#include <fcntl.h>
+#include <unistd.h>
+#include <sys/ioctl.h>
+#include <cstring>
+#include <cerrno>
+#include <cstdio>
+
+#define VL53L0X_IOCTL_INIT    _IO('p', 0x01)
+#define VL53L0X_IOCTL_STOP    _IO('p', 0x05)
+#define VL53L0X_IOCTL_GETDATA _IOR('p', 0x0b, VL53L0X_RangingMeasurementData_t)
+
+VL53L0X::VL53L0X() : fd(-1) {}
+
+VL53L0X::~VL53L0X()
+{
+    if (fd > 0)
+    {
+        stop();
+        close(fd);
+        fd = -1;
+    }
+}
+
+bool VL53L0X::init()
+{
+    fd = open("/dev/stmvl53l0x_ranging", O_RDWR | O_SYNC);
+    if (fd <= 0)
+    {
+        fprintf(stderr, "[VL53L0X] open failed: %s\n", strerror(errno));
+        return false;
+    }
+
+    ioctl(fd, VL53L0X_IOCTL_STOP, nullptr);
+
+    if (ioctl(fd, VL53L0X_IOCTL_INIT, nullptr) < 0)
+    {
+        fprintf(stderr, "[VL53L0X] init failed: %s\n", strerror(errno));
+        close(fd);
+        fd = -1;
+        return false;
+    }
+
+    fprintf(stderr, "[VL53L0X] init OK\n");
+    return true;
+}
+
+bool VL53L0X::readRange(VL53L0X_RangingMeasurementData_t &data)
+{
+    if (fd <= 0)
+        return false;
+
+    if (ioctl(fd, VL53L0X_IOCTL_GETDATA, &data) < 0)
+    {
+        fprintf(stderr, "[VL53L0X] read failed: %s\n", strerror(errno));
+        return false;
+    }
+    return true;
+}
+
+bool VL53L0X::stop()
+{
+    if (fd <= 0)
+        return false;
+
+    if (ioctl(fd, VL53L0X_IOCTL_STOP, nullptr) < 0)
+    {
+        fprintf(stderr, "[VL53L0X] stop failed: %s\n", strerror(errno));
+        return false;
+    }
+    return true;
+}