cbb/Motor_v2
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

更新OTA功能，config独立成文件统一定义

Browse Source 
refactor(build): 更新项目配置和链接器设置
- 修改GD32C103CB的ROM基地址和大小配置
- 更新系统时钟向量表偏移量设置
- 在cproject.yml中添加BIN输出格式支持
- 增加-O3优化级别和代码移除选项
- 更新csolution版本依赖

feat(device): 优化ADC采样和配置管理
- 将DengFOC依赖替换为FOC模块
- 从config.h引入配置常量
- 重命名FILTER_SIZE为ADC_FILTER_SIZE以提高清晰度
- 优化温度和电流采样滤波实现

refactor(algorithm): 简化FOC控制函数接口
- 为angle_control、velocity_control等函数移除不必要的参数
- 统一函数指针类型定义
- 优化电流计算函数的代码格式
- 调整PID控制器初始化参数
This commit is contained in:
cbb
2026-01-04 22:26:22 +08:00
parent abd64da360
commit d0198fdff7
26 changed files with 716 additions and 439 deletions
Download Patch File Download Diff File Expand all files Collapse all files
Robot_V2/Algorithm/DengFOC.cpp → Robot_V2/Algorithm/FOC.cpp
+30 -54
View File
@@ -1,26 +1,15 @@
#include "DengFOC.h"
#include "FOC.h"
#include "Can.h"
#include "Timer.h"
#include "adc.h"
#include "ang_sensor.h"
#include "initial.h"
#include "config.h"
#include "motor.h"
#include "pid.h"
#include <cmath>
#include <gd32c10x.h>
#include <stdint.h>
#include "motor.h"
constexpr double PI = 3.1415926535897932384626433832795f;
constexpr float ANGLE_FACTOR = 25.0f / 360.0f * 2.0f * 3.14159265358979323846f;
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 sqrt3dv2 = 0.86602540378f; // sqrt(3)/2的预计算值
constexpr float zero_electric_angle = 0.0f;
constexpr u16 LUT_SCALE = (LUT_SIZE / (2.0f * PI));
FOC_Controlroller FOC; // FOC控制初始化
extern sampler sampler;
@@ -62,7 +51,7 @@ inline float FOC_Controlroller::cal_Iq_Id(float current_a, float current_b,
sin_el = sin_lut[(u16)(angle_el * LUT_SCALE) & LUT_MASK];
float I_q = I_beta * cos_el - I_alpha * sin_el;
return I_q*1000.0f; // 返回Iq值，乘以1000以转换为mA
return I_q * 1000.0f; // 返回Iq值，乘以1000以转换为mA
}
/**
@@ -123,13 +112,15 @@ void FOC_Controlroller::drv_stop() { Set_3Pwm(0.0f, 0.0f, 0.0f); }
/**
* @brief 电机锁止
*/
void FOC_Controlroller::drv_hold(float duty) {
if (duty > 0.95f) {
duty = 0.95f;
} else if (duty < 0.0f) {
duty = 0.0f;
float FOC_Controlroller::drv_hold() {
float hold_pwm_duty = HOLD_PWM_DUTY;
if (HOLD_PWM_DUTY > 0.95f) {
hold_pwm_duty = 0.95f;
} else if (HOLD_PWM_DUTY < 0.0f) {
hold_pwm_duty = 0.0f;
}
Set_3Pwm(duty, duty, duty);
Set_3Pwm(hold_pwm_duty, hold_pwm_duty, hold_pwm_duty);
return hold_pwm_duty;
}
/**
* @brief 电机恢复
@@ -173,7 +164,8 @@ float FOC_Controlroller::velocity_open_loop() {
setPhaseVoltage(Uq, 0.0f);
// CAN0_transmiter.testfloat_union.f =
cal_Iq_Id(sampler.get_current('U'), sampler.get_current('V'), sampler.get_current('W'));
cal_Iq_Id(sampler.get_current('U'), sampler.get_current('V'),
sampler.get_current('W'));
return limit_voltage;
}
@@ -198,7 +190,7 @@ float FOC_Controlroller::velocity_open_loop() {
* @param target_angle: 目标角度
* @return float: 当前角度
*/
float FOC_Controlroller::angle_control(float target_angle) {
float FOC_Controlroller::angle_control() {
// PID 控制计算
float Uq = angle_loop(target_angle - rt_angle);
@@ -216,45 +208,27 @@ float FOC_Controlroller::angle_control(float target_angle) {
* @param max_velocity: 最大速度限制 (单位: rad/s)
* @return float: 当前角度
*/
float FOC_Controlroller::angle_control_with_velocity_limit(float target_angle) {
float FOC_Controlroller::angle_control_with_velocity_limit() {
// 计算角度误差
float angle_error = target_angle - rt_angle;
// 使用角度PID控制器计算目标速度
float target_velocity = ang_inVelAngloop(angle_error);
// 对目标速度进行限幅
target_velocity = _constrain(target_velocity, -limit_velocity, limit_velocity);
target_velocity =
_constrain(target_velocity, -limit_velocity, limit_velocity);
// 使用速度PID控制器计算最终输出电压
float Uq = vel_inVelAngloop(target_velocity - rt_velocity);
// CAN0_transmiter.testfloat_union.f = target_velocity - rt_velocity;
// 获取电机轴角度
shaft_angle = KTH7823.getcurrent_sensorangle() * DEG2RAD;
// 设置相电压
setPhaseVoltage(Uq, 0.0f);
return shaft_angle;
}
/**
* @brief 角度闭环控制
*
* @param target_angle: 目标角度
* @return float: 当前角度
*/
float FOC_Controlroller::angle_control_NPID(float target_angle) {
static float Kp = 20.0f;
// 控制力矩计算
float Uq = _constrain(Kp * (target_angle - rt_angle), -voltage_power_supply,
voltage_power_supply);
// 获取电机轴角度
shaft_angle = KTH7823.getcurrent_sensorangle() * DEG2RAD;
// 设置相电压
setPhaseVoltage(Uq, 0.0f);
return shaft_angle;
}
@@ -264,22 +238,24 @@ float FOC_Controlroller::angle_control_NPID(float target_angle) {
* @param target_velocity: 目标速度
* @return float: 当前速度
*/
float FOC_Controlroller::velocity_control(float target_velocity) {
float FOC_Controlroller::velocity_control() {
// PID 控制计算
float Uq = vel_loop((target_velocity - rt_velocity)*25);
float Uq = vel_loop((target_velocity - rt_velocity) * 25);
// 获取电机轴角度
shaft_angle = KTH7823.getcurrent_sensorangle() * DEG2RAD;
// 设置相电压
setPhaseVoltage(Uq, 0.0f);
// CAN0_transmiter.testfloat_union.f =
cal_Iq_Id(sampler.get_current('U'), sampler.get_current('V'), sampler.get_current('W'));
cal_Iq_Id(sampler.get_current('U'), sampler.get_current('V'),
sampler.get_current('W'));
return rt_velocity;
}
float FOC_Controlroller::current_control(float target_current) {
float FOC_Controlroller::current_control() {
// PID 控制计算
float Iq = cal_Iq_Id(sampler.get_current('U'), sampler.get_current('V'), sampler.get_current('W'));
float Iq = cal_Iq_Id(sampler.get_current('U'), sampler.get_current('V'),
sampler.get_current('W'));
float Uq = current_loop(target_current - Iq);
CAN0_transmiter.testfloat_union.f = Iq;
// 获取电机轴角度
Robot_V2/Algorithm/DengFOC.h → Robot_V2/Algorithm/FOC.h
+14 -13
View File
@@ -1,6 +1,7 @@
// 函数声明
#ifndef _DENGFOC_H
#define _DENGFOC_H
#ifndef _FOC_H
#define _FOC_H
#include "config.h"
#include "pid.h"
#include <stdint.h>
@@ -14,6 +15,7 @@ class FOC_Controlroller {
float limit_velocity = 0.0f;
float target_angle = 0.0f;
float target_velocity = 0.0f;
float target_current = 0.0f;
float rt_angle = 0.0f;
float rt_velocity = 0.0f;
bool over_current = false;
@@ -23,19 +25,18 @@ class FOC_Controlroller {
float v_safe = voltage_power_supply * 1.05f;
float duty_a = 0, duty_b = 0, duty_c = 0;
float Ua = 0, Ub = 0, Uc = 0;
float velocity_open_loop();
float angle_control(float target_angle);
float angle_control_with_velocity_limit(float target_angle);
float velocity_control(float target_velocity);
float current_control(float target_current);
float angle_control_NPID(float target_angle);
float velocity_open_loop(void);
float angle_control(void);
float angle_control_with_velocity_limit(void);
float velocity_control(void);
float current_control(void);
void update_power(void);
void DFOC_alignSensor(int _PP);
void shut_down(void);
void resume(void);
void drv_stop(void);
void drv_hold(float duty = 0.5f);
float drv_hold(void);
protected:
float shaft_angle = 0;
@@ -50,7 +51,7 @@ class FOC_Controlroller {
PIDController current_loop =
PIDController{0.006f, 0.02f, 0, 100000, voltage_power_supply}; // 12V
PIDController vel_inVelAngloop =
PIDController{0.35f, 5.0f,1e-3, 1000000, voltage_power_supply};
PIDController{0.35f, 5.0f, 1e-3, 1000000, voltage_power_supply};
PIDController ang_inVelAngloop =
PIDController{30.0f, 0.0f, 1e-3, 1000000, 999};
@@ -58,12 +59,12 @@ class FOC_Controlroller {
float cal_Iq_Id(float current_a, float current_b, float current_c);
};
// 定义函数指针类型
typedef float (FOC_Controlroller::*FOCFunctionPtr)(void);
float _normalizeAngle(float angle);
inline float _electricalAngle();
constexpr u16 LUT_SIZE = 1024;
constexpr u16 LUT_MASK = (LUT_SIZE - 1);
__attribute__((aligned(4))) const float sin_lut[LUT_SIZE] = {
0.000000f, 0.006136f, 0.012272f, 0.018407f, 0.024541f, 0.030675f,
0.036807f, 0.042938f, 0.049068f, 0.055195f, 0.061321f, 0.067444f,
Robot_V2/Algorithm/Filter.cpp
+1 -2
View File
@@ -4,14 +4,13 @@
#include <algorithm>
#include <cstring>
#include <stdint.h>
#include "config.h"
extern angle_sensor KTH7823;
extern CAN_transmiter CAN0_transmiter;
extern u32 system_runtime_us;
EncoderAngleKalman Angle_kalman(0, 1.0f, 1e-4f, 0.25f);
EncoderVelocityKalman Vel_kalman(0, 1e-5f, 1e-6f, 0.02f);
constexpr u8 SIZE_FLOAT = sizeof(float);
/*!
\brief 中值滤波器函数