diff --git a/Robot_V2/Algorithm/DengFOC.cpp b/Robot_V2/Algorithm/DengFOC.cpp index d08b43d..1788371 100644 --- a/Robot_V2/Algorithm/DengFOC.cpp +++ b/Robot_V2/Algorithm/DengFOC.cpp @@ -1,23 +1,41 @@ #include "DengFOC.h" +#include "Filter.h" #include "Timer.h" +#include "adc.h" +#include "pid.h" #include "systick.h" #include #include #include #include - constexpr double PI = 3.1415926535897932384626433832795f; +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 float sqrt3 = 1.73205080757f; // sqrt(3)的预计算值 +constexpr float zero_electric_angle = 0.0f; + +extern sampler sampler; + +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) \ ((amt) < (low) ? (low) : ((amt) > (high) ? (high) : (amt))) #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; } +inline float _electricalAngle(float shaft_angle) { + return _normalizeAngle((float)(DIR * PP) * shaft_angle - + zero_electric_angle); +} /** * @brief 归一化角度 @@ -90,7 +108,7 @@ float FOC_Controlroller::velocity_open_loop(float target_velocity) { angle_el = _normalizeAngle(angle_el); // 因为在开环控制中，Ud始终为0，只控制Uq - const float Uq = limit_voltage; + const float Uq = voltage_power_supply / 2.0f; const float Ud = 0.0f; // 帕克逆变换 (直接计算，避免函数调用开销) @@ -106,9 +124,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); + Ua = constrain(Ua, 0.0f, voltage_power_supply, 0.0f, limit_voltage * 2); + Ub = constrain(Ub, 0.0f, voltage_power_supply, 0.0f, limit_voltage * 2); + Uc = constrain(Uc, 0.0f, voltage_power_supply, 0.0f, limit_voltage * 2); 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); @@ -141,4 +159,60 @@ void FOC_Controlroller::shut_down() { void FOC_Controlroller::resume() { is_shut_down = 0; gpio_bit_set(GPIOB, GPIO_PIN_12); -} \ No newline at end of file +} + +// 通过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::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); + // 帕克逆变换 + 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()); +} diff --git a/Robot_V2/Algorithm/DengFOC.h b/Robot_V2/Algorithm/DengFOC.h index bd08c8a..e35569a 100644 --- a/Robot_V2/Algorithm/DengFOC.h +++ b/Robot_V2/Algorithm/DengFOC.h @@ -1,9 +1,3 @@ -// 灯哥开源，遵循GNU协议，转载请著名版权！ -// GNU开源协议（GNU General Public License, -// GPL）是一种自由软件许可协议，保障用户能够自由地使用、研究、分享和修改软件。 -// 该协议的主要特点是，要求任何修改或衍生的作品必须以相同的方式公开发布，即必须开源。此外，该协议也要求在使用或分发软件时，必须保留版权信息和许可协议。GNU开源协议是自由软件基金会（FSF）制定和维护的一种协议，常用于GNU计划的软件和其他自由软件中。 -// 仅在DengFOC官方硬件上测试过，欢迎硬件购买/支持作者，淘宝搜索店铺：灯哥开源 -// 你的支持将是接下来做视频和持续开源的经费，灯哥在这里先谢谢大家了 // 函数声明 #ifndef _DENGFOC_H #define _DENGFOC_H @@ -13,7 +7,7 @@ class FOC_Controlroller { FOC_Controlroller(); float control_mode = 0; - float limit_voltage = 2.0f; + float limit_voltage = 0.0f; float limit_velocity = 0.0f; float target_angle = 0.0f; float target_velocity = 0.0f; @@ -30,10 +24,13 @@ class FOC_Controlroller { float shaft_angle = 0; float Ualpha, Ubeta = 0, Ua = 0, Ub = 0, Uc = 0; float zero_electric_angle = 0; - float voltage_power_supply = 12.0f; // 电源电压 + float voltage_power_supply = 24.0f; // 电源电压 void driver_output(float Ua, float Ub, float Uc); 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); @@ -42,4 +39,4 @@ float _electricalAngle(float shaft_angle); float DFOC_M0_Angle(); float serial_motor_target(); -#endif +#endif \ No newline at end of file diff --git a/Robot_V2/Algorithm/pid.cpp b/Robot_V2/Algorithm/pid.cpp new file mode 100644 index 0000000..3b836e3 --- /dev/null +++ b/Robot_V2/Algorithm/pid.cpp @@ -0,0 +1,52 @@ +#include "pid.h" +#include "Timer.h" +#define _constrain(amt,low,high) ((amt)<(low)?(low):((amt)>(high)?(high):(amt))) + + +PIDController::PIDController(float P, float I, float D, float ramp, float limit) + : P(P) + , I(I) + , D(D) + , output_ramp(ramp) // PID控制器加速度限幅 + , limit(limit) // PID控制器输出限幅 + , error_prev(0.0f) + , output_prev(0.0f) + , integral_prev(0.0f) +{ + timestamp_prev = get_system_runtime_us() ; +} + +// PID 控制器函数 +float PIDController::operator() (float error){ + // 计算两次循环中间的间隔时间 + unsigned long timestamp_now = get_system_runtime_us(); + float Ts = (timestamp_now - timestamp_prev) * 1e-6f; + if(Ts <= 0 || Ts > 0.5f) Ts = 1e-3f; + + // P环 + float proportional = P * error; + // Tustin 散点积分（I环） + float integral = integral_prev + I*Ts*0.5f*(error + error_prev); + integral = _constrain(integral, -limit, limit); + // D环（微分环节） + float derivative = D*(error - error_prev)/Ts; + + // 将P,I,D三环的计算值加起来 + float output = proportional + integral + derivative; + output = _constrain(output, -limit, limit); + + if(output_ramp > 0){ + // 对PID的变化速率进行限制 + float output_rate = (output - output_prev)/Ts; + if (output_rate > output_ramp) + output = output_prev + output_ramp*Ts; + else if (output_rate < -output_ramp) + output = output_prev - output_ramp*Ts; + } + // 保存值（为了下一次循环） + integral_prev = integral; + output_prev = output; + error_prev = error; + timestamp_prev = timestamp_now; + return output; +} \ No newline at end of file diff --git a/Robot_V2/Robot_V2.cproject.yml b/Robot_V2/Robot_V2.cproject.yml index 3e94352..97f75c2 100644 --- a/Robot_V2/Robot_V2.cproject.yml +++ b/Robot_V2/Robot_V2.cproject.yml @@ -27,6 +27,8 @@ project: - file: Algorithm/Filter.h - file: Algorithm/DengFOC.cpp - file: Algorithm/DengFOC.h + - file: Algorithm/pid.cpp + - file: pid.h components: - component: ARM::CMSIS:CORE - component: Device:Startup diff --git a/Robot_V2/device/ADC.cpp b/Robot_V2/device/ADC.cpp index 406e3a7..1268fb2 100644 --- a/Robot_V2/device/ADC.cpp +++ b/Robot_V2/device/ADC.cpp @@ -1,14 +1,16 @@ #include "adc.h" #include "Filter.h" #include "math.h" -#include #include +#include #include constexpr float ERROR_ADC_VAL = 255.0f; constexpr float ADC_SCALE = 3.3f / 4096.0f; constexpr float TEMP_FACTOR = 1000.0f / 4.3f; constexpr float VCC_FACTOR = 11.0f; +// constexpr float CURRENT_OFFSET = 0 - 4096.0f / 2.0f; +constexpr float CURRENT_OFFSET = 0; constexpr float CURRENT_FACTOR = 1.0f / 50.0f * 100.0f; // 50倍放大，100倍电流采样电阻, 单位mA constexpr float MAX_TEMP = 150.0f; @@ -18,18 +20,23 @@ constexpr float MIN_CURRENT = -0.1f; constexpr float MAX_VCC = 65.0f; constexpr float MIN_VCC = -0.1f; -u16 adc_value[7] = {0}; // 显式初始化数组元素为0 +u16 adc_value[6] = {0}; // 显式初始化数组元素为0 /** * @brief 采样类初始化 */ void sampler::init() { - for (int i = 0; i < 7; i++) { + for (int i = 0; i < FILTER_SIZE + 1; i++) { mcu_temp_samples[i] = (1.43f - adc_value[0] * ADC_SCALE) * TEMP_FACTOR + 35.0f; - currU_samples[i] = adc_value[2] * ADC_SCALE * CURRENT_FACTOR; - currV_samples[i] = adc_value[3] * ADC_SCALE * CURRENT_FACTOR; - currW_samples[i] = adc_value[4] * ADC_SCALE * CURRENT_FACTOR; + + currU_samples[i] = + (adc_value[2] * ADC_SCALE + CURRENT_OFFSET) * CURRENT_FACTOR; + currV_samples[i] = + (adc_value[3] * ADC_SCALE + CURRENT_OFFSET) * CURRENT_FACTOR; + currW_samples[i] = + (adc_value[4] * ADC_SCALE + CURRENT_OFFSET) * CURRENT_FACTOR; + vbus_samples[i] = adc_value[5] * ADC_SCALE * VCC_FACTOR; float R_now = (float)adc_value[1] / (4096 - (float)adc_value[1]) * 4700; @@ -43,13 +50,13 @@ void sampler::init() { * 温度值，单位为摄氏度，范围应在-40到145之间，如果读取失败则返回0.0f */ float sampler::get_mcu_temp() { - tempora_sample = medium_filter(mcu_temp_samples, 7); + tempora_sample = medium_filter(mcu_temp_samples, FILTER_SIZE + 1); // 检查温度读数是否在有效范围内 if (tempora_sample < MIN_TEMP || tempora_sample > MAX_TEMP) { return ERROR_ADC_VAL; } return tempora_sample; -} +} /** * @brief 获取电机驱动温度 @@ -59,7 +66,7 @@ float sampler::get_mcu_temp() { * 如果计算结果超出有效范围(-40到255摄氏度)，则重新采样计算。 */ float sampler::get_drv_temp() { - tempora_sample = medium_filter(drv_temp_samples, 7); + tempora_sample = medium_filter(drv_temp_samples, FILTER_SIZE + 1); // 检查温度读数是否在有效范围内 if (tempora_sample < MIN_TEMP || tempora_sample > MAX_TEMP) { return ERROR_ADC_VAL; @@ -77,15 +84,15 @@ float sampler::get_drv_temp() { float sampler::get_current(char type) { switch (type) { case 'U': { - tempora_sample = medium_filter(currU_samples, 7); + tempora_sample = medium_filter(currU_samples, FILTER_SIZE + 1); break; } case 'V': { - tempora_sample = medium_filter(currV_samples, 7); + tempora_sample = medium_filter(currV_samples, FILTER_SIZE + 1); break; } case 'W': { - tempora_sample = medium_filter(currW_samples, 7); + tempora_sample = medium_filter(currW_samples, FILTER_SIZE + 1); break; } default: { @@ -106,11 +113,13 @@ float sampler::get_current(char type) { * 如果计算结果超出有效范围(-40到255伏特)，则重新采样计算。 */ float sampler::get_vbus_voltage() { + tempora_sample = medium_filter(vbus_samples, FILTER_SIZE + 1); // 检查电压读数是否在有效范围内 - if (vbus_samples[4] > MAX_VCC || vbus_samples[4] < MIN_VCC) { + if (vbus_samples[FILTER_SIZE / 2] > MAX_VCC || + vbus_samples[FILTER_SIZE / 2] < MIN_VCC) { return ERROR_ADC_VAL; } - return vbus_samples[4]; + return tempora_sample; } /** @@ -120,36 +129,41 @@ float sampler::get_vbus_voltage() { * 如果计算结果超出有效范围(-40到255摄氏度)，则重新采样计算。 */ void sampler::update_mcu_temp() { - memmove(mcu_temp_samples, mcu_temp_samples + 1, 6 * sizeof(float)); - mcu_temp_samples[6] = (1.43f - adc_value[0] * ADC_SCALE) * TEMP_FACTOR + 35.0f; + memmove(mcu_temp_samples, mcu_temp_samples + 1, FILTER_SIZE * sizeof(float)); + mcu_temp_samples[FILTER_SIZE] = + (1.43f - adc_value[0] * ADC_SCALE) * TEMP_FACTOR + 35.0f; } /** * @brief 更新MCU温度 */ void sampler::update_drv_temp() { - memmove(drv_temp_samples, drv_temp_samples + 1, 6 * sizeof(float)); + memmove(drv_temp_samples, drv_temp_samples + 1, FILTER_SIZE * sizeof(float)); float R_now = (float)adc_value[1] / (4096 - (float)adc_value[1]) * 4700; - drv_temp_samples[6] = 1.0f / (0.00335f + 0.00025f * log(R_now / 10000.0f)) - 273.15f; + drv_temp_samples[FILTER_SIZE] = + 1.0f / (0.00335f + 0.00025f * log(R_now / 10000.0f)) - 273.15f; } /** * @brief 更新相电流 */ void sampler::update_current() { - memmove(currU_samples, currU_samples + 1, 6 * sizeof(float)); - memmove(currV_samples, currV_samples + 1, 6 * sizeof(float)); - memmove(currW_samples, currW_samples + 1, 6 * sizeof(float)); - - currU_samples[6] = adc_value[2] * ADC_SCALE; - currV_samples[6] = adc_value[3] * ADC_SCALE; - currW_samples[6] = adc_value[4] * ADC_SCALE; + memmove(currU_samples, currU_samples + 1, FILTER_SIZE * sizeof(float)); + memmove(currV_samples, currV_samples + 1, FILTER_SIZE * sizeof(float)); + memmove(currW_samples, currW_samples + 1, FILTER_SIZE * sizeof(float)); + + currU_samples[FILTER_SIZE] = + (adc_value[2] * ADC_SCALE + CURRENT_OFFSET) * CURRENT_FACTOR; + currV_samples[FILTER_SIZE] = + (adc_value[3] * ADC_SCALE + CURRENT_OFFSET) * CURRENT_FACTOR; + currW_samples[FILTER_SIZE] = + (adc_value[4] * ADC_SCALE + CURRENT_OFFSET) * CURRENT_FACTOR; } /** * @brief 更新供电电压 */ void sampler::update_vbus_voltage() { - memmove(vbus_samples, vbus_samples + 1, 6 * sizeof(float)); - vbus_samples[6] = adc_value[5] * ADC_SCALE; + memmove(vbus_samples, vbus_samples + 1, FILTER_SIZE * sizeof(float)); + vbus_samples[FILTER_SIZE] = adc_value[5] * ADC_SCALE; } \ No newline at end of file diff --git a/Robot_V2/device/adc.h b/Robot_V2/device/adc.h index c27027f..59a0e28 100644 --- a/Robot_V2/device/adc.h +++ b/Robot_V2/device/adc.h @@ -6,29 +6,29 @@ extern "C" { #endif #include "stdint.h" - +constexpr u8 FILTER_SIZE = 6; class sampler { - private: - float vbus_samples[7]; - float currU_samples[7]; - float currV_samples[7]; - float currW_samples[7]; - float mcu_temp_samples[7]; - float drv_temp_samples[7]; + private: + float vbus_samples[FILTER_SIZE + 1]; + float currU_samples[FILTER_SIZE + 1]; + float currV_samples[FILTER_SIZE + 1]; + float currW_samples[FILTER_SIZE + 1]; + float mcu_temp_samples[FILTER_SIZE + 1]; + float drv_temp_samples[FILTER_SIZE + 1]; float tempora_sample; + public: void init(); void update_mcu_temp(void); void update_drv_temp(void); void update_vbus_voltage(void); void update_current(void); - + float get_mcu_temp(void); float get_drv_temp(void); float get_vbus_voltage(void); float get_current(char); - }; #ifdef __cplusplus diff --git a/Robot_V2/device/tle5012b.cpp b/Robot_V2/device/tle5012b.cpp index 1ec20e3..e77a899 100644 --- a/Robot_V2/device/tle5012b.cpp +++ b/Robot_V2/device/tle5012b.cpp @@ -5,335 +5,131 @@ #include "stdbool.h" #include "systick.h" -#define SPI_TX_OFF() \ - do { \ - gpio_init(GPIOB, GPIO_MODE_IN_FLOATING, GPIO_OSPEED_50MHZ, GPIO_PIN_5); \ - } while(0) -#define SPI_TX_ON() \ - do { \ - gpio_init(GPIOB, GPIO_MODE_AF_PP, GPIO_OSPEED_50MHZ, GPIO_PIN_5); \ - } while(0) +#define SPI_TX_OFF() \ + do { \ + gpio_init(GPIOB, GPIO_MODE_IN_FLOATING, GPIO_OSPEED_50MHZ, \ + GPIO_PIN_5); \ + } while (0) +#define SPI_TX_ON() \ + do { \ + gpio_init(GPIOB, GPIO_MODE_AF_PP, GPIO_OSPEED_50MHZ, GPIO_PIN_5); \ + } while (0) -#define SPI_CS_ENABLE() gpio_bit_reset(GPIOA, GPIO_PIN_15) +#define SPI_CS_ENABLE() gpio_bit_reset(GPIOA, GPIO_PIN_15) #define SPI_CS_DISABLE() gpio_bit_set(GPIOA, GPIO_PIN_15) - -/** - * @brief SPI command definitions for TLI5012B sensor - * @note All commands are 16-bit values following the TLI5012B protocol - */ - -/* SPI operation types */ -static constexpr uint16_t WRITE = 0; /**< SPI Write Operation */ -static constexpr uint16_t READ = 1; /**< SPI Read Operation */ - -/* SPI communication constants */ -static constexpr uint16_t DUMMY_BYTE = 0xFFFF; /**< Dummy byte for SPI read operations */ - -static constexpr uint16_t WR_REG = 0x0000; /**< Write command - MSB is 0 */ -static constexpr uint16_t RD_REG = 0x8000; /**< Read command - MSB is 1 */ - -static constexpr uint16_t LockValue_LADDR = 0x0000; /**< Lock value for addresses 0x00-0x04 */ -static constexpr uint16_t LockValue_HADDR = 0x5000; /**< Lock value for addresses 0x05-0x11 */ - -/* Update buffer access command */ -static constexpr uint16_t UPD_CMD = 0x0400; /**< Update command - bit 10 is 1, access update buffer */ - -/* Read command values */ -static constexpr uint16_t READ_STAT_VALUE = 0x8001; /**< Read status register */ -static constexpr uint16_t READ_ACSTAT_VALUE = 0x8011; /**< Read activation status register */ -static constexpr uint16_t READ_ANGLE_VALUE = 0x8021; /**< Read angle value register */ -static constexpr uint16_t READ_SPEED_VALUE = 0x8031; /**< Read speed value register */ - -/* Write command values and their corresponding data */ -static constexpr uint16_t WRITE_MOD1_VALUE = 0x5061; /**< Write to MOD1 register (0_1010_0_000110_0001) */ -static constexpr uint16_t MOD1_VALUE = 0x0001; /**< MOD1 register configuration value */ - -static constexpr uint16_t WRITE_MOD2_VALUE = 0x5081; /**< Write to MOD2 register (0_1010_0_001000_0001) */ -static constexpr uint16_t MOD2_VALUE = 0x0809; /**< MOD2 register configuration value */ - -static constexpr uint16_t WRITE_MOD3_VALUE = 0x5091; /**< Write to MOD3 register (0_1010_0_001001_0001) */ -static constexpr uint16_t MOD3_VALUE = 0x0000; /**< MOD3 register configuration value */ - -static constexpr uint16_t WRITE_MOD4_VALUE = 0x50E1; /**< Write to MOD4 register (0_1010_0_001110_0001) */ -static constexpr uint16_t MOD4_VALUE = 0x0080; /**< MOD4 register value: 12-bit 4096, absolute count disable 0x0080; enable 0x0000 */ - /** * @brief Send or read half word through SPI * * @param[in] spi_periph SPI peripheral * @param[in] i_HalfWord data */ -uint16_t spi_rw_half_word(uint32_t spi_periph, const uint16_t i_HalfWord) -{ - uint32_t to; - uint16_t data = 0xFFFF; - bool ok = true; +uint16_t spi_rw_half_word(uint32_t spi_periph, const uint16_t i_HalfWord) { + uint32_t to; + uint16_t data = 0xFFFF; + bool ok = true; - /* ---------- Send data ---------- */ - to = 200; - while ((RESET == spi_i2s_flag_get(spi_periph, SPI_FLAG_TBE)) && --to); - if (to == 0) ok = false; + /* ---------- Send data ---------- */ + to = 200; + while ((RESET == spi_i2s_flag_get(spi_periph, SPI_FLAG_TBE)) && --to) + ; + if (to == 0) + ok = false; - if (ok) - { - if(RESET == spi_i2s_flag_get(spi_periph, SPI_FLAG_RBNE)) - { - (void)spi_i2s_data_receive(spi_periph); /* throw Dummy */ - } - spi_i2s_data_transmit(spi_periph, i_HalfWord); - /* wait rx succ*/ - to = 200; - while ((RESET == spi_i2s_flag_get(spi_periph, SPI_FLAG_RBNE)) && --to); - if (to == 0) ok = false; - else - data = spi_i2s_data_receive(spi_periph); /* throw Dummy */ - } + if (ok) { + if (RESET == spi_i2s_flag_get(spi_periph, SPI_FLAG_RBNE)) { + (void)spi_i2s_data_receive(spi_periph); /* throw Dummy */ + } + spi_i2s_data_transmit(spi_periph, i_HalfWord); + /* wait rx succ*/ + to = 200; + while ((RESET == spi_i2s_flag_get(spi_periph, SPI_FLAG_RBNE)) && --to) + ; + if (to == 0) + ok = false; + else + data = spi_i2s_data_receive(spi_periph); /* throw Dummy */ + } - /* ---------- wait bsu idle ---------- */ - if (ok) - { - to = 200; - while ((SET == spi_i2s_flag_get(spi_periph, SPI_FLAG_TRANS)) \ - && SET == spi_i2s_flag_get(spi_periph, SPI_FLAG_RBNE) && --to); - if (to == 0) ok = false; - } + /* ---------- wait bsu idle ---------- */ + if (ok) { + to = 200; + while ((SET == spi_i2s_flag_get(spi_periph, SPI_FLAG_TRANS)) && + SET == spi_i2s_flag_get(spi_periph, SPI_FLAG_RBNE) && --to) + ; + if (to == 0) + ok = false; + } - return ok ? data : 0xFFFF; + return ok ? data : 0xFFFF; } -/** - * @brief Write data to TLI5012B register - * @param i_Cmd Command word to send - * @param i_Data Data to write to register - * @return Register response value, 0xFFFF if error - * @note This function performs a complete SPI write transaction: - */ -uint16_t cdd_TLI5012_WriteReg(uint16_t i_Cmd, uint16_t i_Data) -{ - uint16_t result = 0xFFFF; /* Default error code */ - SPI_CS_ENABLE(); - __NOP(); __NOP(); __NOP(); // 添加小延时确保CS稳定 - result = spi_rw_half_word(SPI0, i_Cmd); - result = spi_rw_half_word(SPI0, i_Data); - SPI_TX_OFF(); /* Configure MOSI as input to avoid conflicts */ - __NOP();__NOP();__NOP();__NOP();__NOP();__NOP(); - __NOP();__NOP();__NOP();__NOP();__NOP();__NOP(); // 增加延时 - result = spi_rw_half_word(SPI0, DUMMY_BYTE); - SPI_CS_DISABLE(); - SPI_TX_ON(); /* Restore MOSI as output */ - __NOP(); __NOP(); __NOP(); // 添加小延时确保CS稳定 - return result; -} - -/** - * @brief Read data from TLI5012B register - * @param i_Cmd Command word to send - * @param i_Data Not used in read operation (kept for compatibility) - * @return Register data value, 0xFFFF if error - * @note This function performs a complete SPI read transaction: - */ -uint16_t cdd_TLI5012_ReadReg(uint16_t i_Cmd, uint16_t * const i_Data) -{ - uint16_t result = 0xFFFF; /* Default error code */ - SPI_CS_ENABLE(); - __NOP(); __NOP(); __NOP(); // 添加小延时确保CS稳定 - *i_Data = spi_rw_half_word(SPI0, i_Cmd); - SPI_TX_OFF(); /* Configure MOSI as input to avoid conflicts */ - __NOP();__NOP();__NOP();__NOP();__NOP();__NOP(); - __NOP();__NOP();__NOP();__NOP();__NOP();__NOP(); // 增加延时 - *i_Data = spi_rw_half_word(SPI0, DUMMY_BYTE); - result = spi_rw_half_word(SPI0, DUMMY_BYTE); - SPI_CS_DISABLE(); - SPI_TX_ON(); /* Restore MOSI as output */ - __NOP(); __NOP(); __NOP(); // 添加小延时确保CS稳定 - return result; -} - -void tle5012b_gpio_init(void) -{ +void KHT7823_gpio_init(void) { rcu_periph_clock_enable(RCU_GPIOA); - rcu_periph_clock_enable(RCU_GPIOB); - rcu_periph_clock_enable(RCU_SPI0); - rcu_periph_clock_enable(RCU_AF); - /* 关闭 JTAG，仅保留 SWD（释放 PA15、PB3、PB4） */ - gpio_pin_remap_config(GPIO_SWJ_SWDPENABLE_REMAP, ENABLE); - - /* -------- SPI0 全 remap：PB3/4/5 + PA15 -------- */ - gpio_pin_remap_config(GPIO_SPI0_REMAP, ENABLE); - /* PA15 → NSS, 推挽输出 */ - gpio_init(GPIOA, GPIO_MODE_OUT_PP, GPIO_OSPEED_50MHZ, GPIO_PIN_15); - /* PB3 → SCK, PB5 → MOSI, 推挽输出 */ - gpio_init(GPIOB, GPIO_MODE_AF_PP, GPIO_OSPEED_50MHZ, GPIO_PIN_3 | GPIO_PIN_5); - /* PB4 → MISO, 浮空输入 */ - gpio_init(GPIOB, GPIO_MODE_IN_FLOATING, GPIO_OSPEED_50MHZ, GPIO_PIN_4 | GPIO_PIN_10); + rcu_periph_clock_enable(RCU_GPIOB); + rcu_periph_clock_enable(RCU_SPI0); + rcu_periph_clock_enable(RCU_AF); + /* 关闭 JTAG，仅保留 SWD（释放 PA15、PB3、PB4） */ + gpio_pin_remap_config(GPIO_SWJ_SWDPENABLE_REMAP, ENABLE); + /* -------- SPI0 全 remap：PB3/4/5 + PA15 -------- */ + gpio_pin_remap_config(GPIO_SPI0_REMAP, ENABLE); + /* PA15 → NSS, 推挽输出 */ + gpio_init(GPIOA, GPIO_MODE_OUT_PP, GPIO_OSPEED_50MHZ, GPIO_PIN_15); + /* PB3 → SCK, PB5 → MOSI, 推挽输出 */ + gpio_init(GPIOB, GPIO_MODE_AF_PP, GPIO_OSPEED_50MHZ, + GPIO_PIN_3 | GPIO_PIN_5); + /* PB4 → MISO, 浮空输入 */ + gpio_init(GPIOB, GPIO_MODE_IN_FLOATING, GPIO_OSPEED_50MHZ, + GPIO_PIN_4 | GPIO_PIN_10); } /** * @brief Initialize SPI module * * @details Configures SPI hardware and initializes necessary settings */ -void tle5012b_spi_init(void) -{ - spi_parameter_struct spi_init_str; - spi_struct_para_init(&spi_init_str); +void KHT7823_spi_init(void) { + spi_parameter_struct spi_init_str; + spi_struct_para_init(&spi_init_str); - /* SPI参数：16bit, 主机, 全双工, CPOL=0 CPHA=1, MSB first */ - spi_init_str.device_mode = SPI_MASTER; - spi_init_str.trans_mode = SPI_TRANSMODE_FULLDUPLEX; - spi_init_str.frame_size = SPI_FRAMESIZE_16BIT; - spi_init_str.clock_polarity_phase = SPI_CK_PL_HIGH_PH_2EDGE; - spi_init_str.nss = SPI_NSS_SOFT; - spi_init_str.endian = SPI_ENDIAN_MSB; + /* SPI参数：16bit, 主机, 全双工, CPOL=0 CPHA=1, MSB first */ + spi_init_str.device_mode = SPI_MASTER; + spi_init_str.trans_mode = SPI_TRANSMODE_FULLDUPLEX; + spi_init_str.frame_size = SPI_FRAMESIZE_16BIT; + spi_init_str.clock_polarity_phase = SPI_CK_PL_HIGH_PH_2EDGE; + spi_init_str.nss = SPI_NSS_SOFT; + spi_init_str.endian = SPI_ENDIAN_MSB; - spi_init_str.prescale = SPI_PSC_16; /* 8MHz @ 128MHz */ - spi_init(SPI0, &spi_init_str); - spi_nss_output_enable(SPI0); - spi_nssp_mode_enable(SPI0); - spi_enable(SPI0); -} - - - -/** - * @brief Initialize TLI5012B magnetic angle sensor - * @note This function configures the sensor registers for proper operation - * @return None - */ -void cdd_TLI5012_Init(void) -{ - uint16_t u16Command = 0xFFFF; - uint16_t u16Data = 0x0000; - uint16_t u16Safe = 0x0000; - - /* ---------- Step 1: Read and configure STAT register ---------- */ - /* Read STAT register to get current status */ - u16Command = READ_STAT_VALUE; - u16Safe = cdd_TLI5012_ReadReg(u16Command, &u16Data); - printf("\r\n @Joe [TLI5012Init] [STAT] read u16Data: 0x%04X SafeWord: 0x%04X \r\n", u16Data, u16Safe); - - /* Write STAT register, set Slave_Number = 00 */ - u16Command = 0x0001; - u16Data &= 0x9FFF; /* Clear bits 13-14 (Slave_Number) */ - u16Safe = cdd_TLI5012_WriteReg(u16Command, u16Data); - printf("\r\n @Joe [TLI5012Init] [STAT] write u16Data: 0x%04X SafeWord: 0x%04X \r\n", u16Data, u16Safe); - - /* Read STAT register again to verify write operation */ - u16Command = READ_STAT_VALUE; - u16Safe = cdd_TLI5012_ReadReg(u16Command, &u16Data); - printf("\r\n @Joe [TLI5012Init] [STAT] read u16Data: 0x%04X SafeWord: 0x%04X \r\n", u16Data, u16Safe); - - /* ---------- Step 2: Read ACSTAT register ---------- */ - u16Command = READ_ACSTAT_VALUE; - u16Safe = cdd_TLI5012_ReadReg(u16Command, &u16Data); - printf("\r\n @Joe [TLI5012Init] read [ACSTAT]: 0x%04X SafeWord: 0x%04X \r\n", u16Data, u16Safe); - - /* ---------- Step 3: Configure MOD1 register ---------- */ - /* Read current MOD1 register value */ - u16Command = RD_REG | WRITE_MOD1_VALUE; - u16Safe = cdd_TLI5012_ReadReg(u16Command, &u16Data); - printf("\r\n @Joe [TLI5012Init] read MOD1: 0x%04X, SafeWord: 0x%04X \r\n", u16Data, u16Safe); - - /* Write MOD1 register with configuration value */ - u16Command = WRITE_MOD1_VALUE; - u16Data = MOD1_VALUE; - u16Safe = cdd_TLI5012_WriteReg(u16Command, u16Data); - printf("\r\n @Joe [TLI5012Init] [MOD1] write u16Data: 0x%04X, SafeWord: 0x%04X \r\n", u16Data, u16Safe); - - /* Verify MOD1 register write operation */ - u16Command = RD_REG | WRITE_MOD1_VALUE; - u16Safe = cdd_TLI5012_ReadReg(u16Command, &u16Data); - printf("\r\n @Joe [TLI5012Init] read MOD1: 0x%04X, SafeWord: 0x%04X \r\n", u16Data, u16Safe); - - /* ---------- Step 4: Configure MOD2 register ---------- */ - /* Read current MOD2 register value */ - u16Command = RD_REG | WRITE_MOD2_VALUE; - u16Safe = cdd_TLI5012_ReadReg(u16Command, &u16Data); - printf("\r\n @Joe [TLI5012Init] read MOD2: 0x%04X, SafeWord: 0x%04X \r\n", u16Data, u16Safe); - - /* Write MOD2 register with configuration value */ - u16Command = WRITE_MOD2_VALUE; - u16Data = MOD2_VALUE; - u16Safe = cdd_TLI5012_WriteReg(u16Command, u16Data); - printf("\r\n @Joe [TLI5012Init] [MOD2] write u16Data: 0x%04X, SafeWord: 0x%04X \r\n", u16Data, u16Safe); - - /* Verify MOD2 register write operation */ - u16Command = RD_REG | WRITE_MOD2_VALUE; - u16Safe = cdd_TLI5012_ReadReg(u16Command, &u16Data); - printf("\r\n @Joe [TLI5012Init] read MOD2: 0x%04X, SafeWord: 0x%04X \r\n", u16Data, u16Safe); - - /* ---------- Step 5: Configure MOD3 register ---------- */ - /* Read current MOD3 register value */ - u16Command = RD_REG | WRITE_MOD3_VALUE; - u16Safe = cdd_TLI5012_ReadReg(u16Command, &u16Data); - printf("\r\n @Joe [TLI5012Init] read MOD3: 0x%04X, SafeWord: 0x%04X \r\n", u16Data, u16Safe); - - /* Write MOD3 register with configuration value */ - u16Command = WRITE_MOD3_VALUE; - u16Data = MOD3_VALUE; - u16Safe = cdd_TLI5012_WriteReg(u16Command, u16Data); - printf("\r\n @Joe [TLI5012Init] [MOD3] write u16Data: 0x%04X, SafeWord: 0x%04X \r\n", u16Data, u16Safe); - - /* Verify MOD3 register write operation */ - u16Command = RD_REG | WRITE_MOD3_VALUE; - u16Safe = cdd_TLI5012_ReadReg(u16Command, &u16Data); - printf("\r\n @Joe [TLI5012Init] read MOD3: 0x%04X, SafeWord: 0x%04X \r\n", u16Data, u16Safe); - - /* ---------- Step 6: Configure MOD4 register ---------- */ - /* Read current MOD4 register value */ - u16Command = RD_REG | WRITE_MOD4_VALUE; - u16Safe = cdd_TLI5012_ReadReg(u16Command, &u16Data); - printf("\r\n @Joe [TLI5012Init] read MOD4: 0x%04X, SafeWord: 0x%04X \r\n", u16Data, u16Safe); - - /* Write MOD4 register with configuration value */ - u16Command = WRITE_MOD4_VALUE; - u16Data = MOD4_VALUE; - u16Safe = cdd_TLI5012_WriteReg(u16Command, u16Data); - printf("\r\n @Joe [TLI5012Init] [MOD4] write u16Data: 0x%04X, SafeWord: 0x%04X \r\n", u16Data, u16Safe); - - /* Verify MOD4 register write operation */ - u16Command = RD_REG | WRITE_MOD4_VALUE; - u16Safe = cdd_TLI5012_ReadReg(u16Command, &u16Data); - printf("\r\n @Joe [TLI5012Init] read MOD4: 0x%04X, SafeWord: 0x%04X \r\n", u16Data, u16Safe); + spi_init_str.prescale = SPI_PSC_16; /* 8MHz @ 128MHz */ + spi_init(SPI0, &spi_init_str); + spi_nss_output_enable(SPI0); + spi_nssp_mode_enable(SPI0); + spi_enable(SPI0); } +void KHT7823_init(void) { + KHT7823_gpio_init(); + KHT7823_spi_init(); +} +float KHT7823_readAngle() { + u16 u16Command = 0x4000; -void Test_TLI5012B(void) -{ - uint16_t u16Command = 0xFFFF; - uint16_t u16Data = 0x0000; - uint16_t u16Safe = 0x0000; - float angle = 0; - u16Command = READ_ANGLE_VALUE; - u16Safe = cdd_TLI5012_ReadReg(u16Command, &u16Data); - - // 输出原始读取的数据，用于诊断 - printf("\r\n @Joe RAW AVAL = 0x%04X, SafeWord = 0x%04X \r\n", u16Data, u16Safe); - - // 检查是否有错误标志位 - if (u16Safe != 0xFFFF && u16Safe != 0x0000) { - printf("\r\n @Joe Warning: Non-ideal SafeWord detected \r\n"); - } - - // 检查错误位（bit 15） - if (u16Data & 0x8000) { - printf("\r\n @Joe Error: Communication error detected (bit 15 set) \r\n"); - } - - // 提取角度值(15位) - u16Data = u16Data & 0x7FFF; - printf ( "\r\n @Joe AVAL = 0x%04X, SafeWord = 0x%04X \r\n", u16Data, u16Safe); - - // 将15位角度值转换为角度(0-360度) - // 0x7FFF = 32767 对应 360度 - angle = ((float)u16Data * 360.0f) / 32767.0f; - printf ( "\r\n @Joe angle = %f, AVL = %d \r\n", angle, (int16_t)u16Data); - - // 添加额外的传感器状态检查 - uint16_t status_data, status_safe; - u16Command = READ_STAT_VALUE; - status_safe = cdd_TLI5012_ReadReg(u16Command, &status_data); - printf("\r\n @Joe STATUS = 0x%04X, Status SafeWord = 0x%04X \r\n", status_data, status_safe); + float ang = 0; + + SPI_CS_ENABLE(); + // 2. 发送命令1，同时接收回答1 + SPI_DATA(SPI0) = u16Command; + while (RESET == spi_i2s_flag_get(SPI0, SPI_FLAG_TBE)) + ; // 等待发送缓冲区空 + while (RESET == spi_i2s_flag_get(SPI0, SPI_FLAG_RBNE)) + ; // 等待接收缓冲区满 + u16 u16data = SPI_DATA(SPI0); // 读取回答1 + + SPI_CS_DISABLE(); + + // 将15位角度值转换为角度(0-360度) + ang = ((float)u16data * 360.0f) / 32767.0f; + + return ang; } \ No newline at end of file diff --git a/Robot_V2/device/tle5012b.h b/Robot_V2/device/tle5012b.h index c921ba4..cf42134 100644 --- a/Robot_V2/device/tle5012b.h +++ b/Robot_V2/device/tle5012b.h @@ -6,13 +6,9 @@ extern "C" { #include "gd32c10x_eval.h" #include "stdio.h" -void tle5012b_gpio_init(void); -void tle5012b_spi_init(void); +void KHT7823_init(void); u16 spi_rw_half_word(u32 spi_periph, const u16 i_HalfWord); -u16 cdd_TLI5012_WriteReg(u16 i_Cmd, u16 i_Data); -u16 cdd_TLI5012_ReadReg(u16 i_Cmd, u16 *const i_Data); -void cdd_TLI5012_Init(void); -void Test_TLI5012B(void); +void KHT7823_readAngle(void); #ifdef __cplusplus } #endif diff --git a/Robot_V2/main.cpp b/Robot_V2/main.cpp index b900f21..b8aee29 100644 --- a/Robot_V2/main.cpp +++ b/Robot_V2/main.cpp @@ -15,7 +15,6 @@ #include // 全局变量定义 -CurrentKalman kalman(0.0f, 0.0026f, 4.8f, 0.0001f, 1e-6f, 0.02f); CAN_transmiter CAN0_transmiter; FOC_Controlroller FOC; sampler sampler; @@ -29,14 +28,14 @@ int main(void) { sampler.init(); // 采样器初始化 driver_init(); // 电机驱动初始化 - tle5012b_gpio_init(); // TLI5012B初始化 - tle5012b_spi_init(); + KHT7823_init(); watchdog_init(5000); while (1) { /* clear the FWDGT reset flag */ // std::printf("1\n"); + // KHT7823_init(); fwdgt_counter_reload(); CAN0_transmiter.communication_check(); } diff --git a/Robot_V2/pid.h b/Robot_V2/pid.h new file mode 100644 index 0000000..f0335cb --- /dev/null +++ b/Robot_V2/pid.h @@ -0,0 +1,24 @@ +#ifndef PID_H +#define PID_H + +class PIDController +{ +public: + PIDController(float P, float I, float D, float ramp, float limit); + ~PIDController() = default; + + float operator() (float error); + + float P; //!< 比例增益(P环增益) + float I; //!< 积分增益（I环增益） + float D; //!< 微分增益（D环增益） + float output_ramp; + float limit; +protected: + float error_prev; //!< 最后的跟踪误差值 + float output_prev; //!< 最后一个 pid 输出值 + float integral_prev; //!< 最后一个积分分量值 + unsigned long timestamp_prev; //!< 上次执行时间戳 +}; + +#endif \ No newline at end of file