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

feat(FOC): 添加开关机功能并优化定时器配置

Browse Source
This commit is contained in:
cbb
2025-10-29 21:22:10 +08:00
parent 7f0ca1857b
commit 5d14731af5
6 changed files with 139 additions and 23 deletions
Download Patch File Download Diff File Expand all files Collapse all files
Robot_V2/Algorithm/DengFOC.cpp
+16 -2
View File
@@ -3,8 +3,10 @@
#include "systick.h"
#include <cmath>
#include <cstdio>
#include <gd32c10x.h>
#include <stdint.h>
constexpr double PI = 3.1415926535897932384626433832795f;
constexpr float _3PI_2 = 4.71238898038f;
constexpr u8 PP = 11, DIR = 1;
@@ -15,7 +17,6 @@ constexpr float sqrt3 = 1.73205080757f; // sqrt(3)的预计算值
#define constrain(value, min, max, target_min, target_max) \
((value - min) / (max - min) * (target_max - target_min))
inline float _electricalAngle(float shaft_angle) { return shaft_angle * PP; }
/**
@@ -81,7 +82,7 @@ void FOC_Controlroller::setPhaseVoltage(float Uq, float Ud, float angle_el) {
*/
float FOC_Controlroller::velocity_open_loop(float target_velocity) {
// 通过时间间隔和目标速度计算需要转动的机械角度，存储在shaft_angle中，在此之前，还需要对轴角度归一化
// 预计算常量 1e-4 = 100微秒控制周期 / 1秒 = 100e-6
// 预计算常量 1e-4 = 100微秒控制周期
shaft_angle = _normalizeAngle(shaft_angle + target_velocity * 1e-4f);
// 直接计算电气角度，避免函数调用开销
@@ -105,6 +106,9 @@ float FOC_Controlroller::velocity_open_loop(float target_velocity) {
Uc = (-Ualpha - sqrt3 * Ubeta) * 0.5f + v_half;
// 计算占空比
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);
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);
@@ -128,3 +132,13 @@ void FOC_Controlroller::DFOC_alignSensor(int _PP, int _DIR) {
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() {
is_shut_down = 0;
gpio_bit_set(GPIOB, GPIO_PIN_12);
}
Robot_V2/Algorithm/DengFOC.h
+3
View File
@@ -17,9 +17,12 @@ class FOC_Controlroller {
float limit_velocity = 0.0f;
float target_angle = 0.0f;
float target_velocity = 0.0f;
u8 is_shut_down = 0;
float velocity_open_loop(float target_velocity);
void DFOC_alignSensor(int _PP, int _DIR);
void shut_down(void);
void resume(void);
private:
std::u64 last_time_us = 0;
Robot_V2/device/Can.cpp
+10 -1
View File
@@ -596,10 +596,19 @@ void CAN_transmiter::communication_check() {
// }
if (receive_message.rx_efid == 0x18ff0200) {
FOC.control_mode = receive_message.rx_data[0];
if (float(unpack_bytes_to_u16(receive_message.rx_data, 1)) / 1000 !=
FOC.limit_voltage) {
FOC.limit_voltage =
float(unpack_bytes_to_u16(receive_message.rx_data, 1)) / 1000;
float(unpack_bytes_to_u16(receive_message.rx_data, 1)) /
1000;
FOC.is_shut_down = 100; // 1s滤除启动电流
}
if (unpack_bytes_to_float(receive_message.rx_data, 3).f !=
FOC.limit_velocity) {
FOC.limit_velocity =
unpack_bytes_to_float(receive_message.rx_data, 3).f;
FOC.is_shut_down = 100; // 1s滤除启动电流
}
FOC.target_angle =
unpack_bytes_to_float(receive_message.rx_data, 7).f;
FOC.target_velocity =
Robot_V2/device/Timer.cpp
+90 -5
View File
@@ -5,8 +5,10 @@
#include "adc.h"
#include "gd32c10x.h"
#include "gd32c10x_gpio.h"
#include "gd32c10x_misc.h"
#include "gd32c10x_rcu.h"
#include "gd32c10x_timer.h"
#include "systick.h"
#include <cstdio>
#include <stdbool.h>
#include <stdint.h>
@@ -18,6 +20,8 @@
// 然后设置周期为99，即计数100次，1MHz/100 = 10kHz = 100微秒
constexpr int TIMER2_PRESCALER = 119; // 120MHz / 120 = 1MHz
constexpr int TIMER2_PERIOD = 99; // 1MHz / 100 = 100us
constexpr int TIMER3_PRESCALER = 119; // 120MHz / 120 = 1MHz
constexpr int TIMER3_PERIOD = 9999; // 1MHz / 10000 = 100Hz = 10ms
constexpr int TIMER1_PRESCALER = 0; // 不分频，计数器时钟为120MHz
constexpr int TIMER1_PERIOD = 5999; // 120MHz / (0+1) / (5999+1) = 20kHz
@@ -47,13 +51,14 @@ void nvic_config(void) {
nvic_priority_group_set(NVIC_PRIGROUP_PRE2_SUB2);
/* 使能TIMER2中断，设置抢占优先级为1，子优先级为1 */
nvic_irq_enable(TIMER2_IRQn, 1, 1);
nvic_irq_enable(TIMER3_IRQn, 1, 2);
}
/**
* @brief 配置定时器1
* @brief 配置定时器2
* @return void
*
*配置定时器1
*配置定时器2，用于定时执行FOC控制、采样、CAN发送等任务
*/
void TIMER2_config(void) {
/* ----------------------------------------------------------------------------
@@ -90,6 +95,47 @@ void TIMER2_config(void) {
timer_enable(TIMER2);
}
/**
* @brief 配置定时器3
* @return void
*
*配置定时器3，用于诊断恢复
*/
void TIMER3_config(void) {
/* ----------------------------------------------------------------------------
TIMER3 Configuration:
TIMER3CLK = SystemCoreClock/120 = 1MHz, the period is 10ms(10000/1000000 =
10ms).
----------------------------------------------------------------------------
*/
timer_parameter_struct timer_struct;
/* 使能TIMER3时钟 */
rcu_periph_clock_enable(RCU_TIMER3);
/* 复位TIMER3外设到默认状态 */
timer_deinit(TIMER3);
/* 初始化TIMER初始化参数结构体 */
timer_struct_para_init(&timer_struct);
/* TIMER3配置参数设置 */
timer_struct.prescaler =
TIMER3_PRESCALER; /* 预分频值：120分频(从0开始计数) */
timer_struct.alignedmode = TIMER_COUNTER_EDGE; /* 边沿对齐模式 */
timer_struct.counterdirection = TIMER_COUNTER_UP; /* 向上计数模式 */
timer_struct.period =
TIMER3_PERIOD; /* 自动重装载值：10000次计数(从0开始计数) */
timer_struct.clockdivision = TIMER_CKDIV_DIV1; /* 时钟分频值：1分频 */
timer_init(TIMER3, &timer_struct);
/* 清除更新中断标志位 */
timer_interrupt_flag_clear(TIMER3, TIMER_INT_FLAG_UP);
/* 使能更新中断 */
timer_interrupt_enable(TIMER3, TIMER_INT_UP);
/* 使能TIMER3 */
timer_enable(TIMER3);
}
/**
* @brief 配置TIMER2中断
* @return void
@@ -142,21 +188,28 @@ void TIMER2_IRQHandler(void) {
sampler.update_vbus_voltage();
// 每10ms执行的代码 (1ms * 10 = 10ms)
static uint8_t ms10_counter = 0;
static u8 ms10_counter = 0;
ms10_counter++;
if (ms10_counter >= 10) {
ms10_counter = 0;
// 可以添加每10ms执行的代码
// 每100ms执行的代码 (10ms * 10 = 100ms)
static uint8_t ms100_counter = 0;
static u8 ms100_counter = 0;
ms100_counter++;
if (ms100_counter >= 10) {
ms100_counter = 0;
// 可以添加每100ms执行的代码
// if (sampler.get_current('U') > 1.0 ||
// sampler.get_current('V') > 1.0 ||
// sampler.get_current('W') > 1.0) {
// if (FOC.is_shut_down == 0) {
// FOC.shut_down();
// }
// }
// 每1s执行的代码 (100ms * 10 = 1s)
static uint8_t sec_counter = 0;
static u8 sec_counter = 0;
sec_counter++;
if (sec_counter >= 10) {
sec_counter = 0;
@@ -168,6 +221,38 @@ void TIMER2_IRQHandler(void) {
}
}
/**
* @brief TIMER3中断处理函数
* @return void
*/
void TIMER3_IRQHandler(void) {
// 检查更新中断标志并处理
if (timer_interrupt_flag_get(TIMER3, TIMER_INT_UP) != RESET) {
// 清除更新中断标志
timer_interrupt_flag_clear(TIMER3, TIMER_INT_UP);
static u8 count = 0;
count++;
if (FOC.is_shut_down > 0)
FOC.is_shut_down--;
else
FOC.resume();
// 10ms测试代码
if (count >= 10) {
count = 0;
// 100ms测试代码
static u8 count_10 = 0;
count_10++;
if (count_10 >= 10) {
count_10 = 0;
// 1s测试代码
}
}
}
}
/**
* @brief TIMER1 GPIO配置函数
*/
Robot_V2/device/Timer.h
+2
View File
@@ -11,6 +11,8 @@ void User_timer_init(void);
void Set_Pwm(uint16_t channel, float duty_cycle);
void Set_3Pwm(float duty_cycle0, float duty_cycle1,float duty_cycle2);
void TIMER2_IRQHandler(void);
void TIMER3_config(void);
void TIMER3_IRQHandler(void);
void timer1_pwm_config(void);
void timer1_gpio_config(void);
Robot_V2/sys/initial.cpp
+3
View File
@@ -1,4 +1,5 @@
#include "initial.h"
#include "gd32c10x_timer.h"
#include "systick.h"
#include <stdint.h>
#include "Timer.h"
@@ -124,6 +125,8 @@ u8 watchdog_init(u32 time) {
void driver_init(void) {
timer1_gpio_config();
timer1_pwm_config();
TIMER3_config();
}
int fputc(int ch, FILE *f) throw() {