/********************************************************************************************************************* * TC264 Opensourec Library (TC264开源库)是一个基于官方 SDK 接口的底层开源库 * Copyright (c) 2022 SEEKFREE 逐飞科技 * * 本文件是 TC264开源库的一部分 * * TC264开源库 是免费软件 * 您可以根据自由软件基金会发布的 GPL（GNU General Public License，GNU通用公共许可证）的条款 * 即 GPL 的第3版（或 GPL3.0），或（根据您的选择）任何更高的版本，重新发布和/或修改它 * * 本开源库的发布是希望它能有所帮助，但不提供任何保证 * 甚至没有默示的适销性或适用于特定用途的保证 * 有关更多详情，请参阅 GPL * * 您应该已经与本开源库一起收到一份 GPL 的副本 * 如果没有，请访问 * * 文件名称 cpu0_main * 公司名称 成都逐飞科技有限公司 * 版本信息 查看 libraries/doc 文件夹下的 version 文件 版本说明 * 开发环境 ADS v1.10.2 * 目标平台 TC264D * 购买地址 https://seekfree.taobao.com/ * * 修改记录 * 日期 作者 备注 * 2022-09-15 pudding first version * 2025-05-01 移植 整合摄像头+电机+编码器+IMU+显示菜单+电池检测 ********************************************************************************************************************/ #include "zf_common_headfile.h" #include "attitude_solution.h" #include "user_menu.h" #include "user_menu_device.h" #include "user_image_process.h" #include "user_line_process.h" #include "pid_control.h" #pragma section all "cpu0_dsram" // *************************** 硬件引脚定义 *************************** // 编码器定义 (直接使用驱动库中的枚举值) // TIM4编码器 — 硬件固定引脚, 仅支持方向模式 #define ENCODER_1_A (TIM4_ENCODER_CH1_P02_8) #define ENCODER_1_B (TIM4_ENCODER_CH2_P00_9) // TIM6编码器 — 硬件固定引脚, 仅支持方向模式 #define ENCODER_2_A (TIM6_ENCODER_CH1_P20_3) #define ENCODER_2_B (TIM6_ENCODER_CH2_P20_0) // 电机驱动定义 (宏定义在 pid_control.h 中, 此处不再重复) #define MOTOR_PWM_FREQ (17000) // 编码器数据 volatile int16 encoder_data_1 = 0; volatile int16 encoder_data_2 = 0; // IMU校准标志 (定义在 isr.c) extern volatile uint8 gyro_calib_ok; // LED #define LED (P21_5) // 系统运行标志 bool car_running = FALSE; // **************************** 主函数 **************************** int core0_main(void) { clock_init(); debug_init(); // cpu0_dsram 段不会被启动代码自动清零，必须手动初始化 encoder_data_1 = 0; encoder_data_2 = 0; car_running = FALSE; // ==================== 硬件初始化 ==================== // 1. 电池电压检测ADC初始化 adc_init(VOLTAGE_PORT, ADC_12BIT); // 2. 摄像头初始化 (MT9V03X 单摄像头模式) // 初始化前清零摄像头状态与缓冲区，避免 cpu0_dsram 段中的随机值干扰 mt9v03x_finish_flag_1 = 0; mt9v03x_finish_flag_2 = 0; memset(mt9v03x_image_1, 0, sizeof(mt9v03x_image_1)); memset(mt9v03x_image_2, 0, sizeof(mt9v03x_image_2)); system_delay_ms(800); if(mt9v03x_double_init(mt9v03x_1)) { printf("\r\n MT9V03X init failed, retrying..."); for(int retry = 0; retry < 3; retry++) { system_delay_ms(800); if(mt9v03x_double_init(mt9v03x_1) == 0) break; printf("\r\n MT9V03X retry %d/3 failed", retry + 1); } } // 关闭 EXTI CH5 (P15_8)，该引脚同时连接摄像头2 PCLK 和 WiFi INT， // 在单摄像头模式下关闭此中断，避免 WiFi 通信产生虚假中断导致画面撕裂 exti_disable(ERU_CH5_REQ1_P15_8); // 3. 电机驱动GPIO与PWM初始化 gpio_init(MOTOR_DIR1, GPO, GPIO_HIGH, GPO_PUSH_PULL); gpio_init(MOTOR_DIR2, GPO, GPIO_HIGH, GPO_PUSH_PULL); pwm_init(MOTOR_PWM1, MOTOR_PWM_FREQ, 0); pwm_init(MOTOR_PWM2, MOTOR_PWM_FREQ, 0); pwm_init(VACUUM_PWM, MOTOR_PWM_FREQ, 0); /* 真空泵 */ // 4. 编码器初始化 (方向模式: A相计数 + B相判向) encoder_dir_init(TIM4_ENCODER, ENCODER_1_A, ENCODER_1_B); encoder_dir_init(TIM6_ENCODER, ENCODER_2_A, ENCODER_2_B); encoder_clear_count(TIM4_ENCODER); encoder_clear_count(TIM6_ENCODER); // 5. IMU初始化 (IMU660RB，单次初始化) gpio_init(LED, GPO, GPIO_HIGH, GPO_PUSH_PULL); if(imu660rb_init() != 0) { printf("\r\n IMU660RB init failed."); } // 6. 蜂鸣器初始化 gpio_init(BEEP_PIN, GPO, GPIO_LOW, GPO_PUSH_PULL); // 7. 按键初始化 key_init(20); // 7.5. 姿态解算初始化 (四元数复位, 须在PIT定时器启动前完成) attitude_init(); // 7.6. 四轮车速度环与转向环PID初始化 // // 左/右速度环 (内环, 100ms): 各自编码器速度 → 各自PWM基础值 // 输出限幅为死区之外的调节余量，最终输出会叠加 MOTOR_PWM_DEADBAND pid_init(&g_pid_speed_left, 0.0f, 1.0f, 0.0f, SPEED_PID_OUT_MAX, -SPEED_PID_OUT_MAX, 100.0f, 1); /* 增量式 */ pid_init(&g_pid_speed_right, 0.0f, 1.0f, 0.0f, SPEED_PID_OUT_MAX, -SPEED_PID_OUT_MAX, 100.0f, 1); /* 增量式 */ // 转向环 (并行, 帧周期): 摄像头中线偏差 → PWM差速 // P项: 中线偏差驱动转向, D项: gyro_z 阻尼抑制过冲 // 输出限幅 ±2000 (差速PWM) pid_init(&g_pid_steering, 0.0f, 0.0f, 0.0f, 2000.0f, -2000.0f, 0.0f, 0); /* 位置式 */ // 8. PIT定时器初始化 // CCU60_CH0: IMU数据采集 (5ms周期) // CCU61_CH0: 编码器读取 (100ms周期) pit_ms_init(CCU60_CH0, 5); pit_ms_init(CCU61_CH0, 100); // 9. 显示菜单初始化 user_menu_init(); // ==================== 主循环 ==================== cpu_wait_event_ready(); while (TRUE) { /* 陀螺仪校准完成后自动启用速度环与转向环控制 */ if(gyro_calib_ok == 1 && !g_control_enabled) { g_control_enabled = TRUE; printf("\r\n PID control enabled."); } // 摄像头帧同步触发 if(mt9v03x_finish_flag_1) { uint32 t_frame_start; /* 帧处理起始时刻 (us) */ mt9v03x_finish_flag_1 = 0; t_frame_start = system_getval_us(); /* 复制帧到共享内存, 并在 CPU0 内完成图像处理 (二值化+边界+中线) */ memcpy(camera_frame_buffer, (const uint8 *)mt9v03x_image_1, (uint32)(MT9V03X_1_H * MT9V03X_1_W)); /* 大津法二值化 */ int otsu_thr = user_otsu_threshold(camera_frame_buffer, MT9V03X_1_W, MT9V03X_1_H); user_image_binarize(camera_frame_buffer, image_binary_1, MT9V03X_1_W, MT9V03X_1_H, otsu_thr); /* 二值化图像降噪 */ user_image_denoise(image_binary_1, MT9V03X_1_W, MT9V03X_1_H); /* 边界追踪 + 中线提取 */ line_process_run((uint8 *)image_binary_1); /* 转向环: 下半部分加权中线偏差 + 陀螺仪 yaw rate → PWM差速 */ if(g_control_enabled && center_line_num > 0) { float line_dev = line_compute_weighted_deviation( (uint16)(MT9V03X_1_W * 0.5f), (uint16)(MT9V03X_1_H / 2)); cascade_steering_loop(line_dev, g_gyro_z_dps); } // 运行菜单循环 (含按键处理、数据显示、图像显示) user_menu_loop(); // 低电量蜂鸣报警 //low_battery_alarm_check(); // CPU占用率监测: 记录本帧处理耗时 cpu_usage_record_frame(system_getval_us() - t_frame_start); } } } #pragma section all restore