diff --git a/Robot_V2/Algorithm/DengFOC.cpp b/Robot_V2/Algorithm/DengFOC.cpp index 9ef2237..7cd50ca 100644 --- a/Robot_V2/Algorithm/DengFOC.cpp +++ b/Robot_V2/Algorithm/DengFOC.cpp @@ -2,6 +2,8 @@ #include "Filter.h" #include "Timer.h" #include "adc.h" +#include "ang_sensor.h" +#include "gd32c10x_dbg.h" #include "pid.h" #include "systick.h" #include @@ -10,21 +12,22 @@ #include constexpr double PI = 3.1415926535897932384626433832795f; +constexpr float DEG2RAD = 1.0f / 360.0f * 6.28318530718f; +constexpr float RAD2DEG = 360.0f / 6.28318530718f; constexpr float _1_SQRT3 = 0.57735026919f; constexpr float _2_SQRT3 = 1.15470053838f; constexpr float _3PI_2 = 4.71238898038f; -constexpr u8 PP = 11, DIR = 1; +constexpr u8 PP = 11; +constexpr float DIR = -1.0; constexpr float sqrt3 = 1.73205080757f; // sqrt(3)的预计算值 constexpr float zero_electric_angle = 0.0f; extern sampler sampler; +extern angle_sensor KTH7823; CurrentKalman curU_kalman(0.0f, 0.0026f, 4.8f, 0.0001f, 1e-6f, 0.02f); CurrentKalman curV_kalman(0.0f, 0.0026f, 4.8f, 0.0001f, 1e-6f, 0.02f); CurrentKalman curW_kalman(0.0f, 0.0026f, 4.8f, 0.0001f, 1e-6f, 0.02f); -PIDController vel_loop_M0 = PIDController{2, 0, 0, 100000, 24.0 / 2.0}; -PIDController angle_loop_M0 = PIDController{2, 0, 0, 100000, 100}; -PIDController current_loop_M0 = PIDController{2, 0, 0, 100000, 800}; // 初始变量及函数定义 #define _constrain(amt, low, high) \ @@ -32,11 +35,6 @@ PIDController current_loop_M0 = PIDController{2, 0, 0, 100000, 800}; #define constrain(value, min, max, target_min, target_max) \ ((value - min) / (max - min) * (target_max - target_min)) -inline float _electricalAngle(float shaft_angle) { - return _normalizeAngle((float)(DIR * PP) * shaft_angle - - zero_electric_angle); -} - /** * @brief 归一化角度 * @@ -48,18 +46,34 @@ inline float _normalizeAngle(float angle) { // 取余运算可以用于归一化，列出特殊值例子算便知 return a >= 0 ? a : (a + 2 * PI); } +inline float _electricalAngle() { + return _normalizeAngle((float)(DIR * PP) * KTH7823.read_sensorAngle() * + -zero_electric_angle); +} /** * @brief 电机控制类构造函数 * */ FOC_Controlroller::FOC_Controlroller() { ; } -/** - * @brief 获取电机 shaft_angle - * - * @return void - */ -void FOC_Controlroller::driver_output(float Ua, float Ub, float Uc) { +/** + * @brief 设置电机输出 + * + * @param Uq: q轴电压 + * @param Ud: d轴电压 + * @param angle_el: 电机轴角度 + */ +inline void FOC_Controlroller::setPhaseVoltage(float Uq, float Ud, + float angle_el) { + angle_el = _normalizeAngle(angle_el); + // 帕克逆变换 + Ualpha = -Uq * sin(angle_el); + Ubeta = Uq * cos(angle_el); + + // 克拉克逆变换 + Ua = Ualpha + voltage_power_supply / 2; + Ub = (sqrt3 * Ubeta - Ualpha) / 2 + voltage_power_supply / 2; + Uc = (-Ualpha - sqrt3 * Ubeta) / 2 + voltage_power_supply / 2; // 计算占空比 Ua = constrain(Ua, 0.0f, voltage_power_supply, 0.0f, limit_voltage); Ub = constrain(Ub, 0.0f, voltage_power_supply, 0.0f, limit_voltage); @@ -71,34 +85,13 @@ void FOC_Controlroller::driver_output(float Ua, float Ub, float Uc) { // std::printf("PWM:%f,%f,%f\n", dc_a, dc_b, dc_c); } -/** - * @brief 设置电机输出 - * - * @param Uq: q轴电压 - * @param Ud: d轴电压 - * @param angle_el: 电机轴角度 - */ -void FOC_Controlroller::setPhaseVoltage(float Uq, float Ud, float angle_el) { - Uq = _constrain(Uq, -voltage_power_supply / 2, voltage_power_supply / 2); - angle_el = _normalizeAngle(angle_el + zero_electric_angle); - // 帕克逆变换 - Ualpha = -Uq * sin(angle_el); - Ubeta = Uq * cos(angle_el); - - // 克拉克逆变换 - Ua = Ualpha + voltage_power_supply / 2; - Ub = (sqrt(3) * Ubeta - Ualpha) / 2 + voltage_power_supply / 2; - Uc = (-Ualpha - sqrt(3) * Ubeta) / 2 + voltage_power_supply / 2; - driver_output(Ua, Ub, Uc); -} - /** * @brief 速度开环控制 * * @param target_velocity: 目标速度 * @return float: q轴电压 */ -float FOC_Controlroller::velocity_open_loop(float target_velocity) { +float FOC_Controlroller::velocity_open_loop() { // 通过时间间隔和目标速度计算需要转动的机械角度，存储在shaft_angle中，在此之前，还需要对轴角度归一化 // 预计算常量 1e-4 = 100微秒控制周期 shaft_angle = _normalizeAngle(shaft_angle + target_velocity * 1e-4f); @@ -139,17 +132,15 @@ float FOC_Controlroller::velocity_open_loop(float target_velocity) { * @brief 电机初始化 * * @param _PP: 电机极队数 - * @param _DIR: 电机极性 */ -void FOC_Controlroller::DFOC_alignSensor(int _PP, int _DIR) { - u8 PP = _PP; - u8 DIR = _DIR; - setPhaseVoltage(3, 0, _3PI_2); - delay_1ms(3000); - zero_electric_angle = _electricalAngle(0); - setPhaseVoltage(0, 0, _3PI_2); - std::printf("0电角度:%f", zero_electric_angle); -} +// void FOC_Controlroller::DFOC_alignSensor(int _PP) { +// u8 PP = _PP; +// setPhaseVoltage(3, 0, _3PI_2); +// delay_1ms(3000); +// zero_electric_angle = _electricalAngle(); +// setPhaseVoltage(0, 0, _3PI_2); +// std::printf("0电角度:%f", zero_electric_angle); +// } void FOC_Controlroller::shut_down() { is_shut_down = 100; @@ -164,62 +155,42 @@ void FOC_Controlroller::resume() { gpio_bit_set(GPIOB, GPIO_PIN_12); } -// 通过Ia,Ib,Ic计算Iq,Id(目前仅输出Iq) -float cal_Iq_Id(float current_a, float current_b, float angle_el) { - float I_alpha = current_a; - float I_beta = _1_SQRT3 * current_a + _2_SQRT3 * current_b; - - float ct = cos(angle_el); - float st = sin(angle_el); - // float I_d = I_alpha * ct + I_beta * st; - float I_q = I_beta * ct - I_alpha * st; - return I_q; +void FOC_Controlroller::update_power(void) { + voltage_power_supply = sampler.get_vbus_voltage(); } -void FOC_Controlroller::setTorque(float Uq, float angle_el) { - Uq = - _constrain(Uq, -(voltage_power_supply) / 2, (voltage_power_supply) / 2); - float Ud = 0; - angle_el = _normalizeAngle(angle_el); +/** + * @brief 角度闭环控制 + * + * @param target_velocity: 目标速度 + * @return float: q轴电压 + */ +float FOC_Controlroller::angle_control(float target_angle) { + float Sensor_angel = KTH7823.getRawAngle() * DEG2RAD; + float Kp = 2.0f; + + float Uq = _constrain(Kp * (target_angle - DIR * Sensor_angel) * RAD2DEG, + -voltage_power_supply, voltage_power_supply); + shaft_angle = _normalizeAngle(KTH7823.read_sensorAngle() * DEG2RAD); + float Ud = 0.0f; + // 直接计算电气角度，避免函数调用开销 + float angle_el = _normalizeAngle(shaft_angle * PP + zero_electric_angle); + // 帕克逆变换 Ualpha = -Uq * sin(angle_el); Ubeta = Uq * cos(angle_el); // 克拉克逆变换 Ua = Ualpha + voltage_power_supply / 2; - Ub = (sqrt(3) * Ubeta - Ualpha) / 2 + voltage_power_supply / 2; - Uc = (-Ualpha - sqrt(3) * Ubeta) / 2 + voltage_power_supply / 2; - driver_output(Ua, Ub, Uc); -} - -float FOC_Controlroller::DFOC_M0_Current() { - // 使用卡尔曼滤波后的三相电流计算Iq - float Iu_filtered = curU_kalman.update(sampler.get_current('U'), Ua); - float Iv_filtered = curV_kalman.update(sampler.get_current('V'), Ub); - float Iw_filtered = curW_kalman.update(sampler.get_current('W'), Uc); - - // 根据基尔霍夫电流定律验证三相电流总和是否接近0 - // float current_sum = Iu_filtered + Iv_filtered + Iw_filtered; - - // 用三相滤波后的电流计算Iq（通过Clarke和Park变换） - // 方法1: 使用U和V相计算 - float I_q_M0_ori = - cal_Iq_Id(Iu_filtered, Iv_filtered, _electricalAngle(shaft_angle)); - - // 方法2: 也可以使用其他两相的组合计算，然后取平均值以提高精度 - // float I_q_UW = cal_Iq_Id(Iu_filtered, Iw_filtered, - // _electricalAngle(shaft_angle)); float I_q_VW = cal_Iq_Id(Iv_filtered, - // Iw_filtered, _electricalAngle(shaft_angle)); float I_q_M0_ori = (I_q_UV + - // I_q_UW + I_q_VW) / 3.0f; - - return I_q_M0_ori; - // return 0; -} -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(); + Ub = (sqrt3 * Ubeta - Ualpha) / 2 + voltage_power_supply / 2; + Uc = (-Ualpha - sqrt3 * Ubeta) / 2 + voltage_power_supply / 2; + // 计算占空比 + 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); + float dc_a = _constrain(Ua / voltage_power_supply, 0.0f, 1.0f); + float dc_b = _constrain(Ub / voltage_power_supply, 0.0f, 1.0f); + float dc_c = _constrain(Uc / voltage_power_supply, 0.0f, 1.0f); + Set_3Pwm(dc_a, dc_b, dc_c); + return Sensor_angel; } \ No newline at end of file diff --git a/Robot_V2/Algorithm/DengFOC.h b/Robot_V2/Algorithm/DengFOC.h index 7fee8fd..dca0a28 100644 --- a/Robot_V2/Algorithm/DengFOC.h +++ b/Robot_V2/Algorithm/DengFOC.h @@ -1,6 +1,7 @@ // 函数声明 #ifndef _DENGFOC_H #define _DENGFOC_H +#include "pid.h" #include class FOC_Controlroller { public: @@ -18,13 +19,16 @@ class FOC_Controlroller { u8 voltage_error = 0; u8 is_shut_down = 0; - float velocity_open_loop(float target_velocity); + float velocity_open_loop(); + float angle_control(float target_angle); + // void SET_VEL_PID(float P, float I, float D, float ramp); // 速度环PID设置 + // void SET_ANGLE_PID(float P, float I, float D, float ramp); // 角度环PID设置 + void update_power(void); - void DFOC_alignSensor(int _PP, int _DIR); + void DFOC_alignSensor(int _PP); void shut_down(void); void resume(void); - protected: std::u64 last_time_us = 0; std::u64 now_us = 0; @@ -33,17 +37,16 @@ class FOC_Controlroller { float zero_electric_angle = 0; float voltage_power_supply = 24.0f; // 电源电压 - void driver_output(float Ua, float Ub, float Uc); + PIDController vel_loop = + PIDController{2, 0, 0, 100000, voltage_power_supply / 2}; + PIDController angle_loop = PIDController{2, 0, 0, 100000, 100}; + void setPhaseVoltage(float Uq, float Ud, float angle_el); - void setTorque(float Uq,float angle_el); - float DFOC_M0_Current(); - void DFOC_M0_setTorque(float Target); }; float _normalizeAngle(float angle); void DFOC_Vbus(float power_supply); -float _electricalAngle(float shaft_angle); -float DFOC_M0_Angle(); -float serial_motor_target(); +inline float _electricalAngle(); +float DFOC_M0_Angle(void); #endif \ No newline at end of file diff --git a/Robot_V2/device/Can.cpp b/Robot_V2/device/Can.cpp index 8af9886..61b1e8f 100644 --- a/Robot_V2/device/Can.cpp +++ b/Robot_V2/device/Can.cpp @@ -431,7 +431,7 @@ void CAN_transmiter::update(void) { motor_temp = sampler.get_drv_temp() * 100; Power_Voltage = sampler.get_vbus_voltage() * 1000; rt_angle_union.f = KTH7823.getAngle(); - rt_velocity_union.f = 0.0f; + rt_velocity_union.f = KTH7823.getVelocity(); Status_code = (gpio_input_bit_get(GPIOB, GPIO_PIN_2) == SET) ? 0 : 1; Target_Control_Mode = FOC.control_mode; @@ -621,7 +621,7 @@ u8 CAN_transmiter::CANFD_all_Send() { pack_u16_to_bytes(transmit_data, 11, Power_Voltage); pack_float_to_bytes(transmit_data, 13, rt_angle_union); pack_float_to_bytes(transmit_data, 17, rt_velocity_union); - success_flag += CANFD_Send(STATUS_ID, 20, transmit_data); + success_flag += CANFD_Send(STATUS_ID, 24, transmit_data); // 打包config数据 transmit_data[0] = controller_Mode; @@ -671,8 +671,10 @@ void CAN_transmiter::communication_check() { unpack_bytes_to_float(receive_message.rx_data, 3).f; FOC.is_shut_down = 100; // 1s滤除启动电流 } - FOC.target_angle = + target_angle_union.f = unpack_bytes_to_float(receive_message.rx_data, 7).f; + FOC.target_angle = target_angle_union.f * ANGLE_FACTOR; + target_velocity_union.f = unpack_bytes_to_float(receive_message.rx_data, 11).f; FOC.target_velocity = target_velocity_union.f * ANGLE_FACTOR; diff --git a/Robot_V2/device/Timer.cpp b/Robot_V2/device/Timer.cpp index c6b7bf1..e18b371 100644 --- a/Robot_V2/device/Timer.cpp +++ b/Robot_V2/device/Timer.cpp @@ -166,9 +166,11 @@ void TIMER2_IRQHandler(void) { system_runtime_us += 100; // 每100us执行一次的操作 - sampler.update_current(); // 更新电流采样值 - KTH7823.update(); // 更新角度传感器值 - FOC.velocity_open_loop(FOC.target_velocity); // 速度开环控制 + sampler.update_current(); // 更新电流采样值 + KTH7823.update(); // 更新角度传感器值 + // FOC.velocity_open_loop(); // 速度开环控制 + // FOC.velocity_control(FOC.target_velocity); // 速度闭环控制 + FOC.angle_control(FOC.target_angle); if (total_us100_counter >= 10) { // 每1ms执行的代码 diff --git a/Robot_V2/device/ang_sensor.cpp b/Robot_V2/device/ang_sensor.cpp index 6957956..2ed33d9 100644 --- a/Robot_V2/device/ang_sensor.cpp +++ b/Robot_V2/device/ang_sensor.cpp @@ -2,6 +2,8 @@ #include "gd32c10x_dma.h" #include "gd32c10x_gpio.h" #include "gd32c10x_spi.h" +#include +#include #include #define SPI_CS_ENABLE() gpio_bit_reset(GPIOA, GPIO_PIN_15) @@ -103,6 +105,7 @@ int spi_rw_half_word_dma(uint32_t spi_periph, uint16_t tx_data, // 禁用DMA通道 dma_channel_disable(DMA0, DMA_CH2); dma_channel_disable(DMA0, DMA_CH1); + printf("SPI DMA transfer timeout\r\n"); return -1; } @@ -206,11 +209,14 @@ float angle_sensor::read_sensorAngle(void) { } else { SPI_CS_DISABLE(); // 处理读取错误 - // printf("Error reading angle from sensor.\n"); + printf("Error reading angle from sensor.\n"); return FAULT_RECEIVE; // 错误返回 } } - +/** + * @brief 更新传感器角度（输出级） + * @return 累积角度值 + */ float angle_sensor::update(void) { float currentAngle = read_sensorAngle(); @@ -225,22 +231,60 @@ float angle_sensor::update(void) { // 处理角度绕回情况（0度和360度之间的跳变） // 如果变化超过180度，说明发生了绕回 - if (delta > 180.0f) { + if (delta > 270.0f) { // 从接近360度绕回到接近0度 delta -= 360.0f; - } else if (delta < -180.0f) { + } else if (delta < -270.0f) { // 从接近0度绕到接近360度 delta += 360.0f; } - - m_cumulativeAngle += delta / REDDUCTION_RATIO; + + std::memmove(velocity, velocity + 1, VEL_FILTER_SIZE * sizeof(float)); + velocity[VEL_FILTER_SIZE - 1] = delta * 1e4; + + m_cumulativeAngle += delta; m_lastAngle = currentAngle; return m_cumulativeAngle; } -float angle_sensor::getAngle(void) const { return m_cumulativeAngle; } +/** + * @brief 获取累积角度值(输出级) + * @return 累积角度值 + */ +float angle_sensor::getAngle(void) const { + return m_cumulativeAngle / REDDUCTION_RATIO; +} +/** + * @brief 获取累积角度值(原始级) + * @return 累积角度值 + */ +float angle_sensor::getRawAngle(void) const { return m_cumulativeAngle; } + +/** + * @brief 获取速度值(输出级) + * @return 速度值(输出级) + */ +float angle_sensor::getVelocity(void) const { + return getRawVelocity() / REDDUCTION_RATIO; +} + +/** + * @brief 获取速度值(原始级) + * @return 速度值(原始级) + */ +float angle_sensor::getRawVelocity(void) const { + float velocity_filted = 0; + for (int i = 0; i < VEL_FILTER_SIZE - 1; i++) { + velocity_filted += velocity[i]; + } + return velocity_filted / VEL_FILTER_SIZE; +} + +/** + * @brief 重置累积角度值 + */ void angle_sensor::reset(void) { m_cumulativeAngle = 0.0f; m_firstUpdate = true; diff --git a/Robot_V2/device/ang_sensor.h b/Robot_V2/device/ang_sensor.h index c901896..23cc3a0 100644 --- a/Robot_V2/device/ang_sensor.h +++ b/Robot_V2/device/ang_sensor.h @@ -11,6 +11,8 @@ u16 spi_rw_half_word(u32 spi_periph, const u16 i_HalfWord); int spi_rw_half_word_dma(uint32_t spi_periph, uint16_t tx_data, uint16_t *rx_data); +constexpr u16 VEL_FILTER_SIZE = 24; + class angle_sensor { public: angle_sensor(); @@ -18,13 +20,17 @@ class angle_sensor { float update(void); void reset(void); float getAngle(void) const; - + float getRawAngle(void) const; + float getVelocity(void) const; + float getRawVelocity(void) const; protected: void KTH7823_init(void); bool m_firstUpdate; float m_lastAngle; float m_cumulativeAngle; + float m_lastcumulativeAngle; + float velocity[VEL_FILTER_SIZE] = {0.0f}; }; #ifdef __cplusplus diff --git a/Robot_V2/main.cpp b/Robot_V2/main.cpp index dca68d2..af788eb 100644 --- a/Robot_V2/main.cpp +++ b/Robot_V2/main.cpp @@ -7,6 +7,7 @@ #include "core_cm4.h" #include "initial.h" #include "systick.h" +#include // 全局变量定义 CAN_transmiter CAN0_transmiter; // CAN初始化 @@ -22,9 +23,10 @@ int main(void) { watchdog_init(5000); // 看门狗初始化 angsens_MG_test(); // 角度传感器磁感应强度测试 - + while (1) { // delay_1ms(100); + // std::printf("1\n"); fwdgt_counter_reload(); // 看门狗复位 CAN0_transmiter.communication_check(); // CAN通信 }