平衡摩托总结

这是对代码逻辑的简要总结，详细的代码位于https://github.com/baikal2411/SMC/

详细的外设要求信息见第十九届智能车摩托组外设清单。

CPU资源安排

对于cpu0而言：需要等待 CPU1 核心完成初始化或某些事件准备就绪，确保多核协作的同步。

seekfree_assistant_interface_init(SEEKFREE_ASSISTANT_WIRELESS_UART)：初始化无线串口助手接口。

seekfree_assistant_camera_information_config(...)：配置摄像头助手的信息，包括摄像头类型、图像数据缓冲区及分辨率。

通过 VOFA 工具发送波形数据，用于调试或监控。

对于cpu1而言：直接运行图像识别。

中断设置

PIT 定时器中断

cc60_pit_ch0_isr 和 cc60_pit_ch1_isr

处理定时器中断，清除中断标志。在cc60_pit_ch0_isr中：

扫描按键（User_keyScan()）。
获取陀螺仪和加速度计数据（imu660ra_get_gyro() 和 imu660ra_get_acc()）。
调用控制逻辑（CH0_LOOP()）。
如果检测到停止标志或陀螺仪角度异常，则停止运动并复位控制。
在cc60_pit_ch1_isr中：清除中断标志.

外部中断

exti_ch0_ch4_isr 到 exti_ch3_ch7_isr：处理外部引脚中断，清除中断标志。

例如：在 exti_ch3_ch7_isr 中，处理摄像头的垂直同步信号（camera_vsync_handler()）和点阵屏扫描（dot_matrix_screen_scan()）。在 exti_ch1_ch5_isr 中，处理 ToF 模块的中断（tof_module_exti_handler()）

DMA中断

dma_ch5_isr处理 DMA 传输完成中断。并且调用摄像头 DMA 数据处理函数（camera_dma_handler()）。

UART中断

发送和接收中断

例如：
- uart0_rx_isr：处理 UART0 的接收中断，调用调试或数据处理函数（如 debug_interrupr_handler()）。
- uart1_rx_isr：处理摄像头 UART 数据接收（camera_uart_handler()）和 TLD7002 回调。
- uart2_rx_isr：处理无线模块 UART 数据接收（wireless_module_uart_handler()）。
- uart3_rx_isr：处理 GNSS 模块的 UART 数据接收（gnss_uart_callback()）。
错误中断
- 例如：
  - uart0_er_isr 到 uart3_er_isr：处理 UART 错误中断，调用错误处理函数（如 IfxAsclin_Asc_isrError()）。

在每个中断服务程序中，使用 interrupt_global_enable(0) 来启用全局中断，避免中断嵌套问题。

控制逻辑

位于文件control_strategy当中

1. 偏差计算

computeDevation 函数
- 通过图像处理结果（如中线、左线和右线的位置），计算车辆相对于路径的偏差。
- 根据路径的曲率（curvity）判断轨迹是左偏、右偏还是直线。
- 计算偏差值（devation 和 part_devation），并根据车辆位置（如是否丢线）进行修正。
- 输出偏差值，用于后续的方向控制。

2. 路径规划

根据图像中线和左右边界线的位置，规划车辆的前视路径（foresight_str 和 foresight_end）。
判断路径的偏移方向（左偏或右偏），并调整偏差值以适应不同的道路情况。

3. 速度控制

getTargetSpeed 函数
- 根据当前偏差值（current_devation），动态调整目标速度（target_speed）。
- 偏差较小时，允许更高的速度；偏差较大时，降低速度以提高安全性。

4. 姿态控制

CH0_LOOP 函数
- 周期性更新车辆的姿态和运动参数：
  - 每 1ms 更新一次 Roll 角速度控制。
  - 每 5ms 更新一次 Roll 角度控制。
  - 每 50ms 更新一次 Yaw 角速度和方向控制。
  - 每 100ms 更新一次速度控制。
- 使用 PID 控制器（如 ROLL_AV_PID、ROLL_AG_PID 和 YAW_AV_PID）对车辆的姿态和速度进行精确控制。
- 调用 controlSteer 和 controlSwing 函数调整舵机和电机的输出。

5. 偏差调整

Devation_Adjust 函数
- 根据不同的场景（如左环岛、右环岛等），对偏差值进行限制和修正。
- 确保车辆在特殊场景下能够保持稳定的轨迹。

下面给出中断的控制函数：

void CH0_LOOP(void)
{

   // float temp_devation = computeDevation(REFERENCE_STR, REFERENCE_END, STRAIGHT_LIMIT);
    devation = Kalman_Filter(&DEVATION_KALMAN,current_devation);

    if (control_time % 2 == 0)
    {
        Attitude_Solution(Mahony);
        gyro_pitch_ag += 3;//correct pitch to zero
    }

    // 100ms更新一次速度环
    if (control_time % 200 == 0)
    {
        control_time = 0;
        Encoder_getData();
        current_speed = -SPEED_INDEX * (encoder_left + encoder_right);
        getTargetSpeed();
        speed_out = positionalPID(&SPEED_PID, speed_pid, current_speed, target_speed);
    }
    // 50ms更新一次Yaw角速度 
    if (control_time % 100 == 0)
    {
        Devation_Adjust();//ADJUST DEVATION
        yaw_av_out = positionalPID(&YAW_AV_PID, yaw_av_pid, gyro_yaw_av, 0);
        target_accel = 2.5f;
        if (gyro_pitch_ag > 10)
            accel_out = incrementalPID(&ACCEL_PID, accel_pid, gyro_roll_ac, (target_accel + PITCH_INDEX * sinf(gyro_pitch_ag * 0.01745f)));
        else
            {
                accel_out = 0;
                Mduty = 2000;
            }
        turn_out = positionalPID(&TURN_PID, turn_pid, devation, 0);
        turn_out = protectRange(turn_out,-80,+80);
        controlSteer(SEVER_CENTER + turn_out);
        bend_out = positionalPID(&BEND_PID, bend_pid, devation, 0);
        Mduty += accel_out;
    }
    // 5ms更新一次 Roll角度
    if (control_time % 10 == 0)
    {
        roll_ag_out = positionalPID(&ROLL_AG_PID, roll_ag_pid, gyro_roll_ag, -yaw_av_out-bend_out);
    }
    // 1ms更新一次 Roll角速度,
    if (control_time % 2 == 0)
    {
        Tduty = incrementalPID(&ROLL_AV_PID,roll_av_pid,gyro_roll_av,roll_ag_out + dynamic_zero);
//        Tduty = incrementalPID(&ROLL_AV_PID,roll_av_pid,gyro_roll_av,roll_ag_out + dynamic_zero);
        // 限幅
        Mduty = protectRange(Mduty,-4000,+4000);

        Tduty = protectRange(Tduty,-4000,+4000);
        if (gyro_pitch_ag > 10)
            controlSwing(-Mduty,Tduty);
        else
            controlSwing(-2000,Tduty);
    }
    control_time += 2;
    system_time++;
}

其中最重要的就是串级PID的使用了，

(1) 外环：速度控制

输入：目标速度（target_speed）与当前速度（current_speed）的误差。
输出：加速度控制目标（speed_out），作为内环的输入。

(2) 内环：加速度控制

输入：目标加速度（target_accel）与当前加速度（gyro_roll_ac）的误差。
输出：电机的PWM补偿（accel_out）。

(3) 方向控制

输入：偏差（devation）与目标偏差（通常为0）的误差。
输出：舵机的控制信号（turn_out）。

(4) 姿态控制

输入：目标Roll角速度（roll_ag_out）与当前Roll角速度（gyro_roll_av）的误差。
输出：电机的PWM信号（Tduty）。

(1) 速度控制

目标：根据当前速度和目标速度，调整车辆的加速度和电机输出。

实现

Encoder_getData();

current_speed = -SPEED_INDEX * (encoder_left + encoder_right);

getTargetSpeed();

speed_out = positionalPID(&SPEED_PID, speed_pid, current_speed, target_speed);

Encoder_getData() 获取当前速度。
getTargetSpeed() 根据偏差计算目标速度。
positionalPID 使用速度 PID 控制器计算速度误差，输出 speed_out。

(2) 加速度控制

目标：根据目标加速度和当前加速度，调整车辆的动力输出。

实现

if (gyro_pitch_ag > 10)

  accel_out = **incrementalPID**(&ACCEL_PID, accel_pid, gyro_roll_ac, (target_accel + PITCH_INDEX * **sinf**(gyro_pitch_ag * 0.01745f)));

else

  accel_out = 0;

使用增量式 PID 控制器（incrementalPID）计算加速度误差，输出 accel_out。
当车辆的 Pitch 角度较大时，考虑重力影响进行补偿。

(3) 方向控制

目标：根据偏差调整车辆的方向。

实现

turn_out = positionalPID(&TURN_PID, turn_pid, devation, 0);

turn_out = protectRange(turn_out, -80, +80);

controlSteer(SEVER_CENTER + turn_out);

使用方向 PID 控制器（TURN_PID）计算偏差误差，输出 turn_out。
调用 controlSteer 调整舵机角度。

(4) Roll 角度控制

目标：通过 Roll 角度 PID 控制器调整车辆的平衡。
实现
1
roll_ag_out = **positionalPID**(&ROLL_AG_PID, roll_ag_pid, gyro_roll_ag, -yaw_av_out - bend_out);
- 使用 Roll 角度 PID 控制器（ROLL_AG_PID）计算 Roll 角度误差，输出 roll_ag_out。
- 结合 Yaw 角速度和弯道补偿（bend_out）进行调整。

(5) Roll 角速度控制

目标：通过 Roll 角速度 PID 控制器调整电机输出。
实现
1
Tduty = incrementalPID(&ROLL_AV_PID, roll_av_pid, gyro_roll_av, roll_ag_out + dynamic_zero);
- 使用 Roll 角速度 PID 控制器（ROLL_AV_PID）计算 Roll 角速度误差，输出电机占空比 Tduty。

图像的预处理

image_process.c 文件的主要作用是对摄像头采集的图像数据进行预处理和特征提取，为后续的路径规划和车辆控制提供基础数据。它包含了图像的二值化、边界检测、噪声过滤以及中线计算等功能。

1. 图像预处理

Sobel 算子 (image_sobel)：
- 使用 Sobel 算子计算图像的梯度，提取边缘信息。
- 根据梯度值和局部平均值进行二值化处理，生成二值图像。
- 输出结果存储在 binary_image 中，用于后续处理。
自适应阈值 (adaptiveThreshold)：
- 根据局部区域的平均值动态计算阈值，对图像进行二值化。
- 适用于光照变化较大的场景。

2. 边界检测

左手法和右手法 (findline_lefthand_adaptive 和 findline_righthand_adaptive)：
- 使用迷宫搜索算法，从种子点开始沿左侧或右侧边界搜索。
- 搜索结果存储在 b_left 和 b_right 中，分别表示左边界和右边界的点集。
边界转换：
- 将 b_left 和 b_right 中的点集转换为每一行的边界值，存储在 left_line 和 right_line 中。

3. 中线计算

中线计算 (maze_image_predeal)：
- 根据左右边界线计算每一行的中线位置，存储在 mid_line 中。
- 中线用于后续的路径规划和偏差计算。

4. 噪声过滤

二值图像过滤 (Bin_Image_Filter)：
- 对二值图像进行噪声过滤，移除孤立点或小面积噪声。
- 根据周围像素的值判断当前像素是否需要保留。
中值滤波 (medianFilter)：
- 使用中值滤波器平滑图像，减少噪声对边界检测的影响。

5. 边界处理

边界绘制 (image_draw_rectan)：
- 在图像的边界区域绘制黑色矩形，避免边界噪声影响处理结果。

6. 图像预处理流程

迷宫图像预处理 (maze_image_predeal)：
- 调用 image_draw_rectan 绘制边界。
- 使用 GetSeed 提取种子点。
- 调用 findline_lefthand_adaptive 和 findline_righthand_adaptive 搜索左右边界。
- 将边界点集转换为每一行的边界值，并计算中线。

预处理的流程：

图像预处理：
- 对摄像头采集的原始图像进行梯度计算、二值化和噪声过滤，生成清晰的二值图像。
边界检测：
- 使用迷宫搜索算法提取道路的左右边界线。
中线计算：
- 根据左右边界线计算道路的中线，为路径规划和车辆控制提供参考。
噪声处理：
- 通过滤波算法移除图像中的噪声，增强边界检测的鲁棒性。
数据输出：
- 输出左右边界线和中线的坐标数据，供后续模块使用。

图像边界的提取

image_tool.c 文件主要实现了图像处理相关的功能，核心目的是从图像数据中提取道路信息（如中线、左右边界线、拐点等），并为路径规划和车辆控制提供支持。以下是文件的主要功能和实现的详细解释：

文件的主要功能

迷宫算法：通过迷宫搜索算法提取道路的左右边界线或拐点。
拐点检测：根据梯度变化或坐标变化检测道路的拐点。
线段拟合：通过拟合算法判断道路是否为直线。
丢线处理：在丢失边界线时，通过补线算法恢复道路信息。
辅助判断：提供道路类型（如环岛、障碍物）的辅助判断功能。

2. 核心函数解析

(1) maze_find_inflection

功能：使用迷宫算法从起始点开始，沿左侧或右侧边界搜索拐点。
输入参数：
- choose：选择搜索方向（'L' 表示左侧，'R' 表示右侧）。
- StartPoint：起始点坐标。
- endline：搜索的最小行索引。
- num：最大搜索点数。
- Qiedian：输出的拐点坐标。
- thre：梯度变化的阈值。
实现逻辑：
- 使用迷宫搜索算法，沿着图像的二值化边界线进行遍历。
- 通过梯度变化（cal_gradient_Point）检测拐点。
- 如果找到满足条件的拐点，则将其坐标存储到 Qiedian 中。

(2) FillingLine

功能：在丢失边界线时，通过起点和终点之间的直线补全边界线。
输入参数：
- Choose：选择补全的边界线（'L' 表示左边界，'R' 表示右边界）。
- StarPoint 和 EndPoint：起点和终点坐标。
实现逻辑：
- 根据起点和终点计算直线的斜率和截距。
- 遍历起点到终点之间的行，计算每一行的边界线坐标并填充。

(3) GetSeed

功能：从图像中提取种子点，用于后续的边界线搜索。
实现逻辑：
- 在图像的某一行（如第 118 行）中，检测黑白跳变点。
- 根据跳变点的位置，提取左右边界的种子点。

(4) getValidMidRow

功能：计算有效的中线行索引，用于判断道路的有效区域。
实现逻辑：
- 从图像底部向上遍历，检测中线是否有效。
- 使用滑动窗口对中线行索引进行平滑处理。

(5) cal_gradient_Point

功能：计算两点之间的梯度（斜率）。
实现逻辑：
- 根据两点的坐标计算斜率。
- 如果两点的 X 坐标相同，则返回一个极大值（表示垂直线）。

(6) find_straight

功能：判断一段边界线是否为直线。
实现逻辑：
- 使用最小二乘法拟合直线，计算直线的斜率和截距。
- 根据直线与实际点的偏差，判断是否为直线。

(7) EvaluateCircleBoundary

功能：判断当前道路是否为环岛。
实现逻辑：
- 根据左右边界线的变化（如跳变、间距）判断是否为环岛。
- 如果满足环岛条件，则返回 true。

(8) EvaluateBarrierBoundary

功能：判断当前道路是否存在障碍物。
实现逻辑：
- 检测左右边界线的跳变幅度。
- 如果跳变幅度超过阈值，则认为存在障碍物。

(9) UpInflection_Handle_Test

功能：在丢失边界线时，向上搜索拐点并补全边界线。
实现逻辑：
- 从起始点向上搜索黑白跳变点。
- 使用迷宫算法找到拐点，并通过 FillingLine 补全边界线。

3. 文件的整体作用

图像处理：从二值化图像中提取道路的左右边界线和中线。
路径规划：通过拐点检测和直线拟合，为路径规划提供支持。
丢线处理：在边界线丢失时，通过补线算法恢复道路信息。
辅助判断：提供环岛、障碍物等特殊道路场景的判断功能。

image_tool.c 文件是图像处理模块的核心部分，主要通过迷宫搜索、梯度计算和拟合算法提取道路信息，并为车辆的路径规划和控制提供基础数据。它在整个系统中起到了桥梁作用，将图像数据转化为可用于控制的道路信息。

元素识别处理

camera.c 文件的主要功能是处理摄像头采集的图像数据，进行图像预处理、道路特征提取、道路类型识别以及特定道路元素的处理。以下是文件的内容梳理：

1. 文件的主要功能

图像采集与预处理：从摄像头获取原始图像数据，并进行二值化、边界增强等处理。
道路特征提取：提取左右边界线、中线以及其他道路特征。
道路类型识别：识别直道、弯道、环岛、十字路口、障碍物等道路类型。
道路元素处理：根据识别的道路类型，执行相应的处理逻辑。
调试与显示：通过点阵屏或显示屏输出调试信息和图像。

2. 文件的主要模块

(1) 全局变量

图像数据：
- temp_Image：存储摄像头采集的原始图像。
- binary_image 和 black_white_Image：存储二值化后的图像。
道路特征：
- left_line、right_line、mid_line：分别表示左右边界线和中线的坐标。
状态标志：
- straight_flag、bend_flag、S_bend_flag 等：标志当前道路类型。
- Stop_Flag：标志是否需要停车。
道路类型结构体：
- Road_Type_Test：包含各种道路类型的标志（如十字路口、环岛、障碍物等）。

(2) 图像预处理

Camera_Display_Test：
- 核心函数，用于处理摄像头采集的图像。
- 主要步骤：
  1. 调用 Copy_Origin_Image_Test 复制原始图像。
  2. 调用 Local_Dajin 进行二值化处理。
  3. 调用 image_sobel 提取边缘信息。
  4. 调用 Bin_Image_Filter 和 black_white_Image_Filter 进行噪声过滤。
  5. 调用 image_draw_rectan 增强边界条件。
  6. 调用 maze_image_predeal 提取左右边界线和中线。
  7. 调用 getValidMidRow 计算有效中线行。
  8. 判断道路类型（直道或弯道），并计算偏差值。

(3) 道路类型识别

Element_Ack_usingCNT：
- 根据当前道路状态，识别特定的道路类型（如十字路口、环岛、障碍物等）。
- 主要逻辑：
  - 调用 Clarify_Cross、Clarify_Left_Circle、Clarify_Right_Circle 等函数，判断是否为特定道路类型。
  - 更新 Road_Type_Test 中的标志位。
Element_Ack_Test：
- 类似于 Element_Ack_usingCNT，但不依赖 process_count，直接检测道路类型。

(4) 道路元素处理

Element_Handle_Test：
- 根据识别的道路类型，调用相应的处理函数。
- 主要处理函数：
  - Handle_Left_CircleIsland_Test 和 Handle_Right_CircleIsland_Test：处理左环岛和右环岛。
  - Handle_Normal_Cross_Test：处理十字路口。
  - Handle_Left_Barrier_Test 和 Handle_Right_Barrier_Test：处理左侧和右侧障碍物。
  - Handle_Stop_Test：处理停车标志。
  - Handle_S_Bend：处理 S 型弯道。

(5) 道路类型识别与处理的辅助函数

十字路口：
- Clarify_Cross：判断是否为十字路口。
- Handle_Normal_Cross_Test：处理十字路口的逻辑。
- FixCross_Midline：修正十字路口的中线。
- CrossExitAssistant：辅助判断是否离开十字路口。
环岛：
- Clarify_Left_Circle 和 Clarify_Right_Circle：判断是否为左环岛或右环岛。
- Handle_Left_CircleIsland_Test 和 Handle_Right_CircleIsland_Test：处理环岛的逻辑。
障碍物：
- Clarify_Left_Barrier 和 Clarify_Right_Barrier：判断是否存在左侧或右侧障碍物。
- Handle_Left_Barrier_Test 和 Handle_Right_Barrier_Test：处理障碍物的逻辑。
停车：
- Clarify_Stop_Test：判断是否需要停车。
- Handle_Stop_Test：处理停车逻辑。
S 型弯道：
- Clarify_S_Bend：判断是否为 S 型弯道。
- Handle_S_Bend：处理 S 型弯道的逻辑。

(6) 图像二值化

Local_Dajin：
- 使用 Otsu 算法对图像进行二值化。
- 分别对图像的上半部分和下半部分计算阈值，并进行二值化处理。
GetHistGram 和 otsu2dTh：
- 计算图像的直方图，并使用 Otsu 算法计算二值化阈值。
binaryzation：
- 根据阈值对图像进行二值化。

(7) 调试与显示

light_show：
- 根据当前道路状态，通过点阵屏显示调试信息。
- 显示内容包括十字路口、环岛、障碍物等状态。
ips200_displayimage03x：
- 在显示屏上显示二值化图像。
ips200_show_string 和 ips200_show_int：
- 显示调试字符串和数值。

3. 文件的整体作用

图像处理：
- 从摄像头获取原始图像，并进行二值化、边界增强和特征提取。
道路识别：
- 识别直道、弯道、十字路口、环岛、障碍物等道路类型。
道路处理：
- 根据识别的道路类型，执行相应的处理逻辑。
调试与显示：
- 输出调试信息和图像，便于开发和调试。

我们以右环岛为例说明识别逻辑：

右环岛的识别通过函数 Clarify_Right_Circle() 实现，其核心逻辑是基于图像特征（如边界线、梯度变化、黑白区域分布等）判断当前道路是否为右环岛。

识别步骤

边界条件检查：
- 调用 EvaluateCircleBoundary('R', ...) 检查右边界是否满足环岛的基本条件（如边界线的连续性和形状）。
- 如果不满足条件，直接返回 false。
寻找右边界拐点：
- 使用迷宫搜索算法 maze_find_inflection_bythre('R', ...) 从图像中寻找右边界的拐点（right_down）。
- 如果拐点位置不合理（如超出图像范围或过于靠近底部），返回 false。
统计黑白区域分布：
- 在右边界拐点附近统计黑白区域的分布情况，判断是否符合右环岛的特征。
- 如果黑白区域的分布不符合预期，返回 false。
计算边界长度：
- 通过遍历右边界线，计算第一段和第二段的长度（firstlen 和 seclen）。
- 如果两段长度符合右环岛的特征（如第一段较长，第二段较短），返回 true，否则返回 false。

右环岛的处理通过函数 Handle_Right_CircleIsland_Test() 实现，其核心逻辑是基于状态机逐步完成右环岛的处理。

处理步骤

状态机初始化：
- 使用变量 RightCircleIsland_Stage_Test 表示右环岛的当前处理阶段。
- 每个阶段对应不同的处理逻辑。
各阶段处理逻辑：
- 阶段 0
  
  ：检测右边界的丢失行数和丢失点数，寻找右边界的拐点，并补全右边界线。
  - 如果丢失点数超过阈值且时间满足条件，进入下一阶段。
- 阶段 1
  
  ：继续寻找右边界的拐点，并补全右边界线。
  - 如果丢失点数小于阈值且时间满足条件，进入下一阶段。
- 阶段 2
  
  ：检测右边界的上方拐点，并补全右边界线。
  - 如果满足条件（如拐点位置合理），进入下一阶段。
- 阶段 3
  
  ：处理右边界的特殊情况（如大面积丢失），并补全右边界线。
  - 如果满足条件（如时间或边界特征），进入下一阶段。
- 阶段 4
  
  ：检测右边界的最终拐点，并补全右边界线。
  - 如果满足条件，进入下一阶段。
- 阶段 5：完成右环岛的处理，清除右环岛标志位。
状态机结束：
- 当状态机进入最终阶段（阶段 5），清除右环岛标志位，并记录右环岛处理完成的次数。