FOC开环调试成功

Robot_V2/Algorithm/DengFOC.cpp

@@ -0,0 +1,109 @@
+#include "DengFOC.h"
+#include "Timer.h"
+#include "systick.h"
+#include <cmath>
+#include <cstdio>
+#include <stdint.h>
+
+constexpr double PI = 3.1415926535897932384626433832795f;
+constexpr float _3PI_2 = 4.71238898038f;
+constexpr u8 PP = 11, DIR = 1;
+// 初始变量及函数定义
+#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))
+
+float _electricalAngle(float shaft_angle) { return shaft_angle * PP; }
+
+/**
+ * @brief 归一化角度
+ *
+ * @param angle
+ * @return float
+ */
+float _normalizeAngle(float angle) {
+	float a = fmod(angle, 2 * PI);
+	// 取余运算可以用于归一化，列出特殊值例子算便知
+	return a >= 0 ? a : (a + 2 * PI);
+}
+/**
+ * @brief 电机控制类构造函数
+ *
+ */
+FOC_Controlroller::FOC_Controlroller() { ; }
+/**
+ * @brief 获取电机 shaft_angle
+ *
+ * @return void
+ */
+void FOC_Controlroller::driver_output(float Ua, float Ub, float Uc) {
+
+	// 计算占空比
+	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 Uq: q轴电压
+ * @param Ud: d轴电压
+ * @param angle_el: 电机轴角度
+ */
+void FOC_Controlroller::setPhaseVoltage(float Uq, float Ud, float angle_el) {
+	Uq = _constrain(Uq, -voltage_power_supply / 2, voltage_power_supply / 2);
+	angle_el = _normalizeAngle(angle_el + zero_electric_angle);
+	// 帕克逆变换
+	Ualpha = -Uq * sin(angle_el);
+	Ubeta = Uq * cos(angle_el);
+
+	// 克拉克逆变换
+	Ua = Ualpha + voltage_power_supply / 2;
+	Ub = (sqrt(3) * Ubeta - Ualpha) / 2 + voltage_power_supply / 2;
+	Uc = (-Ualpha - sqrt(3) * Ubeta) / 2 + voltage_power_supply / 2;
+	driver_output(Ua, Ub, Uc);
+}
+
+/**
+ * @brief 速度开环控制
+ *
+ * @param target_velocity: 目标速度
+ * @return float: q轴电压
+ */
+float FOC_Controlroller::velocity_open_loop(float target_velocity) {
+	now_us = get_system_runtime_us();
+	float dt = (now_us - last_time_us) * 1e-6f;
+	// 限制时间间隔
+	if (dt <= 0.0f || dt > 0.5f)
+		dt = 1e-3f;
+	// 通过时间间隔和目标速度计算需要转动的机械角度，存储在shaft_angle中，在此之前，还需要对轴角度归一化
+	shaft_angle = _normalizeAngle(shaft_angle + target_velocity * dt);
+
+	float uq = limit_voltage; // 设定一个固定的q轴电压
+	setPhaseVoltage(uq, 0, _electricalAngle(shaft_angle));
+	last_time_us = now_us;
+	return uq;
+}
+
+/**
+ * @brief 电机初始化
+ *
+ * @param _PP: 电机极队数
+ * @param _DIR: 电机极性
+ */
+void FOC_Controlroller::DFOC_alignSensor(int _PP, int _DIR) {
+	u8 PP = _PP;
+	u8 DIR = _DIR;
+	setPhaseVoltage(3, 0, _3PI_2);
+	delay_1ms(3000);
+	zero_electric_angle = _electricalAngle(0);
+	setPhaseVoltage(0, 0, _3PI_2);
+	std::printf("0电角度:%f", zero_electric_angle);
+}

Robot_V2/Algorithm/DengFOC.h

@@ -0,0 +1,42 @@
+// 灯哥开源，遵循GNU协议，转载请著名版权！
+// GNU开源协议（GNU General Public License,
+// GPL）是一种自由软件许可协议，保障用户能够自由地使用、研究、分享和修改软件。
+// 该协议的主要特点是，要求任何修改或衍生的作品必须以相同的方式公开发布，即必须开源。此外，该协议也要求在使用或分发软件时，必须保留版权信息和许可协议。GNU开源协议是自由软件基金会（FSF）制定和维护的一种协议，常用于GNU计划的软件和其他自由软件中。
+// 仅在DengFOC官方硬件上测试过，欢迎硬件购买/支持作者，淘宝搜索店铺：灯哥开源
+// 你的支持将是接下来做视频和持续开源的经费，灯哥在这里先谢谢大家了
+// 函数声明
+#ifndef _DENGFOC_H
+#define _DENGFOC_H
+#include <stdint.h>
+class FOC_Controlroller {
+  public:
+	FOC_Controlroller();
+
+	float control_mode = 0;
+	float limit_voltage = 2.0f;
+	float limit_velocity = 0.0f;
+	float target_angle = 0.0f;
+	float target_velocity = 0.0f;
+
+	float velocity_open_loop(float target_velocity);
+	void DFOC_alignSensor(int _PP, int _DIR);
+
+  private:
+	std::u64 last_time_us = 0;
+	std::u64 now_us = 0;
+	float shaft_angle = 0;
+	float Ualpha, Ubeta = 0, Ua = 0, Ub = 0, Uc = 0;
+	float zero_electric_angle = 0;
+	float voltage_power_supply = 12.0f; // 电源电压
+
+	void driver_output(float Ua, float Ub, float Uc);
+	void setPhaseVoltage(float Uq, float Ud, float angle_el);
+};
+
+float _normalizeAngle(float angle);
+void DFOC_Vbus(float power_supply);
+float _electricalAngle(float shaft_angle);
+float DFOC_M0_Angle();
+float serial_motor_target();
+
+#endif