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

feat(FOC): 集成ARM数学库优化FOC算法实现

Browse Source 
- 引入arm_math.h库以使用ARM优化的数学函数
- 将角度归一化从弧度制改为角度制，使用360.0f替代2*PI
- 使用arm_sin_cos_f32替代查找表实现正弦余弦计算
- 移除注释中的ARM Clark和Park变换函数调用
- 修正机械角度计算中的时间常量为TIMER2_FREQUENCY倒数
- 移除传感器角度到弧度的转换，保持原始角度值
- 激活电流环控制中的Iq/Id计算并发送CAN数据
This commit is contained in:
cbb
2026-01-07 23:13:21 +08:00
parent fccb93dd41
commit 72506ce87d
14 changed files with 133 additions and 452 deletions
Download Patch File Download Diff File Expand all files Collapse all files
Robot_V2/device/ADC.cpp
+1
View File
@@ -2,6 +2,7 @@
#include "Can.h"
#include "FOC.h"
#include "Filter.h"
#include "arm_math.h"
#include "config.h"
#include "math.h"
#include "systick.h"
Robot_V2/device/Can.cpp
+2
View File
@@ -444,6 +444,7 @@ u8 CAN_transmiter::init() {
void CAN_transmiter::update(void) {
target_angle_union.f = FOC.target_angle;
target_velocity_union.f = FOC.target_velocity;
target_current_union.f = FOC.target_current;
limit_velocity_union.f = FOC.limit_velocity;
limit_voltage = FOC.limit_voltage * 1000;
mcu_temp = sampler.get_mcu_temp() * 100;
@@ -591,6 +592,7 @@ void CAN_transmiter::communication_check() {
target_velocity_union.f =
unpack_bytes_to_float(receive_message.rx_data, 11).f;
FOC.target_velocity = target_velocity_union.f;
FOC.target_current = FOC.target_velocity * 10.0f;
} 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];
Robot_V2/device/Can.h
+4 -3
View File
@@ -87,9 +87,10 @@ class CAN_transmiter {
u8 Target_Control_Mode = 0; // 目标控制模式
u8 EEPROMStatus = 0; // EEPROM状态码
float_union_t target_velocity_union = {0}; // 目标速度
float_union_t u_current_union = {0}; // U相电流
float_union_t w_current_union = {0}; // W相电流
float_union_t v_current_union = {0}; // V相电流
float_union_t target_current_union = {0};
float_union_t u_current_union = {0}; // U相电流
float_union_t w_current_union = {0}; // W相电流
float_union_t v_current_union = {0}; // V相电流
can_trasnmit_message_struct transmit_message; // 发送消息结构体
u8 transmit_mailbox = CAN_NOMAILBOX; // 初始化为无mailbox状态
Robot_V2/device/Timer.cpp
+56 -48
View File
@@ -12,7 +12,6 @@
#include <stdint.h>
#include <stdio.h>
volatile u32 total_us100_counter = 0; // 总计数器，每100微秒增加1
extern u16 adc_value[7];
extern FOC_Controlroller FOC;
@@ -96,66 +95,75 @@ void TIMER2_IRQHandler(void) {
if (timer_interrupt_flag_get(TIMER2, TIMER_INT_UP) != RESET) {
// 清除更新中断标志
timer_interrupt_flag_clear(TIMER2, TIMER_INT_UP);
// 每100us执行一次的操作
scheduler.taskFlags |= (1 << TASK_ANGLE_UPDATE) |
(1 << TASK_VELOCITY_UPDATE) |
(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;
}
}
// 每50us执行一次的操作 - 电机控制
scheduler.taskFlags |= (1 << TASK_MOTOR_CONTROL);
scheduler.taskFlags |=
(1 << TASK_ANGLE_UPDATE) | (1 << TASK_VELOCITY_UPDATE);
total_us100_counter++;
if (total_us100_counter > 9) {
total_us100_counter = 0;
// 可以添加每1ms执行的代码，无负载均衡
static u8 ms1_counter = 0;
// 使用标志位控制，每100us执行一次其他任务
static bool isEvenCall = false;
isEvenCall = !isEvenCall;
switch (ms1_counter) { // 每10ms执行的代码，负载均衡
if (isEvenCall) {
// 每100us执行一次的操作 - 保持原有频率
switch (total_us100_counter) { // 每1ms执行的代码，负载均衡
case 0: {
scheduler.taskFlags |= (1 << TASK_THERMAL_CONTROL);
scheduler.taskFlags |= (1 << TASK_MCU_TEMP_UPDATE);
break;
}
case 1: {
scheduler.taskFlags |= (1 << TASK_MAGNETIC_CHECK);
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;
}
}
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执行的代码
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执行的代码
}
}
}
}
Robot_V2/device/ang_sensor.cpp
+1 -1
View File
@@ -253,7 +253,7 @@ float angle_sensor::update(void) {
float delta = current_angle_deg - m_lastAngle;
// 处理角度绕回情况（0度和360度之间的跳变）
// 如果变化超过180度，说明发生了绕回
// 如果变化超过300度，说明发生了绕回
if (delta > ANGLE_WRAP_THRESHOLD) {
// 从接近360度绕回到接近0度
delta -= ANGLE_WRAP_VALUE;