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

feat(can): 增强CAN通信缓冲区管理机制

Browse Source 
- 实现循环缓冲区队列管理CAN消息接收
- 添加缓冲区满/空状态检查函数
- 实现消息出队列dequeue_rx_message功能
- 在中断处理程序中添加消息入队列机制
- 优化CAN消息处理流程，支持批量处理

refactor(motor): 优化电机控制类结构和安全机制
- 重命名PID控制器参数变量名以提高可读性
- 添加过热关机标志位thermal_shutdown和相关处理函数
- 实现过热关机shut_down_thermal和过热恢复resume_from_thermal功能
- 调整关机逻辑，添加drv_stop调用确保电机完全停止
- 修正电流环控制中的占空比设置，改为单相保持

feat(motor): 实现FOC控制算法改进和零点校准功能
- 实现电机零点校准函数calibrate_zero_position用于位置补偿
- 修正FOC控制器中的电气角度计算，集成KTH7823校准角度补偿
- 新增d轴电流计算功能
- 重构PID参数初始化方式，使用统一的电压限制配置
- 更新SVPWM相电压设置算法，改进角度归一化处理
- 修改缺省扇区输出占空比从0.0f改为0.5f以提高稳定性

fix(control): 修复控制回路方向和频率计算问题
- 修正角度控制回路中的误差计算方向
- 修正开环速度控制中的频率参数使用
- 修正速度环控制中的误差计算方向
- 修正MIT模式下多级PID控制器的参数设置和误差计算

refactor(filter): 修正编码器速度计算算法
- 移除速度计算中的冗余乘法因子2

feat(pid): 扩展PID控制器功能
- 添加reset方法用于重置PID内部状态

chore(build): 更新开发工具版本和ADC配置
- 更新csolution版本至2.13.0
- 调整ADC采样时间配置以优化性能
This commit is contained in:
cbb
2026-03-09 01:35:26 +08:00
parent 9689841148
commit a26dd1cc8e
14 changed files with 514 additions and 313 deletions
Download Patch File Download Diff File Expand all files Collapse all files
Robot_V2/Algorithm/FOC.cpp
+141 -77
View File
@@ -7,6 +7,7 @@
#include "config.h"
#include "motor.h"
#include "pid.h"
#include "systick.h"
#include <arm_math.h>
#include <cmath>
#include <gd32c10x.h>
@@ -24,11 +25,35 @@ extern CurrentKalman Current_kalman;
#define constrain(value, min, max, target_min, target_max) \
((value - min) / (max - min) * (target_max - target_min))
// 校准步骤（代码模板）
void calibrate_zero_position(void) {
// Set_3Pwm(.25, 0, 0); // 起劲
// delay_1ms(1000);
// KTH7823.update();
// // 改进校准算法：获取当前传感器角度并计算补偿值
// float current_sensor_angle = KTH7823.getcurrent_sensorangle();
// float electrical_angle = fmodf(current_sensor_angle * PP, 360.0f); //
// 计算当前电角度
// // 计算需要的补偿值，使得当前位置对应0°电角度
// KTH7823.calibration_angle = -electrical_angle;
// // 对补偿角进行归一化处理，确保在[-180, 180]范围内
// KTH7823.calibration_angle = fmodf(KTH7823.calibration_angle + 180.0f,
// 360.0f); if (KTH7823.calibration_angle < 0) { KTH7823.calibration_angle
// += 360.0f;
// }
// KTH7823.calibration_angle -= 180.0f;
// Set_3Pwm(0, 0, 0); // 停止
}
// 通过Ia,Ib,Ic计算Iq,Id(目前仅输出Iq)
inline float FOC_Controller::cal_Iq_Id(float current_a, float current_b,
float current_c) {
// 计算电气角度
float angle_el = fmod(shaft_angle * PP, 360.0f);
float angle_el = fmod(shaft_angle * PP - KTH7823.calibration_angle, 360.0f);
// Clarke变换 (使用三相电流)
float I_alpha, I_beta;
@@ -41,9 +66,11 @@ inline float FOC_Controller::cal_Iq_Id(float current_a, float current_b,
arm_sin_cos_f32(angle_el, &sin_el, &cos_el);
float I_q, I_d;
I_q = I_beta * cos_el - I_alpha * sin_el;
I_d = I_beta * sin_el + I_alpha * cos_el;
// 返回Iq值，乘以1000以转换为mA
return Current_kalman.update(I_q * 1000 - KE_FACTOR * rt_velocity);
// return I_q * 1000 - KE_FACTOR * rt_velocity;
}
/**
@@ -53,25 +80,16 @@ inline float FOC_Controller::cal_Iq_Id(float current_a, float current_b,
*/
u8 FOC_Controller::init() {
motor_init(); // 电机驱动初始化
vel_loop.set_PID(DEFAULT_vel_loop_p * 100 / voltage_power_supply,
vel_loop_Ki,
vel_loop_Kd); // 设置电机速度控制参数
angle_loop.set_PID(DEFAULT_angle_loop_p * 100 / voltage_power_supply,
100.0f,
0.2f); // 设置电机角度控制参数
current_loop.set_PID(DEFAULT_current_loop_p * 100 / voltage_power_supply,
current_loop_Ki,
current_loop_Kd); // 设置电机电流控制参数
vel_inVelAngloop.set_PID(DEFAULT_vel_angloop_p * 100, vel_angloop_Ki,
vel_angloop_Kd); // 设置电机速度控制参数
ang_inVelAngloop.set_PID(DEFAULT_ang_angloop_p * 100 / voltage_power_supply,
ang_angloop_Ki,
ang_angloop_Kd); // 设置电机角度控制参数
FOC.cur_loop_p = DEFAULT_current_loop_p * 100 / limit_voltage;
FOC.cur_MIT_p = DEFAULT_current_MIT_p * 100 / limit_voltage;
FOC.vel_loop_p = DEFAULT_vel_loop_p * 100 / limit_voltage;
FOC.vel_angloop_p = DEFAULT_vel_angloop_p * 100;
FOC.vel_MIT_p = DEFAULT_vel_MIT_p * 100;
FOC.ang_loop_p = DEFAULT_angle_loop_p * 100 / limit_voltage;
FOC.ang_angloop_p = DEFAULT_angle_angloop_p * 100 / limit_voltage;
FOC.ang_MIT_p = DEFAULT_angle_MIT_p * 100 / limit_voltage;
vel_inMITloop.set_PID(DEFAULT_ang_angloop_p * 100 / voltage_power_supply,
vel_MIT_Ki,
vel_MIT_Kd); // 设置电机角度控制参数
return 0;
}
@@ -126,14 +144,17 @@ inline void FOC_Controller::setPhaseVoltage(float Uq, float Ud) {
}
/**
* @brief 使用SVPWM算法设置电机输出（优化版）
* @brief 使用SVPWM算法设置电机输出
*
* @param Uq: q轴电压
* @param Ud: d轴电压
*/
inline void FOC_Controller::setSVPWM_PhaseVoltage(float Uq, float Ud) {
// 计算电气角度（保持原有角度单位和范围）
float angle_el = fmod(shaft_angle * PP, 360.0f);
// 计算电气角度
float angle_el = shaft_angle * PP + KTH7823.calibration_angle;
// 对电角度进行归一化，确保在[0, 360)范围内
angle_el = fmodf(angle_el, 360.0f);
// 帕克逆变换
float sin_el, cos_el;
@@ -143,7 +164,7 @@ inline void FOC_Controller::setSVPWM_PhaseVoltage(float Uq, float Ud) {
// 计算角度并归一化到 [0, 2π)
float angle = atan2f(Ubeta, Ualpha);
if (angle < 0)
if (angle < 0.0f)
angle += 2.0f * PI;
// 扇区与局部角度
@@ -213,9 +234,9 @@ inline void FOC_Controller::setSVPWM_PhaseVoltage(float Uq, float Ud) {
duty_c = 0.5f * T0 + T1;
break;
default:
duty_a = 0.0f;
duty_b = 0.0f;
duty_c = 0.0f;
duty_a = 0.5f;
duty_b = 0.5f;
duty_c = 0.5f;
break;
}
@@ -224,12 +245,36 @@ inline void FOC_Controller::setSVPWM_PhaseVoltage(float Uq, float Ud) {
}
/**
* @brief 电机关机
* @brief 电机关机（通用关机，不带原因标记）
*
*/
void FOC_Controller::shut_down() {
is_shut_down = 1;
drv_stop();
motor_sleep();
is_shut_down = true;
}
/**
* @brief 过热关机
*
*/
void FOC_Controller::shut_down_thermal() {
drv_stop();
motor_sleep();
is_shut_down = true;
thermal_shutdown = true;
}
/**
* @brief 过热恢复
*
*/
void FOC_Controller::resume_from_thermal() {
thermal_shutdown = false;
if (!voltage_error && !over_current) {
is_shut_down = false;
motor_resume();
}
}
/**
* @brief 占空比置0
@@ -246,7 +291,7 @@ float FOC_Controller::drv_hold() {
} else if (HOLD_PWM_DUTY < 0.0f) {
hold_pwm_duty = 0.0f;
}
Set_3Pwm(hold_pwm_duty, hold_pwm_duty, hold_pwm_duty);
Set_3Pwm(hold_pwm_duty, 0, 0);
return hold_pwm_duty;
}
/**
@@ -283,17 +328,17 @@ void FOC_Controller::update_power(float voltage) {
float FOC_Controller::velocity_open_loop() {
// 通过时间间隔和目标速度计算需要转动的机械角度，存储在shaft_angle中，在此之前，还需要对轴角度归一化
shaft_angle = fmod(shaft_angle + target_velocity * REDUCTION_RATIO * 1.0 /
TIMER2_FREQUENCY,
MOTOR_FREQUENCY / 2,
360.0f);
// 获取电机轴角度
float shaft_angle = KTH7823.getcurrent_sensorangle();
CAN0_transmiter.testfloat_union.f = shaft_angle;
rt_current = cal_Iq_Id(sampler.get_current('U'), sampler.get_current('V'),
sampler.get_current('W'));
// 因为在开环控制中，Ud始终为0，只控制Uq
const float Uq = voltage_power_supply / 2.0f;
setSVPWM_PhaseVoltage(Uq, 0.0f);
// CAN0_transmiter.testfloat_union.f = Uq;
// cal_Iq_Id(sampler.get_current('U'), sampler.get_current('V'),
// sampler.get_current('W'));
return limit_voltage;
}
@@ -305,7 +350,7 @@ float FOC_Controller::velocity_open_loop() {
*/
float FOC_Controller::angle_control() {
// PID 控制计算
float Uq = angle_loop(target_angle - rt_angle);
float Uq = angle_loop(rt_angle - target_angle);
// 获取电机轴角度
shaft_angle = KTH7823.getcurrent_sensorangle();
@@ -316,6 +361,46 @@ float FOC_Controller::angle_control() {
return shaft_angle;
}
/**
* @brief 速度闭环控制
*
* @param target_velocity: 目标速度
* @return float: 当前速度
*/
float FOC_Controller::velocity_control() {
// PID 控制计算
float Uq = vel_loop((target_velocity - rt_velocity) * REDUCTION_RATIO);
CAN0_transmiter.testfloat_union.f = Uq;
// 获取电机轴角度
shaft_angle = KTH7823.getcurrent_sensorangle();
// 设置相电压
setSVPWM_PhaseVoltage(Uq, 0.0f);
rt_current = cal_Iq_Id(sampler.get_current('U'), sampler.get_current('V'),
sampler.get_current('W'));
return rt_velocity;
}
float FOC_Controller::current_control() {
// PID 控制计算
float Iq = rt_current =
cal_Iq_Id(sampler.get_current('U'), sampler.get_current('V'),
sampler.get_current('W'));
float Uq = current_loop(target_current - Iq);
if (Uq > limit_voltage) {
Uq = limit_voltage;
} else if (Uq < -limit_voltage) {
Uq = -limit_voltage;
}
// 获取电机轴角度
shaft_angle = KTH7823.getcurrent_sensorangle();
CAN0_transmiter.testfloat_union.f = shaft_angle;
// 设置相电压
setSVPWM_PhaseVoltage(Uq, 0.0f);
return 1;
}
/**
* @brief 带速度限制的角度闭环控制
*
@@ -327,6 +412,7 @@ float FOC_Controller::angle_control_with_velocity_limit() {
// 计算角度误差
float angle_error = target_angle - rt_angle;
ang_inVelAngloop.set_P(ang_angloop_p);
// 使用角度PID控制器计算目标速度
float target_velocity = ang_inVelAngloop(angle_error);
@@ -339,13 +425,24 @@ float FOC_Controller::angle_control_with_velocity_limit() {
} else {
vel_inVelAngloop.set_P(vel_angloop_p);
}
float Uq = vel_inVelAngloop(rt_velocity - target_velocity);
float Uq = vel_inVelAngloop(target_velocity - rt_velocity);
CAN0_transmiter.testfloat_union.f = Uq;
// // 实现力度控制：如果电流超过目标电流，则限制输出
// if (abs(rt_current) > abs(target_current)) {
// // 当实际电流超过目标电流时，减小输出以限制电流
// // 可以采用比例限制或直接限制输出电压的方式
// float current_ratio = abs(target_current) / abs(rt_current);
// if (current_ratio < 1.0f) {
// Uq *= current_ratio; // 按比例缩小输出电压
// }
// }
rt_current = cal_Iq_Id(sampler.get_current('U'), sampler.get_current('V'),
sampler.get_current('W'));
// 获取电机轴角度
shaft_angle = KTH7823.getcurrent_sensorangle();
// 获取当前实际电流值
rt_current = cal_Iq_Id(sampler.get_current('U'), sampler.get_current('V'),
sampler.get_current('W'));
// 设置相电压
setSVPWM_PhaseVoltage(Uq, 0.0f);
@@ -363,13 +460,13 @@ float FOC_Controller::MIT_Mode() {
// 第一层：角度环（外环）
// 根据目标角度和实际角度的差值，通过角度环PID控制器计算目标速度
float angle_error = target_angle - rt_angle;
ang_inMITloop.set_P(ang_MIT_p);
float target_velocity =
ang_inVelAngloop(angle_error); // 使用angle_loop作为角度到速度的映射
CAN0_transmiter.testfloat_union.f = target_velocity;
ang_inMITloop(angle_error); // 使用angle_loop作为角度到速度的映射
// 第二层：速度环（中环）
// 根据目标速度和实际速度的差值，通过速度环PID控制器计算目标电流
float velocity_error = rt_velocity - target_velocity;
float velocity_error = (rt_velocity - target_velocity);
vel_inMITloop.set_P(vel_MIT_p);
float target_current = vel_inMITloop(velocity_error);
@@ -380,7 +477,8 @@ float FOC_Controller::MIT_Mode() {
// 通过电流环PID控制器计算输出电压
float current_error = target_current - rt_current;
float Uq = current_loop(current_error);
current_inMITloop.set_P(cur_MIT_p);
float Uq = current_inMITloop(current_error);
// 获取电机轴角度
shaft_angle = KTH7823.getcurrent_sensorangle();
@@ -390,37 +488,3 @@ float FOC_Controller::MIT_Mode() {
return shaft_angle;
}
/**
* @brief 速度闭环控制
*
* @param target_velocity: 目标速度
* @return float: 当前速度
*/
float FOC_Controller::velocity_control() {
// PID 控制计算
float Uq = vel_loop((rt_velocity - target_velocity) * 25);
// 获取电机轴角度
shaft_angle = KTH7823.getcurrent_sensorangle();
// 设置相电压
setSVPWM_PhaseVoltage(Uq, 0.0f);
rt_current = cal_Iq_Id(sampler.get_current('U'), sampler.get_current('V'),
sampler.get_current('W'));
return rt_velocity;
}
float FOC_Controller::current_control() {
// PID 控制计算
float Iq = rt_current =
cal_Iq_Id(sampler.get_current('U'), sampler.get_current('V'),
sampler.get_current('W'));
current_loop.set_P(current_loop_p);
float Uq = current_loop(target_current - Iq);
// 获取电机轴角度
shaft_angle = KTH7823.getcurrent_sensorangle();
// 设置相电压
setSVPWM_PhaseVoltage(Uq, 0.0f);
return 1;
}
Robot_V2/Algorithm/FOC.h
+36 -24
View File
@@ -27,20 +27,23 @@ class FOC_Controller {
bool over_current = false;
bool voltage_error = false;
bool is_shut_down = false;
bool thermal_shutdown = false; // 过热关机标志
float voltage_power_supply = 24.0f; // 电源电压
float v_safe = voltage_power_supply * 1.05f;
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 current_loop_p=DEFAULT_current_loop_p;
float cur_loop_p = DEFAULT_current_loop_p;
float vel_loop_p = DEFAULT_vel_loop_p;
float ang_loop_p = DEFAULT_angle_loop_p;
float vel_angloop_p=DEFAULT_vel_angloop_p;
float vel_loop_p=DEFAULT_vel_loop_p;
float vel_angloop_p = DEFAULT_vel_angloop_p;
float ang_angloop_p = DEFAULT_angle_angloop_p;
float ang_angloop_p=DEFAULT_ang_angloop_p;
float angle_loop_p=DEFAULT_angle_loop_p;
float vel_MIT_p=DEFAULT_vel_MIT_p;
float ang_MIT_p = DEFAULT_angle_MIT_p;
float vel_MIT_p = DEFAULT_vel_MIT_p;
float cur_MIT_p = DEFAULT_current_MIT_p;
float velocity_open_loop(void);
float angle_control(void);
@@ -55,24 +58,35 @@ class FOC_Controller {
void resume(void);
void drv_stop(void);
float drv_hold(void);
void shut_down_thermal(void); // 过热关机
void resume_from_thermal(void); // 过热恢复
void setSVPWM_PhaseVoltage(float Uq, float Ud);
// 此处默认值不作数
PIDController vel_loop = PIDController{vel_loop_p * 100, 0.0f, 0, 10000,
voltage_power_supply}; // 12V
PIDController angle_loop = PIDController{angle_loop_p * 100, 0, 0, 10000000,
voltage_power_supply}; // 12V
PIDController current_loop = PIDController{current_loop_p * 100, 0, 0, 1500,
voltage_power_supply}; // 12V
PIDController vel_inVelAngloop =
PIDController{vel_angloop_p * 100, 0, 0, 1e6, v_safe};
PIDController vel_loop =
PIDController{DEFAULT_vel_loop_p * 100 / limit_voltage, vel_loop_Ki,
vel_loop_Kd, 10000, voltage_power_supply}; // 12V
PIDController angle_loop =
PIDController{DEFAULT_angle_loop_p * 100 / limit_voltage, 100.0f, 0.2f,
10000000, voltage_power_supply}; // 12V
PIDController current_loop = PIDController{
DEFAULT_current_loop_p * 100 / limit_voltage, current_loop_Ki,
current_loop_Kd, 1500, voltage_power_supply}; // 12V
PIDController vel_inVelAngloop =
PIDController{DEFAULT_vel_angloop_p * 100, vel_angloop_Ki,
vel_angloop_Kd, 1e6, v_safe};
PIDController ang_inVelAngloop =
PIDController{ang_angloop_p * 100, 0, 0, 1e4, 100};
PIDController{DEFAULT_angle_angloop_p * 100 / voltage_power_supply,
ang_angloop_Ki, ang_angloop_Kd, 1e4, 100};
PIDController ang_inMITloop =
PIDController{ang_angloop_p * 100, 0, 0, 1e4, 100};
PIDController vel_inMITloop =
PIDController{vel_MIT_p * 100, 0, 0, 1e9, 6000};
PIDController{DEFAULT_angle_MIT_p * 100 / limit_voltage, angle_MIT_Ki,
angle_MIT_Kd, 1e5, 100};
PIDController vel_inMITloop = PIDController{
DEFAULT_vel_MIT_p * 100, vel_MIT_Ki, vel_MIT_Kd, 1e9, 1000};
PIDController current_inMITloop = PIDController{
DEFAULT_current_loop_p * 100 / limit_voltage, current_loop_Ki,
current_loop_Kd, 1500, voltage_power_supply}; // 12V
void update_pid_limits() {
vel_loop.limit = voltage_power_supply;
@@ -86,16 +100,14 @@ class FOC_Controller {
float Ualpha, Ubeta = 0;
float zero_electric_angle = 0;
float v_half = voltage_power_supply * 0.5f; // 电源电压的一半
float startup_ramp_val = 0.0f; // 软启动系数
float startup_ramp_step = 0.01f; // 启动斜坡步长，可调节以控制启动速度
void setPhaseVoltage(float Uq, float Ud);
void setSVPWM_PhaseVoltage(float Uq, float Ud);
float cal_Iq_Id(float current_a, float current_b, float current_c);
};
// 定义函数指针类型
typedef float (FOC_Controller::*FOCFunctionPtr)(void);
void calibrate_zero_position(void);
inline float _normalizeAngle(float angle);
inline float _electricalAngle();
Robot_V2/Algorithm/Filter.cpp
+1 -1
View File
@@ -145,7 +145,7 @@ float EncoderAngleKalman::getVelocity() const {
float vel = 0.0f;
if (time_counts > 0) {
vel = (angle_now - velocity_last_angle) * TIMER2_FREQUENCY /
time_counts * 2;
time_counts ;
}
velocity_last_angle = angle_now;
Robot_V2/Algorithm/pid.cpp
+12 -1
View File
@@ -76,4 +76,15 @@ void PIDController::set_P(float pp) { this->P = pp; }
* @param limit: 输出限幅
* @return void
*/
void PIDController::set_limit(float limit) { this->limit = limit; }
void PIDController::set_limit(float limit) { this->limit = limit; }
/**
* @brief 重置PID控制器状态
*
* @return void
*/
void PIDController::reset() {
error_prev = 0.0f;
output_prev = 0.0f;
integral_prev = 0.0f;
}
Robot_V2/RTE/_Debug_GD32C103CB/RTE_Components.h
+1 -1
View File
@@ -1,6 +1,6 @@
/*
* CSOLUTION generated file: DO NOT EDIT!
* Generated by: csolution version 2.12.0+p41-gfeadfc5e
* Generated by: csolution version 2.13.0+p3-g40b6564e
*
* Project: 'Robot_V2.Debug+GD32C103CB'
* Target: 'Debug+GD32C103CB'
Robot_V2/device/ADC.cpp
+1 -1
View File
@@ -81,7 +81,7 @@ void adc_config(void) {
adc_channel_length_config(ADC0, ADC_REGULAR_CHANNEL, 6);
/* 配置ADC规则通道 */
adc_regular_channel_config(ADC0, 0, ADC_CHANNEL_16,
ADC_SAMPLETIME_239POINT5);
ADC_SAMPLETIME_55POINT5);
adc_regular_channel_config(ADC0, 1, ADC_CHANNEL_9, ADC_SAMPLETIME_28POINT5);
adc_regular_channel_config(ADC0, 2, ADC_CHANNEL_3, ADC_SAMPLETIME_28POINT5);
adc_regular_channel_config(ADC0, 3, ADC_CHANNEL_4, ADC_SAMPLETIME_28POINT5);
Robot_V2/device/Can.cpp
+79 -33
View File
@@ -19,8 +19,6 @@
#include <random>
constexpr u16 CANX = CAN0;
CAN_transmiter CAN0_transmiter; // CAN初始化
extern FOC_Controller FOC;
extern sampler sampler;
extern angle_sensor KTH7823;
@@ -35,6 +33,51 @@ dma_parameter_struct dma_tx_parameter;
uint8_t can_rx_buffer[8]; // CAN接收缓冲区
uint8_t can_tx_buffer[8]; // CAN发送缓冲区
CAN_transmiter CAN0_transmiter; // CAN初始化
// CAN接收缓冲区相关函数实现
/*!
\brief 检查CAN接收缓冲区是否已满
\param[in] none
\param[out] none
\retval bool 返回true表示缓冲区已满，false表示未满
\note 通过比较头指针的下一个位置和尾指针来判断缓冲区是否已满
使用循环缓冲区策略，当(rx_buffer_head + 1) %
CAN_RX_BUFFER_SIZE等于rx_buffer_tail时， 表示缓冲区已达到最大容量
*/
bool CAN_transmiter::is_rx_buffer_full() {
return ((rx_buffer_head + 1) % CAN_RX_BUFFER_SIZE) == rx_buffer_tail;
}
/*!
\brief 检查CAN接收缓冲区是否为空
\param[in] none
\param[out] none
\retval bool 返回true表示缓冲区为空，false表示缓冲区不为空
\note 通过比较头指针和尾指针是否相等来判断缓冲区是否为空
*/
bool CAN_transmiter::is_rx_buffer_empty() {
return rx_buffer_head == rx_buffer_tail;
}
/**
* @brief 从接收缓冲区中取出一条CAN消息
*
* 该函数检查接收缓冲区是否为空，如果不为空则取出缓冲区队列头部的消息，
* 并更新队列尾部索引。如果缓冲区为空，则返回false。
*
* @param message 引用类型的输出参数，用于存储取出的CAN接收消息结构体
* @return bool 返回操作是否成功，true表示成功取出消息，false表示缓冲区为空
*/
bool CAN_transmiter::dequeue_rx_message(can_receive_message_struct &message) {
if (!is_rx_buffer_empty()) {
message = rx_message_buffer[rx_buffer_tail];
rx_buffer_tail = (rx_buffer_tail + 1) % CAN_RX_BUFFER_SIZE;
return true;
}
return false;
}
/*!
\brief initialize CAN GPIO
\param[in] none
@@ -521,30 +564,32 @@ void CAN_transmiter::communication_check() {
can0_send_flag = RESET;
}
// CAN0 接收数据标志
if (SET == can0_receive_flag) {
// printf("\r\n can0 %X receive data:", receive_message.rx_efid);
// for (i = 0; i < receive_message.rx_dlen; i++) {
// printf(" %02x", receive_message.rx_data[i]);
// 处理所有接收到的CAN消息（从环形缓冲区中）
can_receive_message_struct current_message;
while (dequeue_rx_message(current_message)) {
// printf("\r\n can0 %X receive data:", current_message.rx_efid);
// for (i = 0; i < current_message.rx_dlen; i++) {
// printf(" %02x", current_message.rx_data[i]);
// }
if (receive_message.rx_efid == CANFD_CMD_ID) {
if (current_message.rx_efid == CANFD_CMD_ID) {
// 控制模式
FOC.control_mode = receive_message.rx_data[0];
FOC.control_mode = current_message.rx_data[0];
// 目标电压限制
FOC.limit_voltage =
float(unpack_bytes_to_u16(receive_message.rx_data, 1)) / 1000;
float(unpack_bytes_to_u16(current_message.rx_data, 1)) / 1000;
// 目标电流
target_current_union.f =
unpack_bytes_to_float(receive_message.rx_data, 3).f;
unpack_bytes_to_float(current_message.rx_data, 3).f;
FOC.current_loop.set_limit(target_current_union.f);
FOC.vel_inMITloop.set_limit(target_current_union.f);
// 目标速度
target_velocity_union.f =
unpack_bytes_to_float(receive_message.rx_data, 7).f;
unpack_bytes_to_float(current_message.rx_data, 7).f;
FOC.ang_inVelAngloop.set_limit(target_velocity_union.f);
FOC.ang_inMITloop.set_limit(target_velocity_union.f);
// 目标角度
target_angle_union.f =
unpack_bytes_to_float(receive_message.rx_data, 11).f;
unpack_bytes_to_float(current_message.rx_data, 11).f;
// 赋值
FOC.target_angle = target_angle_union.f;
FOC.target_velocity = target_velocity_union.f;
@@ -553,26 +598,26 @@ void CAN_transmiter::communication_check() {
// 解析PID参数
float voltage = FOC.limit_voltage;
// PID参数
u16 P_config = unpack_bytes_to_u16(receive_message.rx_data, 15);
FOC.current_loop_p = DEFAULT_current_loop_p * P_config / voltage;
u16 P_config = unpack_bytes_to_u16(current_message.rx_data, 15);
FOC.cur_loop_p = DEFAULT_current_loop_p * P_config / voltage;
FOC.cur_MIT_p = DEFAULT_current_MIT_p * P_config / voltage;
P_config = unpack_bytes_to_u16(receive_message.rx_data, 17);
P_config = unpack_bytes_to_u16(current_message.rx_data, 17);
FOC.vel_loop_p = DEFAULT_vel_loop_p * P_config / voltage;
P_config = unpack_bytes_to_u16(receive_message.rx_data, 19);
FOC.angle_loop_p = DEFAULT_angle_loop_p * P_config / voltage;
FOC.ang_angloop_p = DEFAULT_ang_angloop_p * P_config / voltage;
P_config = unpack_bytes_to_u16(receive_message.rx_data, 21);
FOC.vel_angloop_p = DEFAULT_vel_angloop_p * P_config;
FOC.vel_MIT_p = DEFAULT_vel_MIT_p * P_config;
} else if (receive_message.rx_efid == CANFD_RESET_ID) {
u8 first_byte = receive_message.rx_data[0];
u8 eigth_byte = receive_message.rx_data[7];
P_config = unpack_bytes_to_u16(current_message.rx_data, 19);
FOC.ang_loop_p = DEFAULT_angle_loop_p * P_config / voltage;
FOC.ang_angloop_p = DEFAULT_angle_angloop_p * P_config / voltage;
FOC.ang_MIT_p = DEFAULT_angle_MIT_p * P_config / voltage;
} else if (current_message.rx_efid == CANFD_RESET_ID) {
u8 first_byte = current_message.rx_data[0];
u8 eigth_byte = current_message.rx_data[7];
// 电机重置
if (first_byte & 0x01) {
motor_reset();
testfloat_union.f = KTH7823.getcurrent_sensorangle();
}
// EEPROM重置
if (first_byte >> 1 & 0x01) {
@@ -593,16 +638,17 @@ void CAN_transmiter::communication_check() {
app_prepare_deep_sleep(true);
// CAN_Sleep();
}
} else if (receive_message.rx_efid == CANFD_VERSION_ID) {
} else if (current_message.rx_efid == CANFD_VERSION_ID) {
// 处理版本号请求，发送当前系统版本信息
Version.sendVersionInfoViaCan();
} else if (receive_message.rx_efid == CANFD_BOOT_ID) {
} else if (current_message.rx_efid == CANFD_BOOT_ID) {
motor_sleep();
flashEEP.save_system_config(false);
fmc_unlock();
OTA_SET(n_APP_LATEST_EN_ADDR, OTA_NOT_AVAL); // 设置APP为不激活
fmc_lock();
jumpToBOOT();
} else if (receive_message.rx_efid == 0x00000102) {
} else if (current_message.rx_efid == 0x00000102) {
volatile u32 *flash_ptr = (volatile u32 *)n_APP_LATEST_EN_ADDR;
u32 app_enable = *flash_ptr;
flash_ptr = (volatile u32 *)n_APP_LATEST_CHECK_ADDR;
@@ -614,9 +660,8 @@ void CAN_transmiter::communication_check() {
CANFD_AsyncSend(CANFD_OTA_ACK_ID, 2, transmit_data);
}
} else {
printf("Unknown CAN ID: %X\n", receive_message.rx_efid);
printf("Unknown CAN ID: %X\n", current_message.rx_efid);
}
can0_receive_flag = RESET;
}
}
@@ -662,10 +707,11 @@ void CAN0_RX0_IRQHandler(void) {
dma_channel_enable(DMA0, DMA_CH4);
// 读取接收到的数据
can_message_receive(CAN0, CAN_FIFO0, &CAN0_transmiter.receive_message);
can_receive_message_struct temp_message;
can_message_receive(CAN0, CAN_FIFO0, &temp_message);
// 接收标志位置1
CAN0_transmiter.can0_receive_flag = SET;
// 将接收到的消息加入环形缓冲区
CAN0_transmiter.enqueue_rx_message_from_isr(temp_message);
}
// 检查其他可能的中断源
Robot_V2/device/Can.h
+19 -1
View File
@@ -71,6 +71,8 @@ void can_loopback_test(void);
class CAN_transmiter {
private:
static const uint8_t CAN_RX_BUFFER_SIZE = 8; // CAN接收缓冲区大小
bool is_sleep = false; // 是否休眠
u8 Target_Control_Mode = 0; // 目标控制模式
@@ -100,16 +102,23 @@ class CAN_transmiter {
u8 transmit_mailbox = CAN_NOMAILBOX; // 初始化为无mailbox状态
u8 transmit_data[64]; // CANFD最大64字节
// CAN接收消息队列
can_receive_message_struct rx_message_buffer[CAN_RX_BUFFER_SIZE];
volatile uint8_t rx_buffer_head = 0;
volatile uint8_t rx_buffer_tail = 0;
void update(); // 更新数据
u8 CAN_Receive(); // CAN接收
u8 CANFD_Receive(); // CANFD接收
bool is_rx_buffer_full();
bool is_rx_buffer_empty();
bool dequeue_rx_message(can_receive_message_struct& message);
public:
// 添加构造函数声明
u8 init(void);
can_receive_message_struct receive_message; // 接收消息结构体
FlagStatus can0_receive_flag; // CAN0接收标志
FlagStatus can0_send_flag; // CAN0发送标志
float_union_t testfloat_union = {0}; // 测试用
u16 test_u16 = 0;
@@ -123,6 +132,15 @@ class CAN_transmiter {
u8 CANFD_AsyncSend(u32 ID, u8 length, u8 *data); // CANFD异步发送(使用DMA)
u8 CAN_all_Send(); // CAN发送
u8 CANFD_all_Send(); // CANFD发送
// 中断处理函数需要访问的公共接口
void enqueue_rx_message_from_isr(const can_receive_message_struct& message) {
if (!is_rx_buffer_full()) {
rx_message_buffer[rx_buffer_head] = message;
rx_buffer_head = (rx_buffer_head + 1) % CAN_RX_BUFFER_SIZE;
}
// 如果缓冲区满，丢弃新消息
}
};
#ifdef __cplusplus
Robot_V2/device/Timer.cpp
+122 -114
View File
@@ -9,7 +9,7 @@
#include "gd32c10x_timer.h"
#include "initial.h"
volatile u32 total_us100_counter = 0; // 总计数器，每100微秒增加1
volatile u32 total_us100_counter = 0; // 总计数器，每 100 微秒增加 1
extern u16 adc_value[7];
extern FOC_Controller FOC;
extern TaskScheduler scheduler;
@@ -17,25 +17,56 @@ extern CAN_transmiter CAN0_transmiter;
// 添加系统运行时间计数器
extern u32 cnt_test;
// 任务调度查找表 - 每 100us 执行的任务 (1ms 周期内)
static const u32 task_schedule_100us[10] = {
(1u << TASK_MCU_TEMP_UPDATE), // 0: MCU 温度更新
(1u << TASK_DRV_TEMP_UPDATE), // 1: 驱动温度更新
(1u << TASK_VBUS_UPDATE), // 2: 电压更新
0, // 3: 空闲
0, // 4: 空闲
0, // 5: 空闲
0, // 6: 空闲
0, // 7: 空闲
0, // 8: 空闲
(1u << TASK_CAN_TRANSMIT) // 9: CAN 发送
};
// 任务调度查找表 - 每 1ms 执行的任务 (10ms 周期内)
static const u32 task_schedule_1ms[10] = {
(1u << TASK_THERMAL_CONTROL), // 0: 热量控制
(1u << TASK_MAGNETIC_CHECK), // 1: 磁感应强度检测
0, 0, 0, 0, 0, 0, 0, 0 // 空闲
};
// 任务调度查找表 - 每 10ms 执行的任务 (100ms 周期内)
static const u32 task_schedule_10ms[10] = {
0, 0, 0, 0, 0, 0, 0, 0, 0, 0 // 当前无任务，预留扩展
};
// 任务调度查找表 - 每 100ms 执行的任务 (1s 周期内)
static const u32 task_schedule_100ms[10] = {
0, 0, 0, 0, 0, 0, 0, 0, 0, 0 // 当前无任务，预留扩展
};
/**
* @brief 设置引脚中断优先级
* @return void
*
*设置引脚中断优先级
* 设置引脚中断优先级
*/
void nvic_config(void) {
/* 设置NVIC优先级分组，1位抢占优先级，3位响应优先级 */
/* 设置 NVIC 优先级分组，1 位抢占优先级，3 位响应优先级 */
nvic_priority_group_set(NVIC_PRIGROUP_PRE2_SUB2);
/* 使能TIMER2中断，设置抢占优先级为1，子优先级为1 */
/* 使能 TIMER2 中断，设置抢占优先级为 1，子优先级为 1 */
nvic_irq_enable(TIMER2_IRQn, 0, 1);
nvic_irq_enable(TIMER1_IRQn, 0, 2);
}
/**
* @brief 配置定时器2-任务管理
* @brief 配置定时器 2-任务管理
* @return void
*
*配置定时器2，用于定时执行FOC控制、采样、CAN发送等任务
* 配置定时器 2，用于定时执行 FOC 控制、采样、CAN 发送等任务
*/
void TIMER2_config(void) {
/* ----------------------------------------------------------------------------
@@ -46,21 +77,21 @@ void TIMER2_config(void) {
*/
timer_parameter_struct timer_struct;
/* 使能TIMER2时钟 */
/* 使能 TIMER2 时钟 */
rcu_periph_clock_enable(RCU_TIMER2);
/* 复位TIMER2外设到默认状态 */
/* 复位 TIMER2 外设到默认状态 */
timer_deinit(TIMER2);
/* 初始化TIMER初始化参数结构体 */
/* 初始化 TIMER 初始化参数结构体 */
timer_struct_para_init(&timer_struct);
/* TIMER2配置参数设置 */
/* TIMER2 配置参数设置 */
timer_struct.prescaler =
TIMER2_PRESCALER; /* 预分频值：120分频(从0开始计数) */
TIMER2_PRESCALER; /* 预分频值：120 分频 (从 0 开始计数) */
timer_struct.alignedmode = TIMER_COUNTER_EDGE; /* 边沿对齐模式 */
timer_struct.counterdirection = TIMER_COUNTER_UP; /* 向上计数模式 */
timer_struct.period =
TIMER2_PERIOD; /* 自动重装载值：100次计数(从0开始计数) */
timer_struct.clockdivision = TIMER_CKDIV_DIV1; /* 时钟分频值：1分频 */
TIMER2_PERIOD; /* 自动重装载值：100 次计数 (从 0 开始计数) */
timer_struct.clockdivision = TIMER_CKDIV_DIV1; /* 时钟分频值：1 分频 */
timer_init(TIMER2, &timer_struct);
/* 清除更新中断标志位 */
@@ -68,108 +99,85 @@ void TIMER2_config(void) {
/* 使能更新中断 */
timer_interrupt_enable(TIMER2, TIMER_INT_UP);
/* 使能TIMER2 */
/* 使能 TIMER2 */
timer_enable(TIMER2);
}
/**
* @brief 配置TIMER2-任务管理中断
* @brief 配置 TIMER2-任务管理中断
* @return void
*/
void User_timer_init(void) {
/* 配置TIMER2 */
/* 配置 TIMER2 */
TIMER2_config();
/* 配置TIMER2中断 */
/* 配置 TIMER2 中断 */
nvic_config();
}
/**
* @brief TIMER2-任务管理中断处理函数
* @brief TIMER2-任务管理中断处理函数 (优化版)
* @return void
* @note 使用查找表替代多层 switch-case，减少代码复杂度和执行时间
* 中断周期：50us
* - 每 50us: 电机控制任务 (角度、速度更新)
* - 每 100us: 传感器采样、CAN 通信
* - 每 1ms: 热量控制、磁检
* - 每 10ms/100ms/1s: 预留扩展任务
*/
void TIMER2_IRQHandler(void) {
// 检查更新中断标志并处理
if (timer_interrupt_flag_get(TIMER2, TIMER_INT_UP) != RESET) {
// 清除更新中断标志
timer_interrupt_flag_clear(TIMER2, TIMER_INT_UP);
// 快速路径：检查并清除中断标志
if (timer_interrupt_flag_get(TIMER2, TIMER_INT_UP) == RESET) {
return;
}
timer_interrupt_flag_clear(TIMER2, TIMER_INT_UP);
// 每50us执行一次的操作 - 电机控制
scheduler.taskFlags |=
(1 << TASK_ANGLE_UPDATE) | (1 << TASK_VELOCITY_UPDATE);
// 固定任务：每 50us 执行电机控制
scheduler.taskFlags |= (1u << TASK_ANGLE_UPDATE) |
(1u << TASK_VELOCITY_UPDATE) |
(1u << TASK_MOTOR_CONTROL);
// 使用标志位控制，每100us执行一次其他任务
static bool isEvenCall = false;
isEvenCall = !isEvenCall;
if (isEvenCall) {
// 每100us执行一次的操作 - 保持原有频率
scheduler.taskFlags |= (1 << TASK_MOTOR_CONTROL);
switch (total_us100_counter) { // 每1ms执行的代码，负载均衡
case 0: {
scheduler.taskFlags |= (1 << TASK_MCU_TEMP_UPDATE);
break;
}
case 1: {
scheduler.taskFlags |= (1 << TASK_DRV_TEMP_UPDATE);
break;
}
case 2: {
scheduler.taskFlags |= (1 << TASK_VBUS_UPDATE);
break;
}
case 3: {
break;
}
case 9: {
scheduler.taskFlags |= (1 << TASK_CAN_TRANSMIT);
break;
}
}
total_us100_counter++;
if (total_us100_counter > 9) {
total_us100_counter = 0;
// 可以添加每1ms执行的代码，无负载均衡
static u8 ms1_counter = 0;
switch (ms1_counter) { // 每10ms执行的代码，负载均衡
case 0: {
scheduler.taskFlags |= (1 << TASK_THERMAL_CONTROL);
break;
}
case 1: {
scheduler.taskFlags |= (1 << TASK_MAGNETIC_CHECK);
break;
}
case 2: {
break;
}
}
ms1_counter++;
if (ms1_counter > 9) {
ms1_counter = 0;
// 可以添加每10ms执行的代码，无负载均衡
static u8 ms10_counter = 0;
ms10_counter++;
if (ms10_counter > 9) {
ms10_counter = 0;
// 可以添加每100ms执行的代码
static u8 sec_counter = 0;
sec_counter++;
if (sec_counter > 9) {
sec_counter = 0;
// 可以添加每1s执行的代码
}
}
}
}
}
}
// 每 100us 执行一次的任务（使用位运算替代布尔变量）
static u32 call_counter = 0;
call_counter++;
if ((call_counter & 1u) == 0u) { // 偶数次调用（每 100us）
// 使用查找表设置 1ms 周期内的任务
scheduler.taskFlags |= task_schedule_100us[total_us100_counter];
total_us100_counter++;
if (total_us100_counter >= 10u) {
total_us100_counter = 0;
// 每 1ms 执行的任务
static u32 ms1_counter = 0;
ms1_counter++;
if (ms1_counter >= 10u) {
ms1_counter = 0;
// 每 10ms 执行的任务
static u32 ms10_counter = 0;
ms10_counter++;
if (ms10_counter >= 10u) {
ms10_counter = 0;
// 每 100ms 执行的任务
static u32 ms100_counter = 0;
ms100_counter++;
if (ms100_counter >= 10u) {
ms100_counter = 0;
// 每 1s 任务预留位置
}
scheduler.taskFlags |= task_schedule_100ms[ms100_counter];
}
scheduler.taskFlags |= task_schedule_10ms[ms10_counter];
}
scheduler.taskFlags |= task_schedule_1ms[ms1_counter];
}
}
}
/**
* @brief TIMER1-PWM GPIO配置函数
* @brief TIMER1-PWM GPIO 配置函数
*/
void timer1_gpio_config(void) {
// 时钟使能
@@ -178,7 +186,7 @@ void timer1_gpio_config(void) {
rcu_periph_clock_enable(RCU_AF);
rcu_periph_clock_enable(RCU_TIMER1);
// TIMER1 GPIO初始化
// TIMER1 GPIO 初始化
gpio_init(GPIOA, GPIO_MODE_AF_PP, GPIO_OSPEED_50MHZ, GPIO_PIN_0);
gpio_init(GPIOA, GPIO_MODE_AF_PP, GPIO_OSPEED_50MHZ, GPIO_PIN_1);
gpio_init(GPIOA, GPIO_MODE_AF_PP, GPIO_OSPEED_50MHZ, GPIO_PIN_2);
@@ -188,7 +196,7 @@ void timer1_gpio_config(void) {
gpio_bit_set(GPIOB, GPIO_PIN_0);
gpio_bit_set(GPIOB, GPIO_PIN_12);
/* 配置TIMER1完全重映射*/
/* 配置 TIMER1 完全重映射*/
gpio_pin_remap_config(GPIO_TIMER1_FULL_REMAP, DISABLE);
}
@@ -199,13 +207,13 @@ void timer1_pwm_config(void) {
timer_parameter_struct timer_initpara;
timer_oc_parameter_struct timer_ocinitpara;
// 使能TIMER1时钟
// 使能 TIMER1 时钟
rcu_periph_clock_enable(RCU_TIMER1);
// 复位TIMER1
// 复位 TIMER1
timer_deinit(TIMER1);
// 配置TIMER1基本参数
// 配置 TIMER1 基本参数
timer_struct_para_init(&timer_initpara);
timer_initpara.prescaler = TIMER1_PRESCALER; // 预分频器
timer_initpara.counterdirection = TIMER_COUNTER_UP; // 向上计数
@@ -218,23 +226,23 @@ void timer1_pwm_config(void) {
timer_ocinitpara.outputstate = TIMER_CCX_ENABLE;
timer_ocinitpara.ocpolarity = TIMER_OC_POLARITY_HIGH;
// 配置三个通道为PWM模式
// 配置三个通道为 PWM 模式
timer_channel_output_config(TIMER1, TIMER_CH_0, &timer_ocinitpara);
timer_channel_output_config(TIMER1, TIMER_CH_1, &timer_ocinitpara);
timer_channel_output_config(TIMER1, TIMER_CH_2, &timer_ocinitpara);
// 配置第四个通道(CH3)用于在PWM周期x%位置触发中断
// 配置第四个通道 (CH3) 用于在 PWM 周期 x% 位置触发中断
timer_ocinitpara.outputstate =
TIMER_CCX_DISABLE; // 不输出PWM波形，仅用于触发中断
TIMER_CCX_DISABLE; // 不输出 PWM 波形，仅用于触发中断
timer_ocinitpara.ocpolarity = TIMER_OC_POLARITY_HIGH;
timer_channel_output_config(TIMER1, TIMER_CH_3, &timer_ocinitpara);
// 设置PWM模式和占空比
// 设置 PWM 模式和占空比
timer_channel_output_mode_config(TIMER1, TIMER_CH_0, TIMER_OC_MODE_PWM0);
timer_channel_output_mode_config(TIMER1, TIMER_CH_1, TIMER_OC_MODE_PWM0);
timer_channel_output_mode_config(TIMER1, TIMER_CH_2, TIMER_OC_MODE_PWM0);
timer_channel_output_mode_config(TIMER1, TIMER_CH_3,
TIMER_OC_MODE_PWM0); // CH3也设为PWM模式
TIMER_OC_MODE_PWM0); // CH3 也设为 PWM 模式
timer_channel_output_pulse_value_config(TIMER1, TIMER_CH_0,
TIMER1_DEFAULT_PULSE);
@@ -242,7 +250,7 @@ void timer1_pwm_config(void) {
TIMER1_DEFAULT_PULSE);
timer_channel_output_pulse_value_config(TIMER1, TIMER_CH_2,
TIMER1_DEFAULT_PULSE);
// 设置CH3比较值为PWM周期x%位置
// 设置 CH3 比较值为 PWM 周期 x% 位置
timer_channel_output_pulse_value_config(TIMER1, TIMER_CH_3,
TIMER1_CH3_COMPARE_VALUE);
@@ -250,26 +258,26 @@ void timer1_pwm_config(void) {
timer_primary_output_config(TIMER1, ENABLE);
timer_auto_reload_shadow_enable(TIMER1);
// 使能CH3比较中断
// 使能 CH3 比较中断
timer_interrupt_enable(TIMER1, TIMER_INT_CH3);
// 启动TIMER1
// 启动 TIMER1
timer_enable(TIMER1);
}
// 添加TIMER1-PWM中断处理函数
// 添加 TIMER1-PWM 中断处理函数
/**
* @brief TIMER1中断处理函数
* @brief TIMER1 中断处理函数
* @return void
*/
void TIMER1_IRQHandler(void) {
// 检查CH3比较中断标志并处理
// 检查 CH3 比较中断标志并处理
if (timer_interrupt_flag_get(TIMER1, TIMER_INT_FLAG_CH3) != RESET) {
// 清除CH3比较中断标志
// 清除 CH3 比较中断标志
timer_interrupt_flag_clear(TIMER1, TIMER_INT_FLAG_CH3);
scheduler.taskFlags |= (1 << TASK_CURRENT_UPDATE);
// 例如触发ADC采样或其他操作
// 例如触发 ADC 采样或其他操作
}
}
@@ -305,4 +313,4 @@ void Set_3Pwm(float duty_cycle0, float duty_cycle1, float duty_cycle2) {
TIMER_CH0CV(TIMER1) = (uint32_t)(duty_cycle0 * TIMER1_PERIOD);
TIMER_CH1CV(TIMER1) = (uint32_t)(duty_cycle1 * TIMER1_PERIOD);
TIMER_CH2CV(TIMER1) = (uint32_t)(duty_cycle2 * TIMER1_PERIOD);
}
}
Robot_V2/device/ang_sensor.cpp
+5 -3
View File
@@ -6,10 +6,10 @@
#include "gd32c10x_dma.h"
#include "gd32c10x_gpio.h"
#include "gd32c10x_spi.h"
#include <cmath>
#include <cstdio>
#include <cstring>
#include <stdint.h>
#define ANG_SENS_DEBUG 1
#define ANG_SPI_CS_ENABLE() gpio_bit_reset(GPIOA, GPIO_PIN_15)
#define ANG_SPI_CS_DISABLE() gpio_bit_set(GPIOA, GPIO_PIN_15)
@@ -199,13 +199,14 @@ u8 angle_sensor::init(void) {
spi_dma_init(); // DMA初始化
return 0;
}
/**
* @ 角度传感器类 设置当前角度
* @param angle 输出级角度值
* @return 0成功/1失败
*/
u8 angle_sensor::angle_set(float angle) {
m_cumulativeAngle = angle * REDUCTION_RATIO; // 从Flash读取上次保存的角度
m_cumulativeAngle = angle * REDUCTION_RATIO;
m_firstUpdate = true;
return 0;
}
@@ -237,6 +238,7 @@ inline float angle_sensor::read_sensorAngle(void) {
return (float)FAULT_RECEIVE;
}
/**
* @brief 更新传感器角度（输出级）
* @return 累积角度值
@@ -286,6 +288,6 @@ float angle_sensor::getRawAngle(void) const { return m_cumulativeAngle; }
* @brief 获取角度值(原始级)
* @return 角度值(原始级)
*/
float angle_sensor::getcurrent_sensorangle(void) const {
float angle_sensor::getcurrent_sensorangle(void) const{
return current_angle_deg;
}
Robot_V2/device/ang_sensor.h
+2
View File
@@ -14,6 +14,7 @@ int spi_rw_half_word_dma(uint32_t spi_periph, uint16_t tx_data,
class angle_sensor {
public:
float calibration_angle = 195.0f;
u8 init(void);
float read_sensorAngle(void);
float update(void);
@@ -22,6 +23,7 @@ class angle_sensor {
float getRawAngle(void) const;
float getcurrent_sensorangle(void) const;
u8 angle_set(float angle);
u8 set_calibration(float angle);
protected:
float m_cumulativeAngle;
Robot_V2/main.cpp
+40 -16
View File
@@ -16,14 +16,27 @@ extern angle_sensor KTH7823; // 角度传感器
extern EncoderVelocityKalman Vel_kalman; // 速度滤波
extern EncoderAngleKalman Angle_kalman; // 角度滤波
extern ThermalController thermalController; // 热量控制器
extern FOC_Controller FOC; // FOC控制
extern FOC_Controller FOC; // FOC控制
extern FlashEEPROM flashEEP; // EEPROM类
extern VersionManager Version;
// 全局版本管理器实例
TaskScheduler scheduler = {0};
u32 motor_runtime_50us = 0; // 电机驱动计算轮次
u32 cnt_test = 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: 速度开环控制
};
int main(void) {
u8 init_status = hardware_init();
@@ -82,24 +95,35 @@ void do_task(u32 currentTasks) {
if (currentTasks & (1 << TASK_MOTOR_CONTROL)) { // 电机控制任务(最高优先级)
motor_runtime_50us++;
// 函数指针数组，包含各种控制模式对应的函数
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: 速度开环控制
};
// 静态变量用于跟踪上一次的控制模式
static int last_control_mode = -1;
// 边界检查，防止数组越界
if (FOC.control_mode >= 0 &&
FOC.control_mode < 7) { // 更新为7个控制模式
// 调用对应的控制函数
(FOC.*control_functions[FOC.control_mode])();
// 检查控制模式是否发生变化
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温度传感器值
Robot_V2/sys/config.h
+48 -37
View File
@@ -9,12 +9,13 @@
#define EEP_DEBUG 1
#define OTA_DEBUG 1
#define SLEEP_DEBUG 1
#define ANG_SENS_DEBUG 1
// 版本号定义 - 可根据实际项目进行调整
constexpr u16 SOFTWARE_VERSION_MAJOR = 26;
constexpr u16 SOFTWARE_VERSION_MINOR = 2;
constexpr u16 SOFTWARE_VERSION_PATCH = 8;
constexpr u16 SOFTWARE_VERSION_BUILD = 5;
constexpr u16 SOFTWARE_VERSION_MINOR = 3;
constexpr u16 SOFTWARE_VERSION_PATCH = 9;
constexpr u16 SOFTWARE_VERSION_BUILD = 1;
constexpr u16 HARDWARE_VERSION_MAJOR = 25;
constexpr u16 HARDWARE_VERSION_MINOR = 12;
@@ -54,36 +55,10 @@ constexpr u32 OTA_AVAL = 0x00000000; // 全零表示可用
constexpr u8 PP = 11;
constexpr float DIR = -1.0f;
constexpr u16 REDUCTION_RATIO = 25;
constexpr float CURRENT_OFFSET = -1.62f; // 当前电流传感器的零点偏移
constexpr float HOLD_PWM_DUTY = 0.95f;
constexpr u16 ANGLESENS_OFFSET = +0;
constexpr float CURRENT_OFFSET = -3.31f/2.0f; // 当前电流传感器的零点偏移
constexpr float HOLD_PWM_DUTY = 0.1f;
constexpr u16 ANGLESENS_OFFSET = +15;
constexpr float KE_FACTOR = 0.0f;
// PID配置
constexpr float DEFAULT_current_loop_p = 0.1f / 100 * 12; // 电流P
constexpr float current_loop_Ki = 0.55f; // 电流P
constexpr float current_loop_Kd = 3e-4; // 电流P
constexpr float DEFAULT_vel_loop_p = 0.018f / 100 * 12; // 速度P
constexpr float vel_loop_Ku = 3.600000e-03 * 0.7 * 100 / 12;
constexpr float vel_loop_Tu = 17.0f * 1e-3f;
constexpr float vel_loop_Ki = 1.2 * vel_loop_Ku / vel_loop_Tu;
constexpr float vel_loop_Kd = 0.075 * vel_loop_Ku * vel_loop_Tu * 2;
constexpr float DEFAULT_vel_angloop_p = 0.4f / 100; // 速度 速度-角度P
constexpr float vel_angloop_Ki = 8.0f; // 速度 速度-角度I
constexpr float vel_angloop_Kd = 5e-05f; // 速度 速度-角度D
constexpr float DEFAULT_ang_angloop_p = 150.0f / 100 * 12; // 角度 速度-角度P
constexpr float ang_angloop_Ki = 0.00f; // 角度 速度-角度I
constexpr float ang_angloop_Kd = 8e-04; // 角度 速度-角度D
constexpr float DEFAULT_angle_loop_p = 60.0f / 100 * 12; // 角度P
constexpr float DEFAULT_vel_MIT_p = 9.0f / 100; // 速度 MIT P
constexpr float vel_MIT_Ku = 6.72f * 100 / 12;
constexpr float vel_MIT_Tu = 62.0f * 1e-3f;
constexpr float vel_MIT_Ki = 0.1f;
constexpr float vel_MIT_Kd = 0.01f;
// 角度卡尔曼滤波器参数
// 参数调节建议：
@@ -104,13 +79,46 @@ constexpr float VELOCITY_KALMAN_INITIAL_COVARIANCE = 1e-5f;
constexpr float VELOCITY_KALMAN_PROCESS_NOISE_Q =
1e-6f; // 过程噪声协方差 - 通常保持较小
constexpr float VELOCITY_KALMAN_MEASUREMENT_NOISE_R =
1.0f; // 测量噪声协方差 - 主要调节参数
1.0f; // 测量噪声协方差 - 主要调节参数1.0
// 电流卡尔曼滤波器参数
constexpr float CURRENT_KALMAN_INITIAL_STATE = 0.0f;
constexpr float CURRENT_KALMAN_INITIAL_COVARIANCE = 100.0f; // 增大初始协方差
constexpr float CURRENT_KALMAN_PROCESS_NOISE_Q = 10.0f; // 降低过程噪声
constexpr float CURRENT_KALMAN_MEASUREMENT_NOISE_R = 1e-3f; // 增大测量噪声
constexpr float CURRENT_KALMAN_INITIAL_COVARIANCE = 100.0f; // 增大初始协方差
constexpr float CURRENT_KALMAN_PROCESS_NOISE_Q = 10.0f; // 降低过程噪声
constexpr float CURRENT_KALMAN_MEASUREMENT_NOISE_R = 5e-5f; // 增大测量噪声
// PID配置
constexpr float DEFAULT_current_loop_p = 0.1f / 100 * 12; // 电流P
constexpr float current_loop_Ki = 0.55f; // 电流P
constexpr float current_loop_Kd = 3e-4f; // 电流P
constexpr float DEFAULT_vel_loop_p = 0.018f / 100 * 12; // 速度P
constexpr float vel_loop_Ku = 3.600000e-03 * 0.7 * 100 / 12;
constexpr float vel_loop_Tu = 17.0f * 1e-3f;
constexpr float vel_loop_Ki = 1.2 * vel_loop_Ku / vel_loop_Tu;
constexpr float vel_loop_Kd = 0.075 * vel_loop_Ku * vel_loop_Tu * 2;
constexpr float DEFAULT_vel_angloop_p = 0.4f / 100; // 速度 速度-角度P
constexpr float vel_angloop_Ki = 5.0f; // 速度 速度-角度I
constexpr float vel_angloop_Kd = 5e-05f; // 速度 速度-角度D
constexpr float DEFAULT_angle_angloop_p = 150.0f / 100 * 12; // 角度 速度-角度P
constexpr float ang_angloop_Ki = 0.00f; // 角度 速度-角度I
constexpr float ang_angloop_Kd = 8e-04f; // 角度 速度-角度D
constexpr float DEFAULT_angle_loop_p = 60.0f / 100 * 12; // 角度P
constexpr float DEFAULT_angle_MIT_p = 100.0f / 100 * 12; // 角度 MIT P
constexpr float angle_MIT_Ki = 0.0f;
constexpr float angle_MIT_Kd = 8e-04f;
constexpr float DEFAULT_vel_MIT_p = 5.0f / 100; // 速度 MIT P
constexpr float vel_MIT_Ki = 15.0f;
constexpr float vel_MIT_Kd = 1e-04f;
constexpr float DEFAULT_current_MIT_p = 0.2f / 100 * 12; // 电流 MIT P
constexpr float current_MIT_Ki = 0.1f;
constexpr float current_MIT_Kd = 3e-4f;
// 常量定义
constexpr u8 SIZE_FLOAT = sizeof(float);
@@ -120,6 +128,7 @@ constexpr float PI = 3.14159265358979323846f;
constexpr float INV_PI_3 = 3.0f / PI;
constexpr float PI_3 = PI / 3.0f;
constexpr float _1_SQRT3 = 0.57735026919f; // 1/sqrt(3)
constexpr float _3PI_2 = 4.71238898038f;
constexpr float SQRT3 = 1.7320508075688772f;
constexpr float SQRT3DV2 = 0.8660254037844386f; // sqrt(3)/2
constexpr float COS_PI_3 = 0.5f;
@@ -175,15 +184,17 @@ constexpr float ANGLESENS_FACTOR = 1.0f / ADC_MAX * 360.0f;
// TIMER2-任务调度
constexpr int TIMER2_PRESCALER = 119; // 120MHz / 120 = 1MHz
constexpr int TIMER2_PERIOD = 49; // 1 / 1MHz * 50 = 50us
constexpr int MOTOR_PERIOD = 2; // 100us/50us = 2
constexpr float TIME_SLICE =
(TIMER2_PERIOD + 1) * (TIMER2_PRESCALER + 1) / 120000000.0f; // 周期
constexpr float TIMER2_FREQUENCY = 1 / TIME_SLICE; // 频率
constexpr float MOTOR_FREQUENCY = 1 / TIME_SLICE / MOTOR_PERIOD; // 频率
// TIMER1-PWM
constexpr int TIMER1_PRESCALER = 0; // 不分频，计数器时钟为120MHz
constexpr int TIMER1_PERIOD = 5999; // 120MHz / (0+1) / (5999+1) = 20kHz
constexpr int TIMER1_DEFAULT_PULSE = 0;
constexpr int TIMER1_CH3_COMPARE_VALUE =
(TIMER1_PERIOD + 1) * 0.5 - 1; // PWM周期50%位置触发中断
(TIMER1_PERIOD + 1) * 0.41 - 1; // PWM周期50%位置触发中断
#endif
Robot_V2/sys/health.cpp
+7 -4
View File
@@ -40,7 +40,7 @@ u32 ThermalController::update() {
remainingTime10Ms = 1; // 可以恢复工作
}
if (remainingTime10Ms == 1) { // 过热恢复
FOC.resume();
FOC.resume_from_thermal();
CAN0_transmiter.ThermalStatus = 0;
return --remainingTime10Ms;
}
@@ -73,7 +73,7 @@ u32 ThermalController::update() {
}
if (thermal_value > thermal_capacity) {
FOC.shut_down();
FOC.shut_down_thermal();
remainingTime10Ms = PROTECT_TIME; // 过热停机
CAN0_transmiter.ThermalStatus = 1;
return remainingTime10Ms;
@@ -99,6 +99,9 @@ void power_manage(void) {
FOC.shut_down();
flashEEP.save_system_config(true);
} else {
FOC.resume();
// 只有在没有过热关机时才恢复
if (!FOC.thermal_shutdown) {
FOC.resume();
}
}
}
}