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

feat(FOC): 添加电源电压更新功能

Browse Source 
在 FOC 控制器中新增 `update_power` 方法，用于实时获取母线电压，
提升控制系统对电源状态的感知能力。

feat(CAN): 优化 CAN 配置与通信处理
将 CAN 相关宏定义改为 constexpr 常量，并引入角度转换因子。
修正接收数据解析方式，增强通信稳定性与精度。

feat(angle_sensor): 实现角度累积与绕回处理
重写角度传感器驱动，支持角度连续测量与自动绕回处理。
新增 `update`、`getAngle` 和 `reset` 接口，提高角度读取准确性。

feat(main): 增加角度传感器磁感应强度测试
在主函数初始化阶段调用磁感应强度检测函数，辅助判断传感器工作状态。
This commit is contained in:
cbb
2025-11-02 01:54:01 +08:00
parent 0c5ab9c1d8
commit 974b8e6675
9 changed files with 79 additions and 66 deletions
Download Patch File Download Diff File Expand all files Collapse all files
Robot_V2/Algorithm/DengFOC.cpp
+3
View File
@@ -220,3 +220,6 @@ void FOC_Controlroller::DFOC_M0_setTorque(float Target) // 电流力矩环
// setTorque(current_loop_M0(Target-DFOC_M0_Current()),_electricalAngle());
}
void FOC_Controlroller::update_power(void) {
voltage_power_supply = sampler.get_vbus_voltage();
}
Robot_V2/Algorithm/DengFOC.h
+2
View File
@@ -19,10 +19,12 @@ class FOC_Controlroller {
u8 is_shut_down = 0;
float velocity_open_loop(float target_velocity);
void update_power(void);
void DFOC_alignSensor(int _PP, int _DIR);
void shut_down(void);
void resume(void);
protected:
std::u64 last_time_us = 0;
std::u64 now_us = 0;
Robot_V2/device/Can.cpp
+10 -10
View File
@@ -13,11 +13,11 @@
#include <stdio.h>
#include <stdlib.h>
#define DEV_CAN0_ID 0x0000
#define DEV_CAN0_MASK 0x0000
#define CANX CAN0
#define DEV_CAN_LOOPBACK_ID 0x0000
constexpr u16 CANX = CAN0;
constexpr u16 DEV_CAN0_ID = 0x0000;
constexpr u16 DEV_CAN0_MASK = 0x0000;
constexpr u16 DEV_CAN_LOOPBACK_ID = 0x0000;
constexpr float ANGLE_FACTOR = 25.0f / 360.0f * 2.0f * 3.14159265358979323846f;
extern CAN_transmiter CAN0_transmiter;
extern FOC_Controlroller FOC;
@@ -430,15 +430,13 @@ void CAN_transmiter::update(void) {
mcu_temp = sampler.get_mcu_temp() * 100;
motor_temp = sampler.get_drv_temp() * 100;
Power_Voltage = sampler.get_vbus_voltage() * 1000;
rt_angle_union.f = KTH7823.readAngle();
rt_angle_union.f = KTH7823.getAngle();
rt_velocity_union.f = 0.0f;
Status_code = (gpio_input_bit_get(GPIOB, GPIO_PIN_2) == SET) ? 0 : 1;
Target_Control_Mode = FOC.control_mode;
controller_Mode = FOC.control_mode;
target_angle_union.f = FOC.target_angle;
target_velocity_union.f = FOC.target_velocity;
u_current_union.f = sampler.get_current('U') * 1000;
w_current_union.f = sampler.get_current('W') * 1000;
v_current_union.f = sampler.get_current('V') * 1000;
@@ -675,10 +673,12 @@ void CAN_transmiter::communication_check() {
}
FOC.target_angle =
unpack_bytes_to_float(receive_message.rx_data, 7).f;
FOC.target_velocity =
target_velocity_union.f =
unpack_bytes_to_float(receive_message.rx_data, 11).f;
FOC.target_velocity = target_velocity_union.f * ANGLE_FACTOR;
can0_receive_flag = RESET;
}
can0_receive_flag = RESET;
}
}
Robot_V2/device/Timer.cpp
+6 -5
View File
@@ -3,6 +3,7 @@
#include "DengFOC.h"
#include "Timer.h"
#include "adc.h"
#include "ang_sensor.h"
#include "gd32c10x.h"
#include "gd32c10x_gpio.h"
#include "gd32c10x_misc.h"
@@ -33,6 +34,7 @@ static volatile std::u64 system_runtime_us = 0;
extern FOC_Controlroller FOC;
// 声明外部采样器对象
extern sampler sampler;
extern angle_sensor KTH7823;
/**
* @brief 设置引脚中断优先级
@@ -163,11 +165,10 @@ void TIMER2_IRQHandler(void) {
total_us100_counter++;
system_runtime_us += 100;
// 在定时器中断中执行FOC控制，确保控制频率稳定 (最高频操作放在最前)
FOC.velocity_open_loop(FOC.target_velocity);
// 每100us执行一次的操作
sampler.update_current();
sampler.update_current(); // 更新电流采样值
KTH7823.update(); // 更新角度传感器值
FOC.velocity_open_loop(FOC.target_velocity); // 速度开环控制
if (total_us100_counter >= 10) {
// 每1ms执行的代码
@@ -244,8 +245,8 @@ void TIMER3_IRQHandler(void) {
count_10++;
if (count_10 >= 10) {
count_10 = 0;
// 1s测试代码
FOC.update_power();
}
}
}
Robot_V2/device/ang_sensor.cpp
+40 -49
View File
@@ -10,56 +10,12 @@
constexpr float ANGLE_FACTOR = 1.0f / 65536.0f * 360.0f;
constexpr u16 DEFAULT_COMMAND = 0x0000;
constexpr u16 FAULT_RECEIVE = 0xFFFF;
constexpr u16 REDDUCTION_RATIO = 25;
// DMA传输完成标志
volatile bool spi_dma_tx_complete = false;
volatile bool spi_dma_rx_complete = false;
/**
* @brief Send or read half word through SPI using polling method
*
* @param[in] spi_periph SPI peripheral
* @param[in] i_HalfWord data
*/
u16 spi_rw_half_word(u32 spi_periph, const u16 i_HalfWord) {
u32 timeout;
u16 received_data = FAULT_RECEIVE;
/* Wait until transmit buffer is empty */
timeout = 200;
while ((RESET == spi_i2s_flag_get(spi_periph, SPI_FLAG_TBE)) && --timeout)
;
if (timeout == 0)
return FAULT_RECEIVE;
/* Clear RX buffer if not empty */
if (SET == spi_i2s_flag_get(spi_periph, SPI_FLAG_RBNE)) {
(void)spi_i2s_data_receive(spi_periph);
}
/* Send data */
spi_i2s_data_transmit(spi_periph, i_HalfWord);
/* Wait for response */
timeout = 200;
while ((RESET == spi_i2s_flag_get(spi_periph, SPI_FLAG_RBNE)) && --timeout)
;
if (timeout == 0)
return FAULT_RECEIVE;
/* Read received data */
received_data = spi_i2s_data_receive(spi_periph);
/* Wait until SPI is not busy */
timeout = 200;
while ((SET == spi_i2s_flag_get(spi_periph, SPI_FLAG_TRANS)) && --timeout)
;
if (timeout == 0)
return FAULT_RECEIVE;
return received_data;
}
/**
* @brief SPI DMA initialization
*/
@@ -102,8 +58,8 @@ void spi_dma_init(void) {
spi_dma_enable(SPI0, SPI_DMA_RECEIVE);
// 配置DMA中断
nvic_irq_enable(DMA0_Channel2_IRQn, 1, 1);
nvic_irq_enable(DMA0_Channel1_IRQn, 1, 1);
nvic_irq_enable(DMA0_Channel2_IRQn, 0, 1);
nvic_irq_enable(DMA0_Channel1_IRQn, 0, 1);
// 使能DMA传输完成中断
dma_interrupt_enable(DMA0, DMA_CH2, DMA_INT_FTF);
@@ -239,7 +195,7 @@ angle_sensor::angle_sensor(void) { KTH7823_init(); }
* @brief 读取传感器数据
* @return 传感器数据
*/
float angle_sensor::readAngle(void) {
float angle_sensor::read_sensorAngle(void) {
u16 u16data = FAULT_RECEIVE;
SPI_CS_ENABLE();
@@ -250,7 +206,42 @@ float angle_sensor::readAngle(void) {
} else {
SPI_CS_DISABLE();
// 处理读取错误
printf("Error reading angle from sensor.\n");
// printf("Error reading angle from sensor.\n");
return FAULT_RECEIVE; // 错误返回
}
}
float angle_sensor::update(void) {
float currentAngle = read_sensorAngle();
if (m_firstUpdate) {
m_lastAngle = currentAngle;
m_firstUpdate = false;
return m_cumulativeAngle;
}
// 计算角度变化
float delta = currentAngle - m_lastAngle;
// 处理角度绕回情况（0度和360度之间的跳变）
// 如果变化超过180度，说明发生了绕回
if (delta > 180.0f) {
// 从接近360度绕回到接近0度
delta -= 360.0f;
} else if (delta < -180.0f) {
// 从接近0度绕到接近360度
delta += 360.0f;
}
m_cumulativeAngle += delta / REDDUCTION_RATIO;
m_lastAngle = currentAngle;
return m_cumulativeAngle;
}
float angle_sensor::getAngle(void) const { return m_cumulativeAngle; }
void angle_sensor::reset(void) {
m_cumulativeAngle = 0.0f;
m_firstUpdate = true;
}
Robot_V2/device/ang_sensor.h
+8 -1
View File
@@ -14,10 +14,17 @@ int spi_rw_half_word_dma(uint32_t spi_periph, uint16_t tx_data,
class angle_sensor {
public:
angle_sensor();
float readAngle(void);
float read_sensorAngle(void);
float update(void);
void reset(void);
float getAngle(void) const;
protected:
void KTH7823_init(void);
bool m_firstUpdate;
float m_lastAngle;
float m_cumulativeAngle;
};
#ifdef __cplusplus
Robot_V2/main.cpp
+2
View File
@@ -21,6 +21,8 @@ int main(void) {
driver_init(); // 电机驱动初始化
watchdog_init(5000); // 看门狗初始化
angsens_MG_test(); // 角度传感器磁感应强度测试
while (1) {
// delay_1ms(100);
fwdgt_counter_reload(); // 看门狗复位
Robot_V2/sys/initial.cpp
+7 -1
View File
@@ -1,8 +1,8 @@
#include "initial.h"
#include "Timer.h"
#include "ang_sensor.h"
#include <stdint.h>
void GPIO_config(void) {
// 使能GPIOB时钟
rcu_periph_clock_enable(RCU_GPIOB);
@@ -30,6 +30,12 @@ u8 watchdog_init(u32 time) {
fwdgt_enable();
return status;
}
void angsens_MG_test(void) {
u8 MGH = gpio_input_bit_get(GPIOB, GPIO_PIN_10);
u8 MGL = gpio_input_bit_get(GPIOB, GPIO_PIN_11);
std::printf("MGH:%d MGL:%d\r\n", MGH, MGL);
}
/**
* @brief driver输出初始化
*/
Robot_V2/sys/initial.h
+1
View File
@@ -16,6 +16,7 @@ u8 tle5210b_init(void);
u8 USART0_init(void);
u8 watchdog_init(u32 time);
void driver_init(void);
void angsens_MG_test(void);
#ifdef __cplusplus