本项目使用英飞凌TC264双核MCU开发一套智能车赛道循迹控制系统，面向于一张含有二极管，三极管，电阻，电容，开关的电路赛道，通过视觉识别，路口节点检测，姿态解算，串级pid闭环运动控制，在线调整参数与掉点保存进行电路图循迹。采用双核分工架构，充分利用硬件资源，实现视觉高帧处理与运动控制并行处理，1核主要负责摄像头图像采集与上层视觉算法处理，包含自适应阈值求解，图像二值化，赛道边界搜索，中线拟合，赛道误差加权计算，拐点节点识别与路径决策，为整车运动控制提供视觉输入源。2核主要负责底层外设和闭环控制，包含imu陀螺仪解算，编码器速度采样，定时器中断时序调度，双串级pid算法运算，电机差速控制，同时承载按键菜单交互，flash参数存储等调试功能，我主要负责软件。
代码主要包括三个板块，视觉板块，控制板块和调试板块，首先是视觉板块
//使用大津法在指定行获取阈值 uint8_t Otsus_thresholding_method(uint8* mt9v03x_image,uint16 col,uint16_t row_start,uint16_t row_end){//传入行指针参数 uint32_t histogram[256] = {0}; // 灰度直方图 uint32_t pixel_count = 0; // 像素总数 uint32_t pixel_sum = 0; // 灰度总和 //统计灰度直方图 for(uint16_t row = row_start; row < row_end;row++){ for(uint16_t col_index = 0;col_index < col;col_index++){ //接收此像素点的灰度值 uint8_t gray = mt9v03x_image[row*col+col_index]; //统计灰度值个数 histogram[gray]++; //统计像素总数 pixel_count++; //统计灰度综合 pixel_sum+=gray; } } //寻找有效灰度范围，减小计算量 uint8_t min_gray = 0,max_gray = 255; for(min_gray = 0;min_gray < 256 && histogram[min_gray] == 0;min_gray++); for(max_gray = 255;max_gray > min_gray && histogram[max_gray] == 0;max_gray--); if(max_gray == min_gray || min_gray + 1 == max_gray){ return min_gray; } //计算最佳阈值 float max_variance = 0.0f; //先赋最小值 uint8_t best_threshold = min_gray; //背景像素初始化 uint32_t background_count = 0; //背景灰度值初始化 uint32_t background_sum = 0; for(uint8_t threshold = min_gray;threshold < max_gray;threshold++){ background_count += histogram[threshold]; background_sum += histogram[threshold]*threshold; //计算前景像素 uint32_t foreground_count = pixel_count - background_count; //避免除0错误 if(foreground_count == 0){ continue; } //计算前景灰度值 uint32_t foreground_sum = pixel_sum - background_sum; // 计算背景和前景的平均灰度 float background_mean = (float)background_sum / background_count; float foreground_mean = (float)foreground_sum / foreground_count; //类间方差算出前景与背景最大差值 g = n0*n1*(u0*u0 - u1*u1) float variance = (float)foreground_count*background_count*(background_mean - foreground_mean)*(background_mean - foreground_mean); //算出最大方差，记录阈值 if(variance>max_variance){ max_variance = variance; best_threshold = threshold; } } return best_threshold; } //图像二值化 void image_binarization(uint8 (*mt9v03x_image)[COL]){//传入二维数组首元素地址，步长为整行 //大津法自动获取阈值 //若标志位存在则大津法 if(auto_threshold_flag){ //仅首次进入初始化 static uint8 threshold_update_cnt = 0; //每20帧更新一次阈值 if(threshold_update_cnt++ >= 20){ threshold_update_cnt = 0; //分块计算阈值 threshold_far = Otsus_thresholding_method(mt9v03x_image[0],COL,BLOCK_FAR_START, BLOCK_FAR_END);//第一个传参是一维数组首元素的地址，步长为1个元素 threshold_mid = Otsus_thresholding_method(mt9v03x_image[0],COL,BLOCK_MID_START, BLOCK_MID_END); threshold_near = Otsus_thresholding_method(mt9v03x_image[0],COL,BLOCK_NEAR_START, BLOCK_NEAR_END); threshold_far = (threshold_far < THRESHOLD_MIN) ? THRESHOLD_MIN : (threshold_far > THRESHOLD_MAX) ? THRESHOLD_MAX : threshold_far; threshold_mid = (threshold_mid < THRESHOLD_MIN) ? THRESHOLD_MIN : (threshold_mid > THRESHOLD_MAX) ? THRESHOLD_MAX : threshold_mid; threshold_near = (threshold_near < THRESHOLD_MIN) ? THRESHOLD_MIN : (threshold_near > THRESHOLD_MAX) ? THRESHOLD_MAX : threshold_near; } } for(uint16_t row = 0;row <ROW;row++){ //判断阈值区间 if(row < BLOCK_FAR_END){ threshold = threshold_far; }else if(row < BLOCK_MID_END){ threshold = threshold_mid; }else{ threshold = threshold_near; } // } //二值化 for(uint16_t col = 0;col < COL;col++){ bin_image[row][col] = (mt9v03x_image[row][col] > threshold) ? WHITE : BLACK; /* if(bin_image[row][col]){ //记录白像素点数目 white_point++; } */ } } }将一副120*188的灰度图像转化成二值化图像，每个像素点都是一个0-255范围的数值，0代表黑，255代表白，通过比较一个像素阈值，小于该值则为黑，大于该值则为白，所以这个阈值很重要，影响一副图像的质量，确定该阈值我通过大津法来计算，并且每20帧更新一次，大津法原理即先遍历图像每一个像素点，统计每个灰度值出现多少次，统计总像素数和灰度综合，这样就会形成一个灰度直方图，横坐标代表灰度值，纵坐标代表灰度值个数，接下来从最左侧最小灰度值开始，计算背景总像素点和总灰度值以及前景像素点和总灰度值，相除从而得出背景和前景平均灰度值，通过类间方差公式 方差的平方 = 背景像素数量*前景像素数量*（背景平均灰度 - 前景平均灰度）的平方，计算类间方差，方差越大，代表前景背景灰度差异越强，逐个比较灰度直方图每个区间之间的类间方差，寻找出最大的方差并记录此时灰度阈值，即为最优的阈值，通过比较该阈值则可以把灰度图像转换为二值化图像
//搜线 void search_line(){ //左右边线找到标志位 uint8 l_find_flag = 0,r_find_flag = 0; //初始化边线数组 memset(l_border,0,sizeof(l_border)); memset(r_border,COL-1,sizeof(r_border)); memset(l_border_final,0,sizeof(l_border)); memset(r_border_final,COL-1,sizeof(r_border)); memset(mid_line,COL/2,sizeof(mid_line)); //从下往上搜 for(int16 row = ROW - 1;row >=0;row --){ //先找左边线 for(uint8 col = 0;col <= COL - 3;col++){ if(bin_image[row][col] == BLACK && bin_image[row][col+1] == WHITE && bin_image[row][col+2] == WHITE){ l_find_flag = 1; l_border[row] = col; l_border_final[row] = col; break; } } if(!l_find_flag){ l_border[row] = 0; l_border_final[row] = 0; } //找右边线 for(uint8 col = COL - 1;col >= 2;col--){ if(bin_image[row][col] == BLACK && bin_image[row][col-1] == WHITE&& bin_image[row][col-2] == WHITE){ r_find_flag = 1; r_border[row] = col; r_border_final[row] = col; break; } } if(!r_find_flag){ r_border[row] = COL - 1; r_border_final[row] = COL - 1; } //计算中线 mid_line_final[row] = (l_border_final[row]+r_border_final[row])/2; } }接下来根据二值图像搜索左右边线，从下往上逐行扫描，左边线从最左边往最右边检测黑 -> 黑 -> 白跳变点，存储到左边线数组中，右边线同理，从最右边往最左边检测黑 -> 黑 -> 白跳变点，存储到右边线数组中，若整行没有判断到此特征将左右边线值置到边界，不误导后续误差计算，通过（左边线 + 右边线）/2算出中线
void calc_image_error(){ float error = 0 ; static float last_error = 0.0f; float weight_sum = 0.0f; float error_sum = 0.0f; //若边界行存在，只计算80行以下区域权重误差 if(border_flag){ // corner_row = corner_row > ROW-1 ? ROW-1 :corner_row; for(uint8 i = 80;i< ROW;i++){ // draw_line(COL/2,0,COL/2, 80); error_sum += (mid_line_final[i] -COL/2)*image_error_weight[i]; weight_sum += image_error_weight[i]; } //左右直角前瞻靠前，更快转向 }else if(angle_flag_left || angle_flag_right){ for(uint8 i = 0;i< ROW;i++){ error_sum += (mid_line_final[i] -COL/2)*image_error_weight_angle[i]; weight_sum += image_error_weight[i]; } }else{ //正常扫线权重 for(uint8 i = 50;i< ROW;i++){ error_sum += (mid_line_final[i] -COL/2)*image_error_weight[i]; weight_sum += image_error_weight[i]; } } //加权平均 error = error_sum/weight_sum; //互补滤波 error = error*0.9f+last_error*0.1f; last_error = error; error = error < -70.0? -70.0 : (error>70.0 ? 70.0 :error); final_error = error; }根据中线再计算出偏离中线的误差，由于有120行中线值，不能完全依赖一行或者几行的中线值来判定最终的误差值，如果此刻由于光线等原因导致某一行是错误的中线值，计算出来的误差会直接影响车体运动轨迹，所以每一行应该有一个置信度，也就是权重，这样的好处不仅仅能提高系统的鲁棒性，而且还可以跟据车体运动情况调整这个权重值，比如车体运动响应滞后，一部分是因为前瞻太近了，应该让摄像头更倾向远处图像，此时就可以调整远端中线误差的权重值。根据每一行计算的中线值 - 理想中线值 * 权重值，最终在进行加权平均算出最终的误差值，此刻起始普通寻线视觉部分就完成了，但电路赛道多是直角弯，并且线比较细，只有几行会参与误差计算，车体还来不及拐弯就冲出赛道
这种情况就需要进行偏置和拟合边线
//节点判断 void node_judge(){ switch(node_state){ case NODE_NONE: border_flag = 0; if(current_plan->turns[node_count % current_plan -> length] == GO_STRAIGHT){ find_point(ROW-1,65); }else{ find_point(ROW-1,50); } //先检测目前左右是否有拐点 count_paths_on_rect(bin_image); //左右边界有节点再进行拐点检测 if(path_info.left_exist != 0 || path_info.right_exist != 0){ //检测是否有拐点 if(check_point_detected()){ //count_paths_on_rect(bin_image); //执行决策 turn_direction = decide_turn_direction(); if(turn_direction != TURN_NONE){ //改变节点状态 node_state = NODE_DETECTED; //节点数+1 node_count++; } //若没有检测到拐点，将进行补线，检测左右边界是否有白线 }else if(find_best_border_line_at_col(1, bin_image)){ //若存在，将边界点顶底行白线中线偏置，抑制检测不到拐点的情况 //corner_row = find_best_border_line_at_col(1, bin_image); //draw_line(COL/2,0,COL/2,corner_row); border_flag = 1; }else if(find_best_border_line_at_col(COL - 2, bin_image)){ //corner_row = find_best_border_line_at_col(COL - 2, bin_image); //draw_line(COL/2,0,COL/2,corner_row); border_flag = 1; } } break; case NODE_DETECTED: if(!angle_start_locked){ if(turn_direction == TURN_LEFT){ //姿态角清零 yaw_clear_to_zero(); //建立目标姿态角 yaw_targetjudge = -yaw_target; } else if(turn_direction == TURN_RIGHT){ yaw_clear_to_zero(); yaw_targetjudge = yaw_target; } else if(turn_direction == GO_STRAIGHT){ straight_start_dist = total_dis;//记录当前距离 } angle_start_locked = 1; node_state = NODE_TURNING; } break; case NODE_TURNING: //----------------------------------------------------------------直行------------------------------------------------------------------------------------------------------ if(turn_direction == GO_STRAIGHT){ //测试 gpio_set_level(P33_10, 1); mid_bias(); float travel_distance = total_dis - straight_start_dist;//直行行驶距离 //若行驶距离大于阈值且节点数小于3，直行结束 if(travel_distance > 0.15f){ uint8 count = count_paths_on_rect(bin_image);//检测矩形框节点数 if(count < 3){//节点数<3 reset_node(); //测试 gpio_set_level(P33_10, 0); } } } //--------------------------------------------------------------左直角----------------------------------------------------------------------------------------- else if(turn_direction == TURN_LEFT){ //测试 gpio_set_level(P33_10, 1); angle_flag_left = 1; //边线左偏置 left_bias(); if(final_yaw <= yaw_targetjudge){ reset_node(); //左直角标志位清零 angle_flag_left = 0; //测试 gpio_set_level(P33_10, 0); } } //--------------------------------------------------------------右直角----------------------------------------------------------------------------------------- else if(turn_direction == TURN_RIGHT){ //测试 gpio_set_level(P33_10, 1); angle_flag_right = 1; right_bias(); if(final_yaw >= yaw_targetjudge){ reset_node(); //右直角标志位清零 angle_flag_right = 0; gpio_set_level(P33_10, 0); } } break; default: break; } }对当前图像先进行四点矩形框判断，也就是检测图像的上下左右边框处有没有白线，图像自下往上遍历看是否出现黑->白->白跳变点，比如图像最右侧有线则标记右侧边线存在标志位，并对节点数进行统计
若左或右边线存在，则进行角点判断，分别检测左右拐点，自下往上扫线，判断边界数组中前几行变化平稳，之后有几行出现突变值，如果有，记录拐点行数。
如果拐点和边界点都存在，说明现在需要执行左转/右转/直行中的一个策略，我通过一个提前定义好的枚举数组，存放当前赛道每个路口需要行走的路线，并和当前边界点存在情况进行比对，比如左边线存在且当前规划路径是左转，那就判定当前车体需要进行左拐处理
目前已经告诉车体往哪里拐，拐多少也需要进行条件限制，左右拐我利用imu陀螺仪解算出的偏航角作为结束条件，如果当前规划向左拐，则设定目标偏航角为90度，转到设定角度继续执行普通巡线逻辑并等待下一个节点到来，直行我利用编码器数值进行积分，到达指定阈值后继续普通巡线
case NODE_DETECTED: if(!angle_start_locked){ if(turn_direction == TURN_LEFT){ //姿态角清零 yaw_clear_to_zero(); //建立目标姿态角 yaw_targetjudge = -yaw_target; } else if(turn_direction == TURN_RIGHT){ yaw_clear_to_zero(); yaw_targetjudge = yaw_target; } else if(turn_direction == GO_STRAIGHT){ straight_start_dist = total_dis;//记录当前距离 } angle_start_locked = 1; node_state = NODE_TURNING; } break;因为我使用的是摄像头循迹，所以车体严格按照摄像头返回的误差控制行进，所以该“怎么拐”我们只需改变左右边线值则可以控制左右轮差速转向，记录此时的边界行，将边界行以上的区域进行使用线性插值进行偏置（根据传进来的起始坐标和末尾坐标，将这两点中间的值一一映射到这两个坐标的连线上），根据规划路径将节点以上的区域合理偏置（例如左转，将节点行以上的左右边线往左边界偏置，直行将左右边线往中间偏置），到达设定目标阈值，将所有标志位清零进行下一路口处判断
---------------------------------------------------控制部分-----------------------------------------------------
/* * pid.c * * Created on: 2026年3月6日 * Author: gjl66 */ #include "pid.h" #include "wifi.h" static uint8 time_2ms; static uint8 time_5ms; void pid_init(){ time_2ms = 0; time_5ms = 0; //pid1ms中断 pit_ms_init(CCU60_CH0, 1); } //取绝对值 float my_abs(float value){ if(value > 0){ return value; }else{ return -value; } } //速度环参数 float kp1 = 0.0; float ki1 = 0.0; int16 max_speed = 0; int16 min_speed = 0; typedef struct{ //当前pi输出 float current_pwm; //累计pi输出 float cumulative_pwm; //当前偏差 int16 current_error; //上一次误差 int16 pre_error; }SPEED; //创建左右轮速度环中间计算变量结构体指针 SPEED speed_struct_left = { .current_pwm = 0.0f, .cumulative_pwm = 0.0f, .current_error = 0, .pre_error = 0, }; SPEED* speed_left = &speed_struct_left; SPEED speed_struct_right = { .current_pwm = 0.0f, .cumulative_pwm = 0.0f, .current_error = 0, .pre_error = 0, }; SPEED* speed_right = &speed_struct_right; void speed_parameter_init(){ speed_struct_left.cumulative_pwm = 0.0f; speed_struct_left.current_error = 0; speed_struct_left.current_pwm = 0.0f; speed_struct_left.pre_error = 0; speed_struct_right.cumulative_pwm = 0.0f; speed_struct_right.current_error = 0; speed_struct_right.current_pwm = 0.0f; speed_struct_right.pre_error = 0; } float speed_control(int16_t practical_speed,int16_t target_speed,SPEED* speed){ //当前偏差 speed ->current_error = target_speed - practical_speed; //过PI,保存此时刻的误差 speed ->current_pwm = kp1*(speed->current_error - speed->pre_error)+ki1*(speed->current_error); //存上一次偏差 speed->pre_error = speed->current_error; //输出值 speed->cumulative_pwm += speed->current_pwm; //对输出值限幅 speed->cumulative_pwm = speed->cumulative_pwm < min_speed? min_speed : (speed->cumulative_pwm >max_speed ?max_speed : speed->cumulative_pwm); return speed->cumulative_pwm; } //转向环pid参数 float kp11 = 0.0; float kd11 = 0.0; float kp22 = 0.0; float kd22 = 0.0; float gkd = 0.0; float angle_kp11 = 0.0; float angle_kd11 = 0.0; float angle_kp22 = 0.0; float angle_kd22 = 0.0; float final_kp11; float final_kd22; //转向环中间计算变量 typedef struct{ float error1; float error2; float error3; float last_error1; }TURN; TURN turn = {0}; //转向环 int16 turn_control(){ //分段式pd，直角给更快响应 if(angle_flag_left){ final_kp11 = angle_kp11; final_kd22 = angle_kd11; }else if(angle_flag_right){ final_kp11 = angle_kp22; final_kd22 = angle_kd22; }else{ final_kp11 = kp11; final_kd22 = kd22; } turn.error1 = final_error; turn.error2 = imu660rb_gyro_z; turn.error3 = final_kp11*turn.error1 + kd11*(turn.error1 - turn.last_error1)+kp22*turn.error1*my_abs(turn.error1)-(final_kd22*turn.error2)/100; turn.last_error1 = turn.error1; //取整形，因为计算的是编码器目标速度 return (int16)turn.error3; } //左右最终pwm值 int16 left_pwm,right_pwm; int16 error_target,target_speed ; /* 速度决策一些参数 */ uint8 first_speed_flag = 0; uint8 going_speed_flag = 0; //速度决策编码器距离 float judge_speed_dis = 0.0; //左右轮经外环处理后的目标速度 int16 target_speed_l,target_speed_r; //逐飞双串思路，内环2s，外环3s，后面有机会试试三串 void pid_control(){ if(++time_2ms >= 2){ time_2ms = 0; if(!menu_run_flag){//菜单关闭，驱动电机 //软件盲盒，在指定岔路口停车 /* if(node_count >= 44 ){ target_speed = 0; } */ if(angle_flag_left || angle_flag_right){ //外轮差速稍大 target_speed_l = target_speed + (int16)(1.05*error_target); target_speed_r = target_speed - error_target; }else{ target_speed_l = target_speed + error_target; target_speed_r = target_speed - error_target; } left_pwm =(int16)speed_control(encoder_data_1,target_speed_l,speed_left); right_pwm =(int16)speed_control(encoder_data_2,target_speed_r,speed_right); motor_left_set(left_pwm); motor_right_set(right_pwm); } } if(++time_5ms>=3){ time_5ms = 0; error_target = turn_control(); } }采用双串级pid闭环控制，创建两个定时器中断，周期时间分别为1ms和3ms，两个定时器分别对应内环和外环，内环使用增量式pi控制器控制电机达到编码器目标转速，每个电机给相同pwm由于齿轮摩擦等外部因素会导致转速不一致，所以根据编码器值反馈不断修正当前pwm值控制两轮转速一致，使用kp* (当前转速误差 - 上一次转速误差) + ki *（当前转速误差），ki的作用主要调节电机达到目标转速的响应，但过大可能会导致超调，引入kp乘以计算当前误差减去上一次误差抑制超调现象，计算出的最终值累加到pwm中。外环使用位置式pd操作目标编码器转速使得电机实现差速运行，kp*当前摄像头最终误差，kd*陀螺仪角速度（陀螺仪角速度不受任何外界影响，直接反映当前车体转向时的角速度，而摄像头误差变化项可能会由于曝光，噪点等原因导致计算出来的值不准确，所以用陀螺仪角速度作为抑制项好过摄像头误差变化项）将最终计算值+-到左右轮编码器目标转速上，内环不断调整pwm到达目标速度，从而形成一套闭环控制
-----------------------------------------------调试部分-----------------------------------------------------------------
typedef struct{ int16_t current;//当前页码 int16_t next;//下一次页码 int16_t enter;//确认 void (*current_display)();//显示 }menu; menu table_display[45] = { //主目录 {0,1,6,function_image},//image {1,2,21,function_speed},//spedd_ring {2,3,27,function_turn},//turn_ring {3,4,34,function_path},//path {4,5,0,function_reset},//重置路径 {5,0,36,function_fuya},//负压 {6,7,13,function_image_a2},//手动阈值 {7,8,17,function_image_b2},//自动阈值 {8,9,8,function_image_c2},//目标姿态角 {9,10,9,function_image_d2},//二值图像 {10,11,10,function_image_e2},//灰度图像 {11,12,19,function_image_f2},//元素测试 {12,6,0,function_image_g2},//保存退出 {13,14,13,function_image_a3},//远处阈值 {14,15,14,function_image_b3},//中处阈值 {15,16,15,function_image_c3},//近处阈值 {16,13,6,function_image_d3},//保存退出 {17,18,17,function_image_e3},//自动阈值 {18,17,6,function_image_f3},//退出 {19,20,19,function_image_g3},//元素测试 {20,19,6,function_image_h3},//退出 {21,22,21,function_speed_a2},//target_speed {22,23,22,function_speed_b2},//kp {23,24,23,function_speed_c2},//ki {24,25,24,function_speed_d2},//max_speed {25,26,25,function_speed_e2},//min_speed {26,21,0,function_speed_f2},//save {27,28,27,function_turn_a2},//kp11 {28,29,28,function_turn_b2},//kd22 {29,30,29,function_turn_c2},//angle_kp11 {30,31,30,function_turn_d2},//angle_kd11 {31,32,31,function_turn_e2},//angle_kp22 {32,33,32,function_turn_f2},//angle_kd22 {33,27,0,function_turn_g2},//save {34,35,34,function_path_a2},//选择路线 {35,34,0,function_path_b2},//save {36,37,36,function_fuya_a2},//PWM {37,38,37,function_fuya_b2},//ON {38,39,38,function_fuya_c2},//OFF {39,36,0,function_fuya_d2},//save }; void menu_show_114(){ //next键 if(!gpio_get_level(KEY1)){ ips114_clear(); while(!gpio_get_level(KEY1)); function_index = table_display[function_index].next; } //确认键 if(!gpio_get_level(KEY2)){ ips114_clear(); while(!gpio_get_level(KEY2)); //进入到保存参数 if(function_index == 12 ||function_index == 16 ||function_index == 26 ||function_index == 33 || function_index == 35||function_index == 39){ flash_save(); ips114_show_string(20, 60, "save successful!"); system_delay_ms(500); ips114_clear(); } //重置路径 if(function_index == 4){ reset_path(); } //灰度与二值图像切换 if(function_index == 9){ binary_image_flag = 1; }else if(function_index == 10){ binary_image_flag = 0; } //操作负压 if(function_index == 37){ fuya_motor_set(fuya_value); // fuya_motor_set(4200); ips114_clear(); menu_run_flag = 0; function_index = 0; return; }else if(function_index == 38){ fuya_motor_set(0); } if(function_index == 6){//手动阈值 auto_threshold_flag = 0; //阈值限幅 //threshold = (threshold < 90)? 90: (threshold > 200) ? 200: threshold; }else if(function_index == 7){//自动阈值 auto_threshold_flag = 1; } //确认键，跳到下一级菜单 function_index = table_display[function_index].enter; } table_display[function_index].current_display(); }内外环参数，曝光阈值，路径更换等工作需要频繁调试，每次烧录太麻烦，所以引入按键调参菜单，解决反复烧录程序调试参数的问题，支持，支持PID内外环参数，图像阈值，路线选择等参数在线修改，参数修改后可保存Flash,上电自动从页中取值，实现脱机调参
核心采用菜单页表+函数指针的架构。预先定义菜单结构体，包含当前页码，下一页索引，确认跳转索引，页面显示函数指针；构建全局菜单页表数组，数组中每一项对应一个菜单页面，预先固化该页面的跳转逻辑与显示回调函数。全局变量function_index作为当前页码索引，标记当前停留的菜单页码。
封装菜单扫描主函数作为主循环入口，4个按键分工：key1为下一页切换键：按键消抖检测到按下后，读取当前页表内next成员，赋值给全局变量索引，完成同级菜单上下翻页。key2为确定键：消抖后执行页面绑定的确认逻辑，读取页表enter成员，将全局变量索引更新为目标页码，实现进入二级/三级子菜单；当选中保存页面按下确认，调用函数将全部参数写入flash中，key3/key4参数增减，在对应页面的显示函数中调用调参函数，检测增减按键，对目标参数做加减，并增加上下限限幅，防止参数越界，每个页面编写独立显示回调函数，页面函数内部完成屏幕绘制，当前参数数值显示，参数增减逻辑绑定，并在循环末尾执行当前页面显示函数，刷新屏幕内容，新增调参页面只需在页表新增条目并编写对应的显示函数，可快速添加新的可调参数，方便调试
开发项目过程中遇到的问题：
1，路口识别后反复触发：摄像头会连续采集多帧。车经过一个路口时，拐点可能在好几帧中都存在。如果每帧检测到拐点就立即增加节点数，预设路线可能提前走到下一项，表现为“该左转时却直行，转弯方向偶尔不对”
代码采用路口状态机，分为三个状态：
（1），NODE_NONE：检测边线，拐点，并结合预设路径决定方向；确认后才将node_count加1，并切到NODE_DETECTED
（2），NODE_DETECTED:只初始化一次本次动作。转弯时清零yaw并设置目标角度；直行时记录起始里程，切换到NODE_TURNING
（3），NODE_TURNING：暂不按新路口重复决策，只判断本次节点行在哪，动态更新中线偏置，左右达到目标角度后退出；直行超过设定距离后退出，切换状态到NODE_NONE进行下一次路口判断
有效避免了重复判断拐点和节点导致不按规定路径跑的问题
2，起步速度突变：根据“目标速度 - 编码器反馈”计算电机输出。如果程序运行车还是静止时，内环积分项一直在积分累计，起步立刻给最高限幅值，出现起步翘头和轮胎打滑。此时只降低pid增益，可能让正常行驶时的响应也变慢，改进方案是在启动时保存用户设定的最终速度，把目标速度从低值逐步提高。它由10ms定时中断调用，每次增加10，达到设定值后停止递增。假如我目标设为500，按理想调用周期约0.5秒升到目标值。随后速度环将目标速度与编码器反馈比较，分别计算左右轮PWM；转向控制还会在此基础上给左右轮不同的目标速度，从而实现缓启动
