32343
843447835d
- IPS200 display menu with real-time status and parameter settings - Camera image processing and line tracking - IMU attitude solution and PID control - WiFi SPI oscilloscope data transmission
217 lines
7.9 KiB
C
217 lines
7.9 KiB
C
/*********************************************************************************************************************
|
||
* TC264 Opensourec Library (TC264开源库)是一个基于官方 SDK 接口的底层开源库
|
||
* Copyright (c) 2022 SEEKFREE 逐飞科技
|
||
*
|
||
* 本文件是 TC264开源库的一部分
|
||
*
|
||
* TC264开源库 是免费软件
|
||
* 您可以根据自由软件基金会发布的 GPL（GNU General Public License，GNU通用公共许可证）的条款
|
||
* 即 GPL 的第3版（或 GPL3.0），或（根据您的选择）任何更高的版本，重新发布和/或修改它
|
||
*
|
||
* 本开源库的发布是希望它能有所帮助，但不提供任何保证
|
||
* 甚至没有默示的适销性或适用于特定用途的保证
|
||
* 有关更多详情，请参阅 GPL
|
||
*
|
||
* 您应该已经与本开源库一起收到一份 GPL 的副本
|
||
* 如果没有，请访问 <https://www.gnu.org/licenses/>
|
||
*
|
||
* 文件名称 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
|