From b33574d392f376a2b2afcd99c2e789b2ce03af3a Mon Sep 17 00:00:00 2001 From: cbb <2625274797@qq.com> Date: Sat, 8 Nov 2025 21:04:35 +0800 Subject: [PATCH] =?UTF-8?q?fix(FOC):=20=E4=BF=AE=E5=A4=8D=E8=A7=92?= =?UTF-8?q?=E5=BA=A6=E6=8E=A7=E5=88=B6=E4=B8=AD=E7=9A=84=E5=86=97=E4=BD=99?= =?UTF-8?q?CAN=E4=BC=A0=E8=BE=93=E4=B8=8E=E4=BC=A0=E6=84=9F=E5=99=A8?= =?UTF-8?q?=E8=A7=92=E5=BA=A6=E8=8E=B7=E5=8F=96=E9=94=99=E8=AF=AF?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit - 修正速度控制中角度传感器数据读取错误，使用current_sensorangle替代getRawAngle - 在Timer中断中统一使用angle_control进行角度闭环控制 - 完善角度传感器类的角度读取逻辑，增加current_angle_deg和current_angle_rad成员变量 - 添加getcurrent_sensorangle接口用于获取当前原始角度值 --- Robot_V2/Algorithm/DengFOC.cpp | 6 +----- Robot_V2/Algorithm/pid.cpp | 17 +++++++++++------ Robot_V2/device/Timer.cpp | 2 +- Robot_V2/device/ang_sensor.cpp | 19 ++++++++++++++----- Robot_V2/device/ang_sensor.h | 5 ++++- 5 files changed, 31 insertions(+), 18 deletions(-) diff --git a/Robot_V2/Algorithm/DengFOC.cpp b/Robot_V2/Algorithm/DengFOC.cpp index 3602114..b1c0e1d 100644 --- a/Robot_V2/Algorithm/DengFOC.cpp +++ b/Robot_V2/Algorithm/DengFOC.cpp @@ -167,8 +167,6 @@ float FOC_Controlroller::angle_control(float target_angle) { shaft_angle = KTH7823.getRawAngle() * DEG2RAD; // PID 控制计算 float Uq = angle_loop(shaft_angle - target_angle * ANGLE_FACTOR); - CAN0_transmiter.testfloat_union.f = - angle_loop(target_angle * ANGLE_FACTOR - shaft_angle); // 设置相电压 setPhaseVoltage(Uq, 0.0f); return shaft_angle; @@ -200,11 +198,9 @@ float FOC_Controlroller::angle_control_NPID(float target_angle) { * @return float: 当前速度 */ float FOC_Controlroller::velocity_control(float target_velocity) { - shaft_angle = KTH7823.getRawAngle() * DEG2RAD; + shaft_angle = KTH7823.getcurrent_sensorangle() * DEG2RAD; // PID 控制计算 float Uq = vel_loop(rt_velocity - target_velocity); - - CAN0_transmiter.testfloat_union.f = Uq; // 设置相电压 setPhaseVoltage(Uq, 0.0f); return rt_velocity; diff --git a/Robot_V2/Algorithm/pid.cpp b/Robot_V2/Algorithm/pid.cpp index ef0d02c..b23ba74 100644 --- a/Robot_V2/Algorithm/pid.cpp +++ b/Robot_V2/Algorithm/pid.cpp @@ -12,22 +12,26 @@ PIDController::PIDController(float P, float I, float D, float ramp, float limit) , limit(limit) // PID控制器输出限幅 , - error_prev(0.0f), output_prev(0.0f), integral_prev(0.0f) {} + error_prev(0.0f), output_prev(0.0f), integral_prev(0.0f), timestamp_prev(0) {} // PID 控制器函数 float PIDController::operator()(float error) { // P环 float proportional = P * error; + // Tustin 散点积分（I环） - float integral = - integral_prev + I * TIME_SLICE * 0.5f * (error + error_prev); + // 添加积分限幅，防止积分饱和 + float integral = integral_prev + I * TIME_SLICE * 0.5f * (error + error_prev); integral = _constrain(integral, -limit, limit); + // D环（微分环节） float derivative = D * (error - error_prev) / TIME_SLICE; + // 将P,I,D三环的计算值加起来 float output = proportional + integral + derivative; output = _constrain(output, -limit, limit); + // 输出斜坡限制 if (output_ramp > 0) { // 对PID的变化速率进行限制 float output_rate = (output - output_prev) / TIME_SLICE; @@ -36,6 +40,7 @@ float PIDController::operator()(float error) { else if (output_rate < -output_ramp) output = output_prev - output_ramp * TIME_SLICE; } + // 保存值（为了下一次循环） integral_prev = integral; output_prev = output; @@ -53,7 +58,7 @@ float PIDController::operator()(float error) { * @return void */ void PIDController::set_PID(float P, float I, float D) { - P = P; - I = I; - D = D; + this->P = P; + this->I = I; + this->D = D; } \ No newline at end of file diff --git a/Robot_V2/device/Timer.cpp b/Robot_V2/device/Timer.cpp index 02189aa..4275496 100644 --- a/Robot_V2/device/Timer.cpp +++ b/Robot_V2/device/Timer.cpp @@ -175,7 +175,7 @@ void TIMER2_IRQHandler(void) { FOC.rt_velocity = Vel_kalman.update(KTH7823.getVelocity()); if (FOC.control_mode == 1) { - FOC.angle_control_NPID(FOC.target_angle); // 角度闭环控制 + FOC.angle_control(FOC.target_angle); // 角度闭环控制 } else if (FOC.control_mode == 2) { FOC.velocity_control(FOC.target_velocity);// 速度闭环控制 } else if (FOC.control_mode == 4) { diff --git a/Robot_V2/device/ang_sensor.cpp b/Robot_V2/device/ang_sensor.cpp index 487f443..cd9db6a 100644 --- a/Robot_V2/device/ang_sensor.cpp +++ b/Robot_V2/device/ang_sensor.cpp @@ -210,6 +210,8 @@ float angle_sensor::read_sensorAngle(void) { // 使用DMA发送命令并接收数据 if (!spi_rw_half_word_dma(SPI0, DEFAULT_COMMAND, &u16data)) { SPI_CS_DISABLE(); + current_angle_deg = u16data * ANGLE_FACTOR; + current_angle_rad = u16data * RAD_FACTOR; return u16data * ANGLE_FACTOR; } else { SPI_CS_DISABLE(); @@ -223,17 +225,16 @@ float angle_sensor::read_sensorAngle(void) { * @return 累积角度值 */ float angle_sensor::update(void) { - - float currentAngle = read_sensorAngle(); + read_sensorAngle(); if (m_firstUpdate) { - m_lastAngle = currentAngle; + m_lastAngle = current_angle_deg; m_firstUpdate = false; return m_cumulativeAngle; } // 计算角度变化 - float delta = currentAngle - m_lastAngle; + float delta = current_angle_deg - m_lastAngle; // 处理角度绕回情况（0度和360度之间的跳变） // 如果变化超过180度，说明发生了绕回 @@ -249,7 +250,7 @@ float angle_sensor::update(void) { velocity[VEL_FILTER_SIZE - 1] = delta * VELOCITY_SCALE; m_cumulativeAngle += delta; - m_lastAngle = currentAngle; + m_lastAngle = current_angle_deg; return m_cumulativeAngle; } @@ -288,6 +289,14 @@ float angle_sensor::getRawVelocity(void) const { return velocity_filted / (VEL_FILTER_SIZE - 1); // return velocity[VEL_FILTER_SIZE - 1]; } +/** + * @brief 获取角度值(原始级) + * @return 角度值(原始级) + */ +float angle_sensor::getcurrent_sensorangle(void) const { + return current_angle_deg; +} + /** * @brief 重置累积角度值 diff --git a/Robot_V2/device/ang_sensor.h b/Robot_V2/device/ang_sensor.h index f416bce..4752ed0 100644 --- a/Robot_V2/device/ang_sensor.h +++ b/Robot_V2/device/ang_sensor.h @@ -23,12 +23,15 @@ class angle_sensor { float getRawAngle(void) const; float getVelocity(void) const; float getRawVelocity(void) const; + float getcurrent_sensorangle(void) const; protected: void KTH7823_init(void); + float m_cumulativeAngle; + float current_angle_deg; + float current_angle_rad; bool m_firstUpdate; float m_lastAngle; - float m_cumulativeAngle; float m_lastcumulativeAngle; float velocity[VEL_FILTER_SIZE] = {0.0f}; };