From 974b8e66750213e55b4d5355202e5319a11cb2fb Mon Sep 17 00:00:00 2001 From: cbb <2625274797@qq.com> Date: Sun, 2 Nov 2025 01:54:01 +0800 Subject: [PATCH] =?UTF-8?q?feat(FOC):=20=E6=B7=BB=E5=8A=A0=E7=94=B5?= =?UTF-8?q?=E6=BA=90=E7=94=B5=E5=8E=8B=E6=9B=B4=E6=96=B0=E5=8A=9F=E8=83=BD?= =?UTF-8?q?=20=E5=9C=A8=20FOC=20=E6=8E=A7=E5=88=B6=E5=99=A8=E4=B8=AD?= =?UTF-8?q?=E6=96=B0=E5=A2=9E=20`update=5Fpower`=20=E6=96=B9=E6=B3=95?= =?UTF-8?q?=EF=BC=8C=E7=94=A8=E4=BA=8E=E5=AE=9E=E6=97=B6=E8=8E=B7=E5=8F=96?= =?UTF-8?q?=E6=AF=8D=E7=BA=BF=E7=94=B5=E5=8E=8B=EF=BC=8C=20=E6=8F=90?= =?UTF-8?q?=E5=8D=87=E6=8E=A7=E5=88=B6=E7=B3=BB=E7=BB=9F=E5=AF=B9=E7=94=B5?= =?UTF-8?q?=E6=BA=90=E7=8A=B6=E6=80=81=E7=9A=84=E6=84=9F=E7=9F=A5=E8=83=BD?= =?UTF-8?q?=E5=8A=9B=E3=80=82?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit feat(CAN): 优化 CAN 配置与通信处理 将 CAN 相关宏定义改为 constexpr 常量，并引入角度转换因子。 修正接收数据解析方式，增强通信稳定性与精度。 feat(angle_sensor): 实现角度累积与绕回处理 重写角度传感器驱动，支持角度连续测量与自动绕回处理。 新增 `update`、`getAngle` 和 `reset` 接口，提高角度读取准确性。 feat(main): 增加角度传感器磁感应强度测试 在主函数初始化阶段调用磁感应强度检测函数，辅助判断传感器工作状态。 --- Robot_V2/Algorithm/DengFOC.cpp | 3 ++ Robot_V2/Algorithm/DengFOC.h | 2 + Robot_V2/device/Can.cpp | 20 ++++---- Robot_V2/device/Timer.cpp | 11 +++-- Robot_V2/device/ang_sensor.cpp | 89 +++++++++++++++------------------- Robot_V2/device/ang_sensor.h | 9 +++- Robot_V2/main.cpp | 2 + Robot_V2/sys/initial.cpp | 8 ++- Robot_V2/sys/initial.h | 1 + 9 files changed, 79 insertions(+), 66 deletions(-) diff --git a/Robot_V2/Algorithm/DengFOC.cpp b/Robot_V2/Algorithm/DengFOC.cpp index 025d846..9ef2237 100644 --- a/Robot_V2/Algorithm/DengFOC.cpp +++ b/Robot_V2/Algorithm/DengFOC.cpp @@ -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(); +} \ No newline at end of file diff --git a/Robot_V2/Algorithm/DengFOC.h b/Robot_V2/Algorithm/DengFOC.h index 25018a8..7fee8fd 100644 --- a/Robot_V2/Algorithm/DengFOC.h +++ b/Robot_V2/Algorithm/DengFOC.h @@ -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; diff --git a/Robot_V2/device/Can.cpp b/Robot_V2/device/Can.cpp index 4509f37..8af9886 100644 --- a/Robot_V2/device/Can.cpp +++ b/Robot_V2/device/Can.cpp @@ -13,11 +13,11 @@ #include #include -#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; } } diff --git a/Robot_V2/device/Timer.cpp b/Robot_V2/device/Timer.cpp index e610a8c..c6b7bf1 100644 --- a/Robot_V2/device/Timer.cpp +++ b/Robot_V2/device/Timer.cpp @@ -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(); } } } diff --git a/Robot_V2/device/ang_sensor.cpp b/Robot_V2/device/ang_sensor.cpp index 43d45f2..6957956 100644 --- a/Robot_V2/device/ang_sensor.cpp +++ b/Robot_V2/device/ang_sensor.cpp @@ -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; } \ No newline at end of file diff --git a/Robot_V2/device/ang_sensor.h b/Robot_V2/device/ang_sensor.h index f1f6c76..c901896 100644 --- a/Robot_V2/device/ang_sensor.h +++ b/Robot_V2/device/ang_sensor.h @@ -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 diff --git a/Robot_V2/main.cpp b/Robot_V2/main.cpp index 2cce2d2..dca68d2 100644 --- a/Robot_V2/main.cpp +++ b/Robot_V2/main.cpp @@ -20,6 +20,8 @@ int main(void) { User_timer_init(); // Timer初始化 driver_init(); // 电机驱动初始化 watchdog_init(5000); // 看门狗初始化 + + angsens_MG_test(); // 角度传感器磁感应强度测试 while (1) { // delay_1ms(100); diff --git a/Robot_V2/sys/initial.cpp b/Robot_V2/sys/initial.cpp index 94a6e49..f829dbf 100644 --- a/Robot_V2/sys/initial.cpp +++ b/Robot_V2/sys/initial.cpp @@ -1,8 +1,8 @@ #include "initial.h" #include "Timer.h" +#include "ang_sensor.h" #include - 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输出初始化 */ diff --git a/Robot_V2/sys/initial.h b/Robot_V2/sys/initial.h index 902e240..33a2536 100644 --- a/Robot_V2/sys/initial.h +++ b/Robot_V2/sys/initial.h @@ -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