Motor_v2/Robot_V2/main.cpp

#include "main.h"
#include "Can.h"
#include "FOC.h"
#include "Filter.h"
#include "Timer.h"
#include "adc.h"
#include "ang_sensor.h"
#include "flash.h"
#include "health.h"
#include "initial.h"
#include "systick.h"

// MAIN 调试宏定义
#if MAIN_DEBUG
#define MAIN_DEBUG_PRINT(fmt, ...) printf(fmt, ##__VA_ARGS__)
#else
#define MAIN_DEBUG_PRINT(fmt, ...) ((void)0)
#endif

extern CAN_transmiter CAN0_transmiter;		// CAN 初始化
extern sampler sampler;						// 采样器
extern angle_sensor KTH7823;				// 角度传感器
extern EncoderVelocityKalman Vel_kalman;	// 速度滤波
extern EncoderAngleKalman Angle_kalman;		// 角度滤波
extern ThermalController thermalController; // 热量控制器
extern FOC_Controller FOC;					// FOC 控制
extern FlashEEPROM flashEEP;				// EEPROM 类
extern VersionManager Version;

// 全局版本管理器实例
TaskScheduler scheduler = {0};
u32 motor_runtime_50us = 0; // 电机驱动计算轮次
u32 cnt_test = 0;			// 负载检测

// 函数指针数组，包含各种控制模式对应的函数
static FOCFunctionPtr control_functions[] = {
	&FOC_Controller::drv_hold,		   // 0: 占空比置
	&FOC_Controller::angle_control,	   // 1: 位置闭环控制
	&FOC_Controller::velocity_control, // 2: 速度闭环控制
	&FOC_Controller::
		angle_control_with_velocity_limit, // 3:
										   // 角度闭环控制，带速度限制
	&FOC_Controller::MIT_Mode,			   // 4: MIT 模式
	&FOC_Controller::current_control,	   // 5: 电流闭环控制
	&FOC_Controller::velocity_open_loop,   // 6: 速度开环控制
	&FOC_Controller::PWM_out,		   // 0: 占空比置
};
int main(void) {
	u8 init_status = hardware_init();

	while (init_status) { // 初始化检测
		MAIN_DEBUG_PRINT("初始化失败\r\n");
	}
	OTA_initial(); //  OTA 初始化，A/B 分区处理

	angsens_MG_check(); // 角度传感器磁感应强度测试
	watchdog_init(50);	// 看门狗初始化

	while (1) {
		__disable_irq();
		u32 currentTasks =
			scheduler.taskFlags; // 保存当前任务标志快照以提高效率并确保一致性
		scheduler.taskFlags &= ~currentTasks; // 一次性清除所有已处理的任务标志
		__enable_irq();

		do_task(currentTasks);
	}
	return 0;
}

u8 hardware_init(void) {
	__enable_irq();
	systick_config();  // 系统时钟初始化
	unused_pin_init(); // 未使用引脚初始化
	USART0_init();	   // 调试串口初始化
	User_timer_init(); // 任务 Timer 初始化

	u8 init_status = 0;
	init_status += KTH7823.init(); // 角度传感器初始化
	init_status += sampler.init(); // 采样器初始化

	// EEPROM 初始化 (必须在 CAN 初始化之前，因为 CAN 会从 Flash 读取节点 ID)
	flashEEP.init(); // EEPROM 初始化

	// CAN 初始化 (会先设置默认节点 ID，然后从 Flash 加载已保存的节点 ID)
	init_status += CAN0_transmiter.init(); // CAN 初始化

	init_status += Angle_kalman.init(KTH7823.getAngle()); // 角度滤波初始化
	init_status += Vel_kalman.init(0.0f);				  // 速度滤波初始化
	init_status += FOC.init();							  // FOC 初始化
	return init_status;
}

void do_task(u32 currentTasks) {
	cnt_test++;
	// 按照优先级和重要性处理任务
	if (currentTasks & (1 << TASK_CURRENT_UPDATE)) { // 更新电流采样值
		sampler.update_current();
	}
	if (currentTasks & (1 << TASK_ANGLE_UPDATE)) { // 更新角度传感器值
		KTH7823.update();
		FOC.rt_angle = Angle_kalman.update(KTH7823.getAngle());
	}
	if (currentTasks & (1 << TASK_VELOCITY_UPDATE)) { // 更新速度传感器值
		FOC.rt_velocity = Vel_kalman.update(Angle_kalman.getVelocity());
	}
	if (currentTasks & (1 << TASK_MOTOR_CONTROL)) { // 电机控制任务 (最高优先级)
		motor_runtime_50us++;

		// 快速电压检测（每100us执行一次，响应时间 < 200us）
		FOC.fast_voltage_check();

		// 静态变量用于跟踪上一次的控制模式
		static int last_control_mode = -1;

		// 边界检查，防止数组越界
		if (FOC.control_mode >= 0 &&
			FOC.control_mode < 8) { // 更新为 8 个控制模式

			// 检查控制模式是否发生变化
			if (FOC.control_mode != last_control_mode) {
				// 重置所有 PID 控制器的状态，避免积分项累积
				FOC.vel_loop.reset();
				FOC.angle_loop.reset();
				FOC.current_loop.reset();
				FOC.vel_inVelAngloop.reset();
				FOC.ang_inVelAngloop.reset();
				FOC.ang_inMITloop.reset();
				FOC.vel_inMITloop.reset();
				FOC.current_inMITloop.reset();

				// 更新上次控制模式
				last_control_mode = FOC.control_mode;
			}
			if (!FOC.is_shut_down) {
				// 调用对应的控制函数
				(FOC.*control_functions[FOC.control_mode])();
			} else {
				// 调用对应的控制函数
				FOC.drv_stop();
			}
		}
	}
	if (currentTasks & (1 << TASK_MCU_TEMP_UPDATE)) { // 更新 mcu 温度传感器值
		sampler.update_mcu_temp();
	}
	if (currentTasks & (1 << TASK_DRV_TEMP_UPDATE)) { // 更新电机温度传感器值
		sampler.update_drv_temp();
	}
	if (currentTasks & (1 << TASK_THERMAL_CONTROL)) { // 热量控制
		thermalController.update();
	}
	if (currentTasks & (1 << TASK_MAGNETIC_CHECK)) { // 磁感应强度检测
		angsens_MG_check();
	}
	if (currentTasks & (1 << TASK_VBUS_UPDATE)) { // 更新 vbus 电压值
		power_manage();
	}
	if (currentTasks & (1 << TASK_CAN_TRANSMIT)) {				  // CAN 通信
		CAN0_transmiter.test_u16 = CAN0_transmiter.get_node_id(); // 负载检测
		cnt_test = 0;
		CAN0_transmiter.can0_send_flag = SET;
		CAN0_transmiter.communication_check();
		fwdgt_counter_reload(); // 看门狗复位
	}
}