Motor_v2/Robot_V2/Algorithm/DengFOC.cpp

#include "DengFOC.h"
#include "Filter.h"
#include "Timer.h"
#include "adc.h"
#include "ang_sensor.h"
#include "gd32c10x_dbg.h"
#include "pid.h"
#include "systick.h"
#include <cmath>
#include <cstdio>
#include <gd32c10x.h>
#include <stdint.h>

constexpr double PI = 3.1415926535897932384626433832795f;
constexpr float DEG2RAD = 1.0f / 360.0f * 6.28318530718f;
constexpr float RAD2DEG = 360.0f / 6.28318530718f;
constexpr float _1_SQRT3 = 0.57735026919f;
constexpr float _2_SQRT3 = 1.15470053838f;
constexpr float _3PI_2 = 4.71238898038f;
constexpr u8 PP = 11;
constexpr float DIR = -1.0;
constexpr float sqrt3 = 1.73205080757f; // sqrt(3)的预计算值
constexpr float zero_electric_angle = 0.0f;

extern sampler sampler;
extern angle_sensor KTH7823;

CurrentKalman curU_kalman(0.0f, 0.0026f, 4.8f, 0.0001f, 1e-6f, 0.02f);
CurrentKalman curV_kalman(0.0f, 0.0026f, 4.8f, 0.0001f, 1e-6f, 0.02f);
CurrentKalman curW_kalman(0.0f, 0.0026f, 4.8f, 0.0001f, 1e-6f, 0.02f);

// 初始变量及函数定义
#define _constrain(amt, low, high)                                             \
	((amt) < (low) ? (low) : ((amt) > (high) ? (high) : (amt)))
#define constrain(value, min, max, target_min, target_max)                     \
	((value - min) / (max - min) * (target_max - target_min))

/**
 * @brief 归一化角度
 *
 * @param angle
 * @return float
 */
inline float _normalizeAngle(float angle) {
	float a = fmod(angle, 2 * PI);
	// 取余运算可以用于归一化，列出特殊值例子算便知
	return a >= 0 ? a : (a + 2 * PI);
}
inline float _electricalAngle() {
	return _normalizeAngle((float)(DIR * PP) * KTH7823.read_sensorAngle() *
						   -zero_electric_angle);
}
/**
 * @brief 电机控制类构造函数
 *
 */
FOC_Controlroller::FOC_Controlroller() { ; }

/**
 * @brief 设置电机输出
 *
 * @param Uq: q轴电压
 * @param Ud: d轴电压
 * @param angle_el: 电机轴角度
 */
inline void FOC_Controlroller::setPhaseVoltage(float Uq, float Ud,
											   float angle_el) {
	angle_el = _normalizeAngle(angle_el);
	// 帕克逆变换
	Ualpha = -Uq * sin(angle_el);
	Ubeta = Uq * cos(angle_el);

	// 克拉克逆变换
	Ua = Ualpha + voltage_power_supply / 2;
	Ub = (sqrt3 * Ubeta - Ualpha) / 2 + voltage_power_supply / 2;
	Uc = (-Ualpha - sqrt3 * Ubeta) / 2 + voltage_power_supply / 2;
	// 计算占空比
	Ua = constrain(Ua, 0.0f, voltage_power_supply, 0.0f, limit_voltage);
	Ub = constrain(Ub, 0.0f, voltage_power_supply, 0.0f, limit_voltage);
	Uc = constrain(Uc, 0.0f, voltage_power_supply, 0.0f, limit_voltage);
	float dc_a = _constrain(Ua / voltage_power_supply, 0.0f, 1.0f);
	float dc_b = _constrain(Ub / voltage_power_supply, 0.0f, 1.0f);
	float dc_c = _constrain(Uc / voltage_power_supply, 0.0f, 1.0f);
	Set_3Pwm(dc_a, dc_b, dc_c);
	// std::printf("PWM:%f,%f,%f\n", dc_a, dc_b, dc_c);
}

/**
 * @brief 速度开环控制
 *
 * @param target_velocity: 目标速度
 * @return float: q轴电压
 */
float FOC_Controlroller::velocity_open_loop() {
	// 通过时间间隔和目标速度计算需要转动的机械角度，存储在shaft_angle中，在此之前，还需要对轴角度归一化
	// 预计算常量 1e-4 = 100微秒控制周期
	shaft_angle = _normalizeAngle(shaft_angle + target_velocity * 1e-4f);

	// 直接计算电气角度，避免函数调用开销
	float angle_el = shaft_angle * PP + zero_electric_angle;
	angle_el = _normalizeAngle(angle_el);

	// 因为在开环控制中，Ud始终为0，只控制Uq
	const float Uq = voltage_power_supply / 2.0f;
	const float Ud = 0.0f;

	// 帕克逆变换 (直接计算，避免函数调用开销)
	const float sin_el = sin(angle_el);
	const float cos_el = cos(angle_el);
	Ualpha = -Uq * sin_el;
	Ubeta = Uq * cos_el;

	// 克拉克逆变换 (预计算常量值)
	const float v_half = voltage_power_supply * 0.5f; // 电源电压的一半
	Ua = Ualpha + v_half;
	Ub = (sqrt3 * Ubeta - Ualpha) * 0.5f + v_half;
	Uc = (-Ualpha - sqrt3 * Ubeta) * 0.5f + v_half;

	// 计算占空比
	Ua = constrain(Ua, 0.0f, voltage_power_supply, 0.0f, limit_voltage * 2);
	Ub = constrain(Ub, 0.0f, voltage_power_supply, 0.0f, limit_voltage * 2);
	Uc = constrain(Uc, 0.0f, voltage_power_supply, 0.0f, limit_voltage * 2);
	const float dc_a = _constrain(Ua / voltage_power_supply, 0.0f, 1.0f);
	const float dc_b = _constrain(Ub / voltage_power_supply, 0.0f, 1.0f);
	const float dc_c = _constrain(Uc / voltage_power_supply, 0.0f, 1.0f);
	Set_3Pwm(dc_a, dc_b, dc_c);

	return limit_voltage;
}

/**
 * @brief 电机初始化
 *
 * @param _PP: 电机极队数
 */
// void FOC_Controlroller::DFOC_alignSensor(int _PP) {
// 	u8 PP = _PP;
// 	setPhaseVoltage(3, 0, _3PI_2);
// 	delay_1ms(3000);
// 	zero_electric_angle = _electricalAngle();
// 	setPhaseVoltage(0, 0, _3PI_2);
// 	std::printf("0电角度:%f", zero_electric_angle);
// }

void FOC_Controlroller::shut_down() {
	is_shut_down = 100;
	gpio_bit_reset(GPIOB, GPIO_PIN_12);
}

void FOC_Controlroller::resume() {
	over_current = 0;
	temp_error = 0;
	voltage_error = 0;
	is_shut_down = 0;
	gpio_bit_set(GPIOB, GPIO_PIN_12);
}

void FOC_Controlroller::update_power(void) {
	voltage_power_supply = sampler.get_vbus_voltage();
}

/**
 * @brief 角度闭环控制
 *
 * @param target_velocity: 目标速度
 * @return float: q轴电压
 */
float FOC_Controlroller::angle_control(float target_angle) {
	float Sensor_angel = KTH7823.getRawAngle() * DEG2RAD;
	float Kp = 2.0f;

	float Uq = _constrain(Kp * (target_angle - DIR * Sensor_angel) * RAD2DEG,
						  -voltage_power_supply, voltage_power_supply);
	shaft_angle = _normalizeAngle(KTH7823.read_sensorAngle() * DEG2RAD);
	float Ud = 0.0f;
	// 直接计算电气角度，避免函数调用开销
	float angle_el = _normalizeAngle(shaft_angle * PP + zero_electric_angle);

	// 帕克逆变换
	Ualpha = -Uq * sin(angle_el);
	Ubeta = Uq * cos(angle_el);

	// 克拉克逆变换
	Ua = Ualpha + voltage_power_supply / 2;
	Ub = (sqrt3 * Ubeta - Ualpha) / 2 + voltage_power_supply / 2;
	Uc = (-Ualpha - sqrt3 * Ubeta) / 2 + voltage_power_supply / 2;
	// 计算占空比
	Ua = constrain(Ua, 0.0f, voltage_power_supply, 0.0f, limit_voltage);
	Ub = constrain(Ub, 0.0f, voltage_power_supply, 0.0f, limit_voltage);
	Uc = constrain(Uc, 0.0f, voltage_power_supply, 0.0f, limit_voltage);
	float dc_a = _constrain(Ua / voltage_power_supply, 0.0f, 1.0f);
	float dc_b = _constrain(Ub / voltage_power_supply, 0.0f, 1.0f);
	float dc_c = _constrain(Uc / voltage_power_supply, 0.0f, 1.0f);
	Set_3Pwm(dc_a, dc_b, dc_c);
	return Sensor_angel;
}