diff --git a/Robot_V2/Algorithm/FOC.cpp b/Robot_V2/Algorithm/FOC.cpp index b6e06ae..58d262b 100644 --- a/Robot_V2/Algorithm/FOC.cpp +++ b/Robot_V2/Algorithm/FOC.cpp @@ -25,6 +25,7 @@ extern CurrentKalman Current_kalman; #define constrain(value, min, max, target_min, target_max) \ ((value - min) / (max - min) * (target_max - target_min)) +<<<<<<< HEAD // 校准步骤（代码模板） void calibrate_zero_position(void) { // Set_3Pwm(.25, 0, 0); // 起劲 @@ -49,14 +50,23 @@ void calibrate_zero_position(void) { // Set_3Pwm(0, 0, 0); // 停止 } +======= +>>>>>>> c9934849c58e5b5daedd91d8bfde3d56aa02f83f // 通过Ia,Ib,Ic计算Iq,Id(目前仅输出Iq) inline float FOC_Controller::cal_Iq_Id(float current_a, float current_b, float current_c) { // 计算电气角度 +<<<<<<< HEAD float angle_el = fmod(shaft_angle * PP - KTH7823.calibration_angle, 360.0f); +======= + float angle_el = fmod(shaft_angle * PP, 360.0f); +>>>>>>> c9934849c58e5b5daedd91d8bfde3d56aa02f83f // Clarke变换 (使用三相电流) +<<<<<<< HEAD float I_alpha, I_beta; +======= +>>>>>>> c9934849c58e5b5daedd91d8bfde3d56aa02f83f I_alpha = (2.0f / 3.0f) * current_a - (1.0f / 3.0f) * current_b - (1.0f / 3.0f) * current_c; I_beta = _1_SQRT3 * current_b - _1_SQRT3 * current_c; @@ -66,11 +76,17 @@ inline float FOC_Controller::cal_Iq_Id(float current_a, float current_b, arm_sin_cos_f32(angle_el, &sin_el, &cos_el); float I_q, I_d; I_q = I_beta * cos_el - I_alpha * sin_el; +<<<<<<< HEAD I_d = I_beta * sin_el + I_alpha * cos_el; // 返回Iq值，乘以1000以转换为mA return Current_kalman.update(I_q * 1000 - KE_FACTOR * rt_velocity); // return I_q * 1000 - KE_FACTOR * rt_velocity; +======= + + return I_q * 1000.0f - + KE_FACTOR * rt_velocity; // 返回Iq值，乘以1000以转换为mA +>>>>>>> c9934849c58e5b5daedd91d8bfde3d56aa02f83f } /** @@ -80,6 +96,7 @@ inline float FOC_Controller::cal_Iq_Id(float current_a, float current_b, */ u8 FOC_Controller::init() { motor_init(); // 电机驱动初始化 +<<<<<<< HEAD FOC.cur_loop_p = DEFAULT_current_loop_p * 100 / limit_voltage; FOC.cur_MIT_p = DEFAULT_current_MIT_p * 100 / limit_voltage; @@ -89,7 +106,22 @@ u8 FOC_Controller::init() { FOC.ang_loop_p = DEFAULT_angle_loop_p * 100 / limit_voltage; FOC.ang_angloop_p = DEFAULT_angle_angloop_p * 100 / limit_voltage; FOC.ang_MIT_p = DEFAULT_angle_MIT_p * 100 / limit_voltage; +======= + vel_loop.set_PID(DEFAULT_vel_loop_p * 100 / voltage_power_supply, vel_loop_Ki, + vel_loop_Kd); // 设置电机速度控制参数 + angle_loop.set_PID(DEFAULT_angle_loop_p * 100 / voltage_power_supply, 100.0f, + 0.2f); // 设置电机角度控制参数 + current_loop.set_PID(DEFAULT_current_loop_p * 100 / voltage_power_supply, 5e-7f, + 7e-8); // 设置电机电流控制参数 + vel_inVelAngloop.set_PID(DEFAULT_vel_angloop_p * 100, vel_angloop_Ki, + vel_angloop_Kd); // 设置电机速度控制参数 + ang_inVelAngloop.set_PID(DEFAULT_ang_angloop_p * 100 / voltage_power_supply, + ang_angloop_Ki, + ang_angloop_Kd); // 设置电机角度控制参数 +>>>>>>> c9934849c58e5b5daedd91d8bfde3d56aa02f83f + + CAN0_transmiter.testfloat_union.f = DEFAULT_ang_angloop_p * 100 / voltage_power_supply; return 0; } @@ -140,6 +172,107 @@ inline void FOC_Controller::setPhaseVoltage(float Uq, float Ud) { duty_b = Ub / voltage_power_supply * voltage_exchange; duty_c = Uc / voltage_power_supply * voltage_exchange; +<<<<<<< HEAD +======= + Set_3Pwm(duty_a, duty_b, duty_c); +} + +/** + * @brief 使用SVPWM算法设置电机输出（优化版） + * + * @param Uq: q轴电压 + * @param Ud: d轴电压 + */ +inline void FOC_Controller::setSVPWM_PhaseVoltage(float Uq, float Ud) { + // 计算电气角度（保持原有角度单位和范围） + float angle_el = fmod(shaft_angle * PP, 360.0f); + + // 帕克逆变换 + float sin_el, cos_el; + arm_sin_cos_f32(angle_el, &sin_el, &cos_el); + float Ualpha = Ud * cos_el - Uq * sin_el; + float Ubeta = Ud * sin_el + Uq * cos_el; + + // 计算角度并归一化到 [0, 2π) + float angle = atan2f(Ubeta, Ualpha); + if (angle < 0) + angle += 2.0f * PI; + + // 扇区与局部角度 + int sector = (int)(angle * INV_PI_3) + 1; + if (sector < 1) + sector = 1; + else if (sector > 6) + sector = 6; + float local_angle = angle - (sector - 1) * PI_3; + + // 模长计算 + float magnitude = sqrtf(Ualpha * Ualpha + Ubeta * Ubeta); + + // 计算电压调制比，同时限制最大输出电压 + float modulation_index = + (magnitude / voltage_power_supply); // 90%作为安全裕量 + + // 电压限制 + if (limit_voltage > voltage_power_supply) { + limit_voltage = voltage_power_supply; + } + float limit_percent = limit_voltage / v_safe; + if (modulation_index > limit_percent) { + // 限制电压 + modulation_index = limit_percent; + // 同时相应地调整magnitude，保持电压矢量方向不变，根据调整后的调制比反推magnitude + magnitude = voltage_power_supply * limit_percent; + } + + // 使用arm_sin_cos_f32计算local_angle的sin和cos + float sin_l, cos_l; + arm_sin_cos_f32(local_angle, &sin_l, &cos_l); + float T1 = modulation_index * (SQRT3DV2 * cos_l - COS_PI_3 * sin_l); + float T2 = modulation_index * sin_l; + float T0 = 1.0f - T1 - T2; + + // 计算占空比（按规范化时间映射） + switch (sector) { + case 1: + duty_a = 0.5f * T0 + T1 + T2; + duty_b = 0.5f * T0 + T2; + duty_c = 0.5f * T0; + break; + case 2: + duty_a = 0.5f * T0 + T1; + duty_b = 0.5f * T0 + T1 + T2; + duty_c = 0.5f * T0; + break; + case 3: + duty_a = 0.5f * T0; + duty_b = 0.5f * T0 + T1 + T2; + duty_c = 0.5f * T0 + T2; + break; + case 4: + duty_a = 0.5f * T0; + duty_b = 0.5f * T0 + T1; + duty_c = 0.5f * T0 + T1 + T2; + break; + case 5: + duty_a = 0.5f * T0 + T2; + duty_b = 0.5f * T0; + duty_c = 0.5f * T0 + T1 + T2; + break; + case 6: + duty_a = 0.5f * T0 + T1 + T2; + duty_b = 0.5f * T0; + duty_c = 0.5f * T0 + T1; + break; + default: + duty_a = 0.0f; + duty_b = 0.0f; + duty_c = 0.0f; + break; + } + + // 设置PWM输出 +>>>>>>> c9934849c58e5b5daedd91d8bfde3d56aa02f83f Set_3Pwm(duty_a, duty_b, duty_c); } @@ -249,7 +382,11 @@ inline void FOC_Controller::setSVPWM_PhaseVoltage(float Uq, float Ud) { * */ void FOC_Controller::shut_down() { +<<<<<<< HEAD drv_stop(); +======= + is_shut_down = 1; +>>>>>>> c9934849c58e5b5daedd91d8bfde3d56aa02f83f motor_sleep(); is_shut_down = true; } @@ -313,7 +450,11 @@ void FOC_Controller::update_power(float voltage) { voltage_error = false; voltage_power_supply = voltage; v_half = voltage * 0.5f; +<<<<<<< HEAD v_safe = voltage * 1.1f; +======= + v_safe = voltage * 1.05f; +>>>>>>> c9934849c58e5b5daedd91d8bfde3d56aa02f83f } else { voltage_error = true; } @@ -328,6 +469,7 @@ void FOC_Controller::update_power(float voltage) { float FOC_Controller::velocity_open_loop() { // 通过时间间隔和目标速度计算需要转动的机械角度，存储在shaft_angle中，在此之前，还需要对轴角度归一化 shaft_angle = fmod(shaft_angle + target_velocity * REDUCTION_RATIO * 1.0 / +<<<<<<< HEAD MOTOR_FREQUENCY / 2, 360.0f); // 获取电机轴角度 @@ -339,6 +481,19 @@ float FOC_Controller::velocity_open_loop() { // 因为在开环控制中，Ud始终为0，只控制Uq const float Uq = voltage_power_supply / 2.0f; setSVPWM_PhaseVoltage(Uq, 0.0f); +======= + TIMER2_FREQUENCY, + 360.0f); + + // 因为在开环控制中，Ud始终为0，只控制Uq + const float Uq = voltage_power_supply / 2.0f; + + setSVPWM_PhaseVoltage(Uq, 0.0f); + // CAN0_transmiter.testfloat_union.f = Uq; + // cal_Iq_Id(sampler.get_current('U'), sampler.get_current('V'), + // sampler.get_current('W')); + +>>>>>>> c9934849c58e5b5daedd91d8bfde3d56aa02f83f return limit_voltage; } @@ -358,6 +513,84 @@ float FOC_Controller::angle_control() { sampler.get_current('W')); // 设置相电压 setSVPWM_PhaseVoltage(Uq, 0.0f); +<<<<<<< HEAD +======= + return shaft_angle; +} + +/** + * @brief 带速度限制的角度闭环控制 + * + * @param target_angle: 目标角度 + * @param max_velocity: 最大速度限制 (单位: rad/s) + * @return float: 当前角度 + */ +float FOC_Controller::angle_control_with_velocity_limit() { + // 计算角度误差 + float angle_error = target_angle - rt_angle; + + // 使用角度PID控制器计算目标速度 + float target_velocity = ang_inVelAngloop(angle_error); + + // 使用速度PID控制器计算最终输出电压 + // 根据目标速度大小，选择不同的PID参数 + if (target_velocity < -120.0f) { + vel_inVelAngloop.set_P(vel_angloop_p * 2.0f); + } else if (target_velocity < -30.0f) { + vel_inVelAngloop.set_P(vel_angloop_p * 1.5f); + } else { + vel_inVelAngloop.set_P(vel_angloop_p); + } + float Uq = vel_inVelAngloop(rt_velocity - target_velocity); + + rt_current = cal_Iq_Id(sampler.get_current('U'), sampler.get_current('V'), + sampler.get_current('W')); + // 获取电机轴角度 + shaft_angle = KTH7823.getcurrent_sensorangle(); + + // 设置相电压 + setSVPWM_PhaseVoltage(Uq, 0.0f); + + return shaft_angle; +} + +/** + * @brief 带速度限制的角度闭环控制 + * + * @param target_angle: 目标角度 + * @param max_velocity: 最大速度限制 (单位: rad/s) + * @return float: 当前角度 + */ +float FOC_Controller::MIT_Mode() { + // 第一层：角度环（外环） + // 根据目标角度和实际角度的差值，通过角度环PID控制器计算目标速度 + float angle_error = rt_angle - target_angle; + float target_velocity = + ang_inVelAngloop(angle_error); // 使用angle_loop作为角度到速度的映射 + + // 第二层：速度环（中环） + // 根据目标速度和实际速度的差值，通过速度环PID控制器计算目标电流 + float velocity_error = target_velocity - rt_velocity; + float target_current = + vel_inVelAngloop(velocity_error); // 使用vel_loop作为速度到电流的映射 + + CAN0_transmiter.testfloat_union.f = target_current; + // 第三层：电流环（内环） + // 计算当前实际电流 + rt_current = cal_Iq_Id(sampler.get_current('U'), sampler.get_current('V'), + sampler.get_current('W')); + + // 通过电流环PID控制器计算输出电压 + float current_error = target_current - rt_current; + float Uq = current_loop(current_error); + + // 获取电机轴角度 + shaft_angle = KTH7823.getcurrent_sensorangle(); + + // 设置相电压 + setSVPWM_PhaseVoltage(Uq, 0.0f); + +>>>>>>> c9934849c58e5b5daedd91d8bfde3d56aa02f83f return shaft_angle; } diff --git a/Robot_V2/Algorithm/FOC.h b/Robot_V2/Algorithm/FOC.h index e2f525e..7b3cc2c 100644 --- a/Robot_V2/Algorithm/FOC.h +++ b/Robot_V2/Algorithm/FOC.h @@ -16,10 +16,12 @@ class FOC_Controller { u8 control_mode = 0; float limit_voltage = 0.0f; + float target_angle = 0.0f; float target_velocity = 0.0f; float target_current = 0.0f; + float rt_angle = 0.0f; float rt_velocity = 0.0f; float rt_current = 0.0f; @@ -100,7 +102,10 @@ class FOC_Controller { float Ualpha, Ubeta = 0; float zero_electric_angle = 0; float v_half = voltage_power_supply * 0.5f; // 电源电压的一半 + float startup_ramp_val = 0.0f; // 软启动系数 + float startup_ramp_step = 0.01f; // 启动斜坡步长，可调节以控制启动速度 void setPhaseVoltage(float Uq, float Ud); + void setSVPWM_PhaseVoltage(float Uq, float Ud); float cal_Iq_Id(float current_a, float current_b, float current_c); }; diff --git a/Robot_V2/Algorithm/pid.cpp b/Robot_V2/Algorithm/pid.cpp index 1bedb2e..25af6a3 100644 --- a/Robot_V2/Algorithm/pid.cpp +++ b/Robot_V2/Algorithm/pid.cpp @@ -68,23 +68,4 @@ void PIDController::set_PID(float P, float I, float D) { * @param P: P参数 * @return void */ -void PIDController::set_P(float pp) { this->P = pp; } - -/** - * @brief 设置输出限幅 - * - * @param limit: 输出限幅 - * @return void - */ -void PIDController::set_limit(float limit) { this->limit = limit; } - -/** - * @brief 重置PID控制器状态 - * - * @return void - */ -void PIDController::reset() { - error_prev = 0.0f; - output_prev = 0.0f; - integral_prev = 0.0f; -} +void PIDController::set_P_percent(float pp) { this->P = pp; } \ No newline at end of file diff --git a/Robot_V2/device/ADC.cpp b/Robot_V2/device/ADC.cpp index f980a31..907fb7a 100644 --- a/Robot_V2/device/ADC.cpp +++ b/Robot_V2/device/ADC.cpp @@ -15,8 +15,7 @@ constexpr float VCC_FACTOR = 11.0f; constexpr float CURRENT_FACTOR = 1.0f / 50.0f * 100.0f; // 50倍放大，100倍电流采样电阻, 单位mA extern CAN_transmiter CAN0_transmiter; -extern FOC_Controller FOC; - +extern FOC_Controlroller FOC; sampler sampler; // 采样器初始化 u16 adc_value[6] = {0}; // 显式初始化数组元素为0 /*! diff --git a/Robot_V2/device/Can.cpp b/Robot_V2/device/Can.cpp index 9f6a848..9367cf9 100644 --- a/Robot_V2/device/Can.cpp +++ b/Robot_V2/device/Can.cpp @@ -19,7 +19,9 @@ #include constexpr u16 CANX = CAN0; -extern FOC_Controller FOC; + +CAN_transmiter CAN0_transmiter; // CAN初始化 +extern FOC_Controlroller FOC; extern sampler sampler; extern angle_sensor KTH7823; extern EncoderAngleKalman Angle_kalman; // 角度滤波 @@ -573,47 +575,37 @@ void CAN_transmiter::communication_check() { // } if (current_message.rx_efid == CANFD_CMD_ID) { // 控制模式 - FOC.control_mode = current_message.rx_data[0]; - // 目标电压限制 + FOC.control_mode = receive_message.rx_data[0]; + // 目标角度 FOC.limit_voltage = - float(unpack_bytes_to_u16(current_message.rx_data, 1)) / 1000; - // 目标电流 - target_current_union.f = - unpack_bytes_to_float(current_message.rx_data, 3).f; - FOC.current_loop.set_limit(target_current_union.f); - FOC.vel_inMITloop.set_limit(target_current_union.f); - // 目标速度 - target_velocity_union.f = - unpack_bytes_to_float(current_message.rx_data, 7).f; - FOC.ang_inVelAngloop.set_limit(target_velocity_union.f); - FOC.ang_inMITloop.set_limit(target_velocity_union.f); + float(unpack_bytes_to_u16(receive_message.rx_data, 1)) / 1000; + // 目标电流&目标速度 + FOC.limit_velocity = + unpack_bytes_to_float(receive_message.rx_data, 3).f; + FOC.target_current = FOC.limit_velocity; // 目标角度 target_angle_union.f = - unpack_bytes_to_float(current_message.rx_data, 11).f; - // 赋值 + unpack_bytes_to_float(receive_message.rx_data, 7).f; FOC.target_angle = target_angle_union.f; + // 目标速度 + target_velocity_union.f = + unpack_bytes_to_float(receive_message.rx_data, 11).f; FOC.target_velocity = target_velocity_union.f; FOC.target_current = target_current_union.f; // 解析PID参数 float voltage = FOC.limit_voltage; - // PID参数 - u16 P_config = unpack_bytes_to_u16(current_message.rx_data, 15); - FOC.cur_loop_p = DEFAULT_current_loop_p * P_config / voltage; - FOC.cur_MIT_p = DEFAULT_current_MIT_p * P_config / voltage; - - P_config = unpack_bytes_to_u16(current_message.rx_data, 17); - FOC.vel_loop_p = DEFAULT_vel_loop_p * P_config / voltage; - FOC.vel_angloop_p = DEFAULT_vel_angloop_p * P_config; - FOC.vel_MIT_p = DEFAULT_vel_MIT_p * P_config; - - P_config = unpack_bytes_to_u16(current_message.rx_data, 19); - FOC.ang_loop_p = DEFAULT_angle_loop_p * P_config / voltage; - FOC.ang_angloop_p = DEFAULT_angle_angloop_p * P_config / voltage; - FOC.ang_MIT_p = DEFAULT_angle_MIT_p * P_config / voltage; - } else if (current_message.rx_efid == CANFD_RESET_ID) { - u8 first_byte = current_message.rx_data[0]; - u8 eigth_byte = current_message.rx_data[7]; + FOC.current_loop.set_P_percent(current_loop_p * P_config / voltage); + P_config = unpack_bytes_to_u16(receive_message.rx_data, 17); + FOC.vel_loop.set_P_percent(vel_loop_p * P_config / voltage); + P_config = unpack_bytes_to_u16(receive_message.rx_data, 19); + FOC.angle_loop.set_P_percent(angle_loop_p * P_config / voltage); + P_config = unpack_bytes_to_u16(receive_message.rx_data, 21); + FOC.vel_inVelAngloop.set_P_percent(vel_angloop_p * P_config / + voltage); + } else if (receive_message.rx_efid == CANFD_RESET_ID) { + u8 first_byte = receive_message.rx_data[0]; + u8 eigth_byte = receive_message.rx_data[7]; // 电机重置 if (first_byte & 0x01) { motor_reset(); diff --git a/Robot_V2/device/Timer.cpp b/Robot_V2/device/Timer.cpp index c5377d5..14ac98d 100644 --- a/Robot_V2/device/Timer.cpp +++ b/Robot_V2/device/Timer.cpp @@ -11,7 +11,7 @@ volatile u32 total_us100_counter = 0; // 总计数器，每 100 微秒增加 1 extern u16 adc_value[7]; -extern FOC_Controller FOC; +extern FOC_Controlroller FOC; extern TaskScheduler scheduler; extern CAN_transmiter CAN0_transmiter; // 添加系统运行时间计数器 @@ -131,49 +131,79 @@ void TIMER2_IRQHandler(void) { } timer_interrupt_flag_clear(TIMER2, TIMER_INT_UP); - // 固定任务：每 50us 执行电机控制 - scheduler.taskFlags |= (1u << TASK_ANGLE_UPDATE) | - (1u << TASK_VELOCITY_UPDATE) | - (1u << TASK_MOTOR_CONTROL); + // 每50us执行一次的操作 - 电机控制 + scheduler.taskFlags |= (1 << TASK_MOTOR_CONTROL); + scheduler.taskFlags |= + (1 << TASK_ANGLE_UPDATE) | (1 << TASK_VELOCITY_UPDATE); - // 每 100us 执行一次的任务（使用位运算替代布尔变量） - static u32 call_counter = 0; - call_counter++; - - if ((call_counter & 1u) == 0u) { // 偶数次调用（每 100us） - // 使用查找表设置 1ms 周期内的任务 - scheduler.taskFlags |= task_schedule_100us[total_us100_counter]; - - total_us100_counter++; - if (total_us100_counter >= 10u) { - total_us100_counter = 0; - - // 每 1ms 执行的任务 - static u32 ms1_counter = 0; - ms1_counter++; - if (ms1_counter >= 10u) { - ms1_counter = 0; - - // 每 10ms 执行的任务 - static u32 ms10_counter = 0; - ms10_counter++; - if (ms10_counter >= 10u) { - ms10_counter = 0; - - // 每 100ms 执行的任务 - static u32 ms100_counter = 0; - ms100_counter++; - if (ms100_counter >= 10u) { - ms100_counter = 0; - // 每 1s 任务预留位置 - } - scheduler.taskFlags |= task_schedule_100ms[ms100_counter]; - } - scheduler.taskFlags |= task_schedule_10ms[ms10_counter]; - } - scheduler.taskFlags |= task_schedule_1ms[ms1_counter]; - } - } + // 使用标志位控制，每100us执行一次其他任务 + static bool isEvenCall = false; + isEvenCall = !isEvenCall; + + if (isEvenCall) { + // 每100us执行一次的操作 - 保持原有频率 + + switch (total_us100_counter) { // 每1ms执行的代码，负载均衡 + case 0: { + scheduler.taskFlags |= (1 << TASK_MCU_TEMP_UPDATE); + break; + } + case 1: { + scheduler.taskFlags |= (1 << TASK_DRV_TEMP_UPDATE); + break; + } + case 2: { + scheduler.taskFlags |= (1 << TASK_VBUS_UPDATE); + break; + } + case 3: { + break; + } + case 9: { + scheduler.taskFlags |= (1 << TASK_CAN_TRANSMIT); + break; + } + } + + total_us100_counter++; + if (total_us100_counter > 9) { + total_us100_counter = 0; + // 可以添加每1ms执行的代码，无负载均衡 + static u8 ms1_counter = 0; + + switch (ms1_counter) { // 每10ms执行的代码，负载均衡 + case 0: { + scheduler.taskFlags |= (1 << TASK_THERMAL_CONTROL); + break; + } + case 1: { + scheduler.taskFlags |= (1 << TASK_MAGNETIC_CHECK); + break; + } + case 2: { + break; + } + } + ms1_counter++; + if (ms1_counter > 9) { + ms1_counter = 0; + // 可以添加每10ms执行的代码，无负载均衡 + static u8 ms10_counter = 0; + ms10_counter++; + if (ms10_counter > 9) { + ms10_counter = 0; + // 可以添加每100ms执行的代码 + static u8 sec_counter = 0; + sec_counter++; + if (sec_counter > 9) { + sec_counter = 0; + // 可以添加每1s执行的代码 + } + } + } + } + } + } } /** diff --git a/Robot_V2/main.cpp b/Robot_V2/main.cpp index c78d2d6..ab0a824 100644 --- a/Robot_V2/main.cpp +++ b/Robot_V2/main.cpp @@ -16,7 +16,7 @@ extern angle_sensor KTH7823; // 角度传感器 extern EncoderVelocityKalman Vel_kalman; // 速度滤波 extern EncoderAngleKalman Angle_kalman; // 角度滤波 extern ThermalController thermalController; // 热量控制器 -extern FOC_Controller FOC; // FOC控制 +extern FOC_Controller FOC; // FOC控制 extern FlashEEPROM flashEEP; // EEPROM类 extern VersionManager Version; @@ -95,35 +95,21 @@ void do_task(u32 currentTasks) { if (currentTasks & (1 << TASK_MOTOR_CONTROL)) { // 电机控制任务(最高优先级) motor_runtime_50us++; - // 静态变量用于跟踪上一次的控制模式 - static int last_control_mode = -1; + // 函数指针数组，包含各种控制模式对应的函数 + static FOCFunctionPtr control_functions[] = { + &FOC_Controlroller::drv_hold, // 0: 占空比置 + &FOC_Controlroller:: + angle_control_with_velocity_limit, // 1: + // 角度闭环控制，带速度限制 + &FOC_Controlroller::velocity_control, // 2: 速度闭环控制 + &FOC_Controlroller::current_control, // 3: 电流闭环控制 + &FOC_Controlroller::velocity_open_loop, // 4: 速度开环控制 + }; // 边界检查，防止数组越界 - if (FOC.control_mode >= 0 && - FOC.control_mode < 7) { // 更新为7个控制模式 - - // 检查控制模式是否发生变化 - if (FOC.control_mode != last_control_mode) { - // 重置所有PID控制器的状态，避免积分项累积 - FOC.vel_loop.reset(); - FOC.angle_loop.reset(); - FOC.current_loop.reset(); - FOC.vel_inVelAngloop.reset(); - FOC.ang_inVelAngloop.reset(); - FOC.ang_inMITloop.reset(); - FOC.vel_inMITloop.reset(); - FOC.current_inMITloop.reset(); - - // 更新上次控制模式 - last_control_mode = FOC.control_mode; - } - if (!FOC.is_shut_down) { - // 调用对应的控制函数 - (FOC.*control_functions[FOC.control_mode])(); - } else { - // 调用对应的控制函数 - FOC.drv_stop(); - } + if (FOC.control_mode >= 0 && FOC.control_mode < 5) { // 更新为7个控制模式 + // 调用对应的控制函数 + (FOC.*control_functions[FOC.control_mode])(); } } if (currentTasks & (1 << TASK_MCU_TEMP_UPDATE)) { // 更新mcu温度传感器值 diff --git a/Robot_V2/sys/config.h b/Robot_V2/sys/config.h index f8d0db3..8ef0f83 100644 --- a/Robot_V2/sys/config.h +++ b/Robot_V2/sys/config.h @@ -1,6 +1,5 @@ #ifndef __CONFIG_H #define __CONFIG_H -#include "gd32c10x_eval.h" #include // 如果使用CMSIS DSP库，需要定义ARM_MATH_CM4 @@ -88,50 +87,17 @@ constexpr float CURRENT_KALMAN_PROCESS_NOISE_Q = 10.0f; // 降低过程噪声 constexpr float CURRENT_KALMAN_MEASUREMENT_NOISE_R = 5e-5f; // 增大测量噪声 // PID配置 -constexpr float DEFAULT_current_loop_p = 0.1f / 100 * 12; // 电流P -constexpr float current_loop_Ki = 0.55f; // 电流P -constexpr float current_loop_Kd = 3e-4f; // 电流P - -constexpr float DEFAULT_vel_loop_p = 0.018f / 100 * 12; // 速度P -constexpr float vel_loop_Ku = 3.600000e-03 * 0.7 * 100 / 12; -constexpr float vel_loop_Tu = 17.0f * 1e-3f; -constexpr float vel_loop_Ki = 1.2 * vel_loop_Ku / vel_loop_Tu; -constexpr float vel_loop_Kd = 0.075 * vel_loop_Ku * vel_loop_Tu * 2; - -constexpr float DEFAULT_vel_angloop_p = 0.4f / 100; // 速度 速度-角度P -constexpr float vel_angloop_Ki = 5.0f; // 速度 速度-角度I -constexpr float vel_angloop_Kd = 5e-05f; // 速度 速度-角度D - -constexpr float DEFAULT_angle_angloop_p = 150.0f / 100 * 12; // 角度 速度-角度P -constexpr float ang_angloop_Ki = 0.00f; // 角度 速度-角度I -constexpr float ang_angloop_Kd = 8e-04f; // 角度 速度-角度D - -constexpr float DEFAULT_angle_loop_p = 60.0f / 100 * 12; // 角度P - -constexpr float DEFAULT_angle_MIT_p = 100.0f / 100 * 12; // 角度 MIT P -constexpr float angle_MIT_Ki = 0.0f; -constexpr float angle_MIT_Kd = 8e-04f; - -constexpr float DEFAULT_vel_MIT_p = 5.0f / 100; // 速度 MIT P -constexpr float vel_MIT_Ki = 15.0f; -constexpr float vel_MIT_Kd = 1e-04f; - -constexpr float DEFAULT_current_MIT_p = 0.2f / 100 * 12; // 电流 MIT P -constexpr float current_MIT_Ki = 0.1f; -constexpr float current_MIT_Kd = 3e-4f; +constexpr float current_loop_p = 6e-2f / 100 * 12; // 电流P +constexpr float vel_loop_p = 0.01f / 100 * 12; // 速度P +constexpr float angle_loop_p = 20.0f / 100 * 12; // 角度P +constexpr float vel_angloop_p = 0.12f / 100 * 12; // 速度 速度-角度P +constexpr float ang_angloop_p = 100.0f / 100 * 12; // 角度 速度-角度P // 常量定义 constexpr u8 SIZE_FLOAT = sizeof(float); constexpr u8 SIZE_U32 = sizeof(uint32_t); constexpr float ADC_MAX = 65536.0f; constexpr float PI = 3.14159265358979323846f; -constexpr float INV_PI_3 = 3.0f / PI; -constexpr float PI_3 = PI / 3.0f; -constexpr float _1_SQRT3 = 0.57735026919f; // 1/sqrt(3) -constexpr float _3PI_2 = 4.71238898038f; -constexpr float SQRT3 = 1.7320508075688772f; -constexpr float SQRT3DV2 = 0.8660254037844386f; // sqrt(3)/2 -constexpr float COS_PI_3 = 0.5f; constexpr float ANGLE_FACTOR = 25.0f / 180.0f * PI; constexpr float DEG2RAD = 1.0f / 180.0f * PI; constexpr float RAD2DEG = 180.0f / PI; // 修正RAD2DEG定义，应该是180/π @@ -183,12 +149,8 @@ constexpr float ANGLESENS_FACTOR = 1.0f / ADC_MAX * 360.0f; // TIMER2-任务调度 constexpr int TIMER2_PRESCALER = 119; // 120MHz / 120 = 1MHz -constexpr int TIMER2_PERIOD = 49; // 1 / 1MHz * 50 = 50us -constexpr int MOTOR_PERIOD = 2; // 100us/50us = 2 -constexpr float TIME_SLICE = - (TIMER2_PERIOD + 1) * (TIMER2_PRESCALER + 1) / 120000000.0f; // 周期 -constexpr float TIMER2_FREQUENCY = 1 / TIME_SLICE; // 频率 -constexpr float MOTOR_FREQUENCY = 1 / TIME_SLICE / MOTOR_PERIOD; // 频率 +constexpr int TIMER2_PERIOD = 49; // 1MHz / 50 = 50us +constexpr float TIMER2_FREQUENCY = 1 / 5e-5; // TIMER1-PWM constexpr int TIMER1_PRESCALER = 0; // 不分频，计数器时钟为120MHz