Motor_v2/Robot_V2/Algorithm/thermal.cpp

#include "thermal.h"
#include "Can.h"
#include "DengFOC.h"
#include "adc.h"

extern sampler sampler;
extern CAN_transmiter CAN0_transmiter;
extern FOC_Controlroller FOC;
#define SQUARE(x) ((x) * (x))
#define max(a, b, c)                                                           \
	((a) > (b) ? ((a) > (c) ? (a) : (c)) : ((b) > (c) ? (b) : (c)))

constexpr u16 PROTECT_TIME = 6000; // 过热保护时间，单位*10ms
u32 ThermalController::update() {
	thermal_value = thermal_value * thermalRate;
	thermal_value +=
		SQUARE(sampler.get_drv_temp()) + SQUARE(sampler.get_mcu_temp());

	if (remainingTime10Ms) { // 当前已经过热停机，正在停机自然散热
		if (thermal_value < thermal_capacity * recoveryRate) {
			remainingTime10Ms = 1; // 可以恢复工作
		}
		if (remainingTime10Ms == 1) { // 过热恢复
			FOC.resume();
			return --remainingTime10Ms;
		}
		return --remainingTime10Ms;
	} else { // 当前没有过热停机
		current_max =
			1000 * max(sampler.get_current('U'), sampler.get_current('V'),
					   sampler.get_current('W')); // 获取电流最大值
		thermal_value +=
			SQUARE(current_max) * gapTimeMs / 100000; // 计算当前电流热积累值
	}
	if (thermal_value > thermal_capacity) {
		FOC.shut_down();
		remainingTime10Ms = PROTECT_TIME; // 过热停机10秒
		return remainingTime10Ms;
	}
	return thermal_value;
}