diff --git a/Robot_V2/Algorithm/DengFOC.cpp b/Robot_V2/Algorithm/DengFOC.cpp index 2993eaa..7a1bac0 100644 --- a/Robot_V2/Algorithm/DengFOC.cpp +++ b/Robot_V2/Algorithm/DengFOC.cpp @@ -4,12 +4,9 @@ #include "Timer.h" #include "adc.h" #include "ang_sensor.h" -#include "gd32c10x_dbg.h" #include "initial.h" #include "pid.h" -#include "systick.h" #include -#include #include #include @@ -76,13 +73,13 @@ inline void FOC_Controlroller::setPhaseVoltage(float Uq, float Ud) { Uc = (-Ualpha - sqrt3 * Ubeta) * 0.5f + v_half; // 计算占空比 - 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); + float dc_a = _constrain(Ua / v_safe, 0.0f, 1.0f); + float dc_b = _constrain(Ub / v_safe, 0.0f, 1.0f); + float dc_c = _constrain(Uc / v_safe, 0.0f, 1.0f); // 设置PWM输出 - dc_a = dc_a * limit_voltage / voltage_power_supply; - dc_b = dc_b * limit_voltage / voltage_power_supply; - dc_c = dc_c * limit_voltage / voltage_power_supply; + dc_a = dc_a * limit_voltage / v_safe; + dc_b = dc_b * limit_voltage / v_safe; + dc_c = dc_c * limit_voltage / v_safe; Set_3Pwm(dc_a, dc_b, dc_c); // std::printf("PWM:%f,%f,%f\n", dc_a, dc_b, dc_c); } @@ -101,9 +98,9 @@ void FOC_Controlroller::shut_down() { * */ void FOC_Controlroller::resume() { - thermal_uncontrol = 0; - voltage_error = 0; - is_shut_down = 0; + thermal_uncontrol = false; + voltage_error = false; + is_shut_down = false; gpio_bit_set(GPIOB, GPIO_PIN_12); } /** @@ -112,7 +109,14 @@ void FOC_Controlroller::resume() { */ void FOC_Controlroller::update_power(void) { voltage_power_supply = sampler.get_vbus_voltage(); - v_half = voltage_power_supply * 0.5f; + if (voltage_power_supply < 36.0f and voltage_power_supply > 8.0f) { + voltage_error = false; + v_half = voltage_power_supply * 0.5f; + v_safe = voltage_power_supply * 0.95f; + } + else { + voltage_error = true; + } } /** @@ -158,7 +162,7 @@ float FOC_Controlroller::velocity_open_loop() { float FOC_Controlroller::angle_control(float target_angle) { // PID 控制计算 float Uq = angle_loop(target_angle - rt_angle); - + // 获取电机轴角度 shaft_angle = KTH7823.getcurrent_sensorangle() * DEG2RAD; // 设置相电压 diff --git a/Robot_V2/Algorithm/DengFOC.h b/Robot_V2/Algorithm/DengFOC.h index ed04b15..2b77a14 100644 --- a/Robot_V2/Algorithm/DengFOC.h +++ b/Robot_V2/Algorithm/DengFOC.h @@ -14,10 +14,10 @@ class FOC_Controlroller { float target_velocity = 0.0f; float rt_angle = 0.0f; float rt_velocity = 0.0f; - bool thermal_uncontrol = 0; - bool voltage_error = 0; - bool is_shut_down = 0; - float voltage_power_supply = 24.0f; // 电源电压 + bool thermal_uncontrol = false; + bool voltage_error = false; + bool is_shut_down = false; + float voltage_power_supply = 24.0f; // 电源电压 float velocity_open_loop(); float angle_control(float target_angle); @@ -36,10 +36,16 @@ class FOC_Controlroller { float Ualpha, Ubeta = 0, Ua = 0, Ub = 0, Uc = 0; float zero_electric_angle = 0; float v_half = voltage_power_supply * 0.5f; // 电源电压的一半 + float v_safe = voltage_power_supply * 0.95f; - PIDController vel_loop =PIDController{1.8f, 1.2f, 0.0002f, 1000000, voltage_power_supply}; - PIDController angle_loop = - PIDController{140.0f, 80.0f, 0.005f, 10000000, voltage_power_supply}; + // PIDController vel_loop =PIDController{1.8f, 1.2f, 0.0002f, 1000000, + // voltage_power_supply};//8V + // PIDController angle_loop = PIDController{140.0f, 80.0f, 0.005f, 10000000, + // voltage_power_supply}; // 8V + PIDController vel_loop = PIDController{1.0f, 0.85f, 0.0002f, 1000000, + voltage_power_supply}; // 12V + PIDController angle_loop = PIDController{70.0f, 75.0f, 0.0001f, 10000000, + voltage_power_supply}; // 12V void setPhaseVoltage(float Uq, float Ud); }; diff --git a/Robot_V2/Algorithm/Filter.cpp b/Robot_V2/Algorithm/Filter.cpp index d585394..dcae90f 100644 --- a/Robot_V2/Algorithm/Filter.cpp +++ b/Robot_V2/Algorithm/Filter.cpp @@ -2,6 +2,7 @@ #include "ang_sensor.h" #include #include +#include #include #include diff --git a/Robot_V2/Algorithm/Filter.h b/Robot_V2/Algorithm/Filter.h index 0fc085a..022c9dd 100644 --- a/Robot_V2/Algorithm/Filter.h +++ b/Robot_V2/Algorithm/Filter.h @@ -1,6 +1,7 @@ #ifndef _FILTER_H #define _FILTER_H +#include #ifdef __cplusplus extern "C" { #endif @@ -95,7 +96,6 @@ template class KalmanFilter { x += K * (measurement - x); // 更新状态估计 P = (1 - K) * P; // 更新误差协方差 - return x; } diff --git a/Robot_V2/Algorithm/pid.cpp b/Robot_V2/Algorithm/pid.cpp index 16a2006..7b9d5f8 100644 --- a/Robot_V2/Algorithm/pid.cpp +++ b/Robot_V2/Algorithm/pid.cpp @@ -1,5 +1,4 @@ #include "pid.h" -#include "Timer.h" #define _constrain(amt, low, high) \ ((amt) < (low) ? (low) : ((amt) > (high) ? (high) : (amt))) #include "Can.h" diff --git a/Robot_V2/Algorithm/thermal.cpp b/Robot_V2/Algorithm/thermal.cpp index a25faff..709769e 100644 --- a/Robot_V2/Algorithm/thermal.cpp +++ b/Robot_V2/Algorithm/thermal.cpp @@ -2,7 +2,6 @@ #include "Can.h" #include "DengFOC.h" #include "adc.h" -#include extern sampler sampler; extern CAN_transmiter CAN0_transmiter; @@ -12,7 +11,7 @@ extern FOC_Controlroller FOC; ((a) > (b) ? ((a) > (c) ? (a) : (c)) : ((b) > (c) ? (b) : (c))) constexpr u16 PROTECT_TIME = 6000; // 过热保护时间，单位*10ms -u16 ThermalController::update() { +u32 ThermalController::update() { thermal_value = thermal_value * thermalRate; thermal_value += SQUARE(sampler.get_drv_temp()) + SQUARE(sampler.get_mcu_temp()); @@ -38,5 +37,5 @@ u16 ThermalController::update() { remainingTime10Ms = PROTECT_TIME; // 过热停机10秒 return remainingTime10Ms; } - return remainingTime10Ms; + return thermal_value; } \ No newline at end of file diff --git a/Robot_V2/Algorithm/thermal.h b/Robot_V2/Algorithm/thermal.h index ab17786..db05cec 100644 --- a/Robot_V2/Algorithm/thermal.h +++ b/Robot_V2/Algorithm/thermal.h @@ -17,7 +17,7 @@ class ThermalController { u16 gapTimeMs = 10; // 单次计算间隔ms u16 remainingTime10Ms = 0; // 过热冷却剩余ms public: - u16 update(); + u32 update(); void reset() { thermal_value = 0; remainingTime10Ms = 0; diff --git a/Robot_V2/device/ADC.cpp b/Robot_V2/device/ADC.cpp index 87a8d39..4b9d56a 100644 --- a/Robot_V2/device/ADC.cpp +++ b/Robot_V2/device/ADC.cpp @@ -1,8 +1,6 @@ #include "adc.h" #include "Filter.h" #include "Can.h" -#include "gd32c10x_can.h" -#include "gd32c10x_eval.h" #include "math.h" #include #include diff --git a/Robot_V2/device/Timer.cpp b/Robot_V2/device/Timer.cpp index 04e02d9..d485404 100644 --- a/Robot_V2/device/Timer.cpp +++ b/Robot_V2/device/Timer.cpp @@ -1,21 +1,17 @@ #include "Timer.h" -#include "Can.h" #include "DengFOC.h" -#include "Filter.h" #include "Timer.h" -#include "adc.h" -#include "ang_sensor.h" #include "gd32c10x.h" #include "gd32c10x_gpio.h" #include "gd32c10x_misc.h" #include "gd32c10x_rcu.h" #include "gd32c10x_timer.h" -#include "thermal.h" -#include +#include "initial.h" #include #include #include + constexpr int TIMER2_PRESCALER = 119; // 120MHz / 120 = 1MHz constexpr int TIMER2_PERIOD = 99; // 1MHz / 100 = 100us constexpr int TIMER3_PRESCALER = 119; // 120MHz / 120 = 1MHz @@ -29,18 +25,11 @@ constexpr int TIMER1_CH2_PULSE = 0; static u32 total_us100_counter = 0; // 总计数器，每100微秒增加1 extern u16 adc_value[7]; -extern CAN_transmiter CAN0_transmiter; +extern FOC_Controlroller FOC; +extern TaskScheduler scheduler; // 添加系统运行时间计数器 static volatile std::u64 system_runtime_us = 0; -// 声明外部FOC控制器对象 -extern FOC_Controlroller FOC; -// 声明外部采样器对象 -extern sampler sampler; -extern angle_sensor KTH7823; -extern EncoderVelocityKalman Vel_kalman; -extern ThermalController thermalController; // 热量控制器初始化 - /** * @brief 设置引脚中断优先级 * @return void @@ -167,53 +156,28 @@ void TIMER2_IRQHandler(void) { // 清除更新中断标志 timer_interrupt_flag_clear(TIMER2, TIMER_INT_UP); - // 增加总计数器 (使用更高效的位运算代替取模运算) - total_us100_counter++; - // system_runtime_us += 100; - // 每100us执行一次的操作 - sampler.update_current(); // 更新电流采样值 - KTH7823.update(); // 更新角度传感器值 - FOC.rt_velocity = Vel_kalman.update(KTH7823.getVelocity()); - FOC.rt_angle = KTH7823.getAngle(); - switch (FOC.control_mode) { - case 0: { - break; - } - case 1: { - FOC.angle_control(FOC.target_angle); // 角度闭环控制 - break; - } - case 2: { - FOC.velocity_control(FOC.target_velocity); // 速度闭环控制 - break; - } - case 3: { - FOC.angle_control_NPID(FOC.target_angle); // 角度闭环控制 - break; - } - case 4: { - FOC.velocity_open_loop(); // 速度闭环控制 - break; - } - } + total_us100_counter++; + scheduler.taskFlags |= + (1 << TASK_CURRENT_UPDATE) | (1 << TASK_ANGLE_UPDATE) | + (1 << TASK_VELOCITY_UPDATE) | (1 << TASK_MOTOR_CONTROL); + switch (total_us100_counter) { // 每1ms执行的代码，负载均衡 case 1: { - // 设置CAN发送标志 - CAN0_transmiter.can0_send_flag = SET; mode = FOC.control_mode; + scheduler.taskFlags |= (1 << TASK_MCU_TEMP_UPDATE); break; } case 2: { - sampler.update_mcu_temp(); + scheduler.taskFlags |= (1 << TASK_DRV_TEMP_UPDATE); break; } case 3: { - sampler.update_drv_temp(); + scheduler.taskFlags |= (1 << TASK_VBUS_UPDATE); break; } case 4: { - sampler.update_vbus_voltage(); + scheduler.taskFlags |= (1 << TASK_CAN_TRANSMIT); break; } } @@ -224,11 +188,10 @@ void TIMER2_IRQHandler(void) { ms10_counter++; switch (ms10_counter) { // 每10ms执行的代码，负载均衡 case 1: { - FOC.update_power(); + scheduler.taskFlags |= (1 << TASK_THERMAL_CONTROL); break; } case 2: { - thermalController.update(); break; } } @@ -239,6 +202,7 @@ void TIMER2_IRQHandler(void) { ms100_counter++; switch (ms100_counter) { // 每100ms执行的代码，负载均衡 case 1: { + break; } if (ms100_counter >= 10) { diff --git a/Robot_V2/device/ang_sensor.h b/Robot_V2/device/ang_sensor.h index 4752ed0..c29a23c 100644 --- a/Robot_V2/device/ang_sensor.h +++ b/Robot_V2/device/ang_sensor.h @@ -1,6 +1,5 @@ #ifndef _angsensor_H #define _angsensor_H -#include #ifdef __cplusplus extern "C" { #endif diff --git a/Robot_V2/main.cpp b/Robot_V2/main.cpp index 0b06313..80bf9bc 100644 --- a/Robot_V2/main.cpp +++ b/Robot_V2/main.cpp @@ -1,34 +1,97 @@ #include "main.h" #include "Can.h" #include "DengFOC.h" +#include "Filter.h" #include "Timer.h" #include "adc.h" #include "ang_sensor.h" -#include "thermal.h" #include "core_cm4.h" #include "initial.h" #include "systick.h" +#include "thermal.h" +TaskScheduler scheduler = {0}; // 全局变量定义 -CAN_transmiter CAN0_transmiter; // CAN初始化 -FOC_Controlroller FOC; // FOC控制初始化 -sampler sampler; // 采样器初始化 -angle_sensor KTH7823; // 角度传感器初始化 +CAN_transmiter CAN0_transmiter; // CAN初始化 +FOC_Controlroller FOC; // FOC控制初始化 +sampler sampler; // 采样器初始化 +angle_sensor KTH7823; // 角度传感器初始化 ThermalController thermalController; // 热量控制器初始化 +extern EncoderVelocityKalman Vel_kalman; int main(void) { - systick_config(); // 系统时钟初始化 - USART0_init(); // 调试串口初始化 - User_timer_init(); // Timer初始化 - driver_init(); // 电机驱动初始化 - watchdog_init(5000); // 看门狗初始化 + systick_config(); // 系统时钟初始化 + USART0_init(); // 调试串口初始化 + User_timer_init(); // Timer初始化 + driver_init(); // 电机驱动初始化 - angsens_MG_test(); // 角度传感器磁感应强度测试 + delay_1ms(10); + angsens_MG_test(); // 角度传感器磁感应强度测试 + watchdog_init(5000); // 看门狗初始化 while (1) { // delay_1ms(100); + fwdgt_counter_reload(); // 看门狗复位 // std::printf("1\n"); - fwdgt_counter_reload(); // 看门狗复位 - CAN0_transmiter.communication_check(); // CAN通信 + + // 保存当前任务标志快照以提高效率并确保一致性 + u32 currentTasks = scheduler.taskFlags; + + // 一次性清除所有已处理的任务标志 + scheduler.taskFlags &= ~currentTasks; + + // 按照优先级和重要性处理任务 + if (currentTasks & + (1 << TASK_MOTOR_CONTROL)) { // 电机控制任务(最高优先级) + switch (FOC.control_mode) { + case 0: { + break; + } + case 1: { + FOC.angle_control(FOC.target_angle); // 角度闭环控制 + break; + } + case 2: { + FOC.velocity_control(FOC.target_velocity); // 速度闭环控制 + break; + } + case 3: { + FOC.angle_control_NPID(FOC.target_angle); // 角度闭环控制 + break; + } + case 4: { + FOC.velocity_open_loop(); // 速度开环控制 + break; + } + } + } + if (currentTasks & (1 << TASK_CURRENT_UPDATE)) { // 更新电流采样值 + sampler.update_current(); + } + if (currentTasks & (1 << TASK_ANGLE_UPDATE)) { // 更新角度传感器值 + KTH7823.update(); + FOC.rt_angle = KTH7823.getAngle(); + } + if (currentTasks & (1 << TASK_VELOCITY_UPDATE)) { // 更新速度传感器值 + FOC.rt_velocity = Vel_kalman.update(KTH7823.getVelocity()); + } + if (currentTasks & (1 << TASK_VBUS_UPDATE)) { // 更新vbus电压值 + sampler.update_vbus_voltage(); + FOC.update_power(); + } + if (currentTasks & (1 << TASK_MCU_TEMP_UPDATE)) { // 更新mcu温度传感器值 + sampler.update_mcu_temp(); + } + if (currentTasks & + (1 << TASK_DRV_TEMP_UPDATE)) { // 更新电机温度传感器值 + sampler.update_drv_temp(); + } + if (currentTasks & (1 << TASK_THERMAL_CONTROL)) { // 热量控制 + CAN0_transmiter.testfloat_union.f = thermalController.update(); + } + if (currentTasks & (1 << TASK_CAN_TRANSMIT)) { // CAN通信 + CAN0_transmiter.can0_send_flag = SET; + CAN0_transmiter.communication_check(); + } } } \ No newline at end of file diff --git a/Robot_V2/sys/initial.h b/Robot_V2/sys/initial.h index 5f7b390..ac00eca 100644 --- a/Robot_V2/sys/initial.h +++ b/Robot_V2/sys/initial.h @@ -4,9 +4,10 @@ #ifdef __cplusplus extern "C" { #endif +#include "gd32c10x_eval.h" #include "stdint.h" #include -#include "gd32c10x_eval.h" + constexpr u16 REDUCTION_RATIO = 25; constexpr float DEG2RAD = 1.0f / 360.0f * 6.28318530718f; @@ -14,14 +15,31 @@ constexpr float RAD2DEG = 360.0f / 6.28318530718f; void rcu_config(void); static volatile uint32_t timer_delay_count = 0; // volatile 很重要！ -int fputc(int ch, FILE *f); // 声明fputc函数以重载标准库实现 +int fputc(int ch, FILE *f); // 声明fputc函数以重载标准库实现 u8 tle5210b_init(void); u8 USART0_init(void); u8 watchdog_init(u32 time); void driver_init(void); void angsens_MG_test(void); +// 任务标志定义 +typedef enum { + TASK_MOTOR_CONTROL = 0, + TASK_CURRENT_UPDATE, + TASK_VELOCITY_UPDATE, + TASK_ANGLE_UPDATE, + TASK_MCU_TEMP_UPDATE, + TASK_DRV_TEMP_UPDATE, + TASK_VBUS_UPDATE, + TASK_THERMAL_CONTROL, + TASK_CAN_TRANSMIT, + TASK_COUNT +} SystemTask; +// 任务管理结构 +typedef struct { + volatile u32 taskFlags; // 任务标志位(每个位代表一个任务) +} TaskScheduler; #ifdef __cplusplus } diff --git a/vcpkg-configuration.json b/vcpkg-configuration.json index 868a5ac..4024b20 100644 --- a/vcpkg-configuration.json +++ b/vcpkg-configuration.json @@ -1,16 +1,16 @@ { - "registries": [ - { - "name": "arm", - "kind": "artifact", - "location": "https://artifacts.tools.arm.com/vcpkg-registry" - } - ], - "requires": { - "arm:tools/open-cmsis-pack/cmsis-toolbox": "2.11.0", - "arm:tools/kitware/cmake": "3.31.5", - "arm:tools/ninja-build/ninja": "1.12.1", - "arm:tools/arm/mdk-toolbox": "1.1.0", - "arm:compilers/arm/armclang": "6.24.0" - } + "registries": [ + { + "name": "arm", + "kind": "artifact", + "location": "https://artifacts.tools.arm.com/vcpkg-registry" + } + ], + "requires": { + "arm:tools/open-cmsis-pack/cmsis-toolbox": "2.12.0", + "arm:tools/kitware/cmake": "3.31.5", + "arm:tools/ninja-build/ninja": "1.12.1", + "arm:tools/arm/mdk-toolbox": "1.1.0", + "arm:compilers/arm/armclang": "6.24.0" + } } \ No newline at end of file