From 242dcb6febe00512d4a6bbd06f7a1a594d96431f Mon Sep 17 00:00:00 2001 From: liumin Date: Fri, 25 Jul 2025 21:35:22 +0800 Subject: [PATCH] =?UTF-8?q?=E6=89=AD=E7=9F=A9=E6=A8=A1=E5=BC=8F=E4=BF=AE?= =?UTF-8?q?=E6=94=B9=E9=99=90=E5=88=B6?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- app/app_differential_drive.c | 41 ++++++++++++++++++++---------------- 1 file changed, 23 insertions(+), 18 deletions(-) diff --git a/app/app_differential_drive.c b/app/app_differential_drive.c index 209410e..b38f051 100644 --- a/app/app_differential_drive.c +++ b/app/app_differential_drive.c @@ -427,10 +427,9 @@ void computeInverseKinematics(float linear_velocity_x, float yaw_rate, float max float left_speed_mps = 0.0f; float right_speed_mps = 0.0f; - float max_torque = diff_data.max_Torq; - - linear_velocity_x = constrain(linear_velocity_x, -max_torque, max_torque); - yaw_rate = constrain(yaw_rate, -2*max_torque, 2*max_torque); + float max_torque = diff_data.max_Torq;//不需要限制，PID输出已经限制了 +// linear_velocity_x = constrain(linear_velocity_x, -max_torque, max_torque); +// yaw_rate = constrain(yaw_rate, -2*max_torque, 2*max_torque); left_speed_mps = linear_velocity_x + yaw_rate; right_speed_mps = linear_velocity_x - yaw_rate; @@ -467,12 +466,13 @@ void computeInverseKinematics(float linear_velocity_x, float yaw_rate, float max motor_speed[2] = left_speed_mps; motor_speed[1] = right_speed_mps; - motor_speed[3] = right_speed_mps; + motor_speed[3] = right_speed_mps; handleVehicleState(&diff_data.motor_state[0], diff_data.left_front_motor_speed, motor_speed[0]); //通过扭矩以及速度来判断挡位 handleVehicleState(&diff_data.motor_state[1], diff_data.right_front_motor_speed, motor_speed[1]); handleVehicleState(&diff_data.motor_state[2], diff_data.left_rear_motor_speed, motor_speed[2]); - handleVehicleState(&diff_data.motor_state[3], diff_data.right_rear_motor_speed, motor_speed[3]); + handleVehicleState(&diff_data.motor_state[3], diff_data.right_rear_motor_speed, motor_speed[3]); + // distributeTorque(diff_data.left_front_motor_speed,diff_data.left_rear_motor_speed,2*left_speed_mps,&motor_speed[0],&motor_speed[2],diff_data.max_Torq,diff_data.min_Torq); // distributeTorque(diff_data.right_front_motor_speed,diff_data.right_rear_motor_speed,2*right_speed_mps,&motor_speed[1],&motor_speed[3],diff_data.max_Torq,diff_data.min_Torq); @@ -622,7 +622,7 @@ static void diffProcess(void *signal_id) computeInverseKinematics(output_speed, output_yaw_rate, diff_data.max_speed, out_torque); - if( ( (diff_data.left_front_motor_speed / diff_data.left_rear_motor_speed) >= diff_data.diff_dead_zone ) || ( (diff_data.left_front_motor_speed / diff_data.left_rear_motor_speed) <= (1/diff_data.diff_dead_zone) ) )//如果超过2倍，或者小于2倍 + if( fabs(diff_data.left_front_motor_speed - diff_data.left_rear_motor_speed) >= diff_data.diff_dead_zone )//如果超过系数 { diff_data.left_speed_diff = diff_data.left_front_motor_speed - diff_data.left_rear_motor_speed; diff_data.left_diff_touue = calculatePidOutput(&Acc_front_speed_pid, 0.0f, diff_data.left_speed_diff, 0.0f, dt); //左侧转速差PID @@ -635,7 +635,7 @@ static void diffProcess(void *signal_id) } - if( ( (diff_data.right_front_motor_speed / diff_data.right_rear_motor_speed) >= diff_data.diff_dead_zone ) || ( (diff_data.right_front_motor_speed / diff_data.right_rear_motor_speed) <= (1/diff_data.diff_dead_zone) ) )//如果超过2倍，或者小于2倍 + if( fabs(diff_data.right_front_motor_speed - diff_data.right_rear_motor_speed) >= diff_data.diff_dead_zone )//如果超过系数 { diff_data.right_speed_diff = diff_data.right_front_motor_speed - diff_data.right_rear_motor_speed; diff_data.right_diff_touue = calculatePidOutput(&Dec_front_speed_pid, 0.0f, diff_data.right_speed_diff, 0.0f, dt); //左侧转速差PID @@ -647,32 +647,37 @@ static void diffProcess(void *signal_id) diff_data.right_diff_touue = 0; } - if(out_torque[0] > 0) + + if(out_torque[0] > 0)//根据大小来限定值为分配扭矩。最小就是0扭矩。 { - diff_data.left_diff_touue = constrain(diff_data.left_diff_touue, -2*out_torque[0], 2*out_torque[0]); + diff_data.left_diff_touue = constrain(diff_data.left_diff_touue, -out_torque[0], out_torque[0]); } else { - diff_data.left_diff_touue = constrain(diff_data.left_diff_touue, 2*out_torque[0], -2*out_torque[0]); + diff_data.left_diff_touue = constrain(diff_data.left_diff_touue, out_torque[0], -out_torque[0]); } if(out_torque[1] > 0) { - diff_data.right_diff_touue = constrain(diff_data.right_diff_touue, -2*out_torque[1], 2*out_torque[1]); + diff_data.right_diff_touue = constrain(diff_data.right_diff_touue, -out_torque[1], out_torque[1]); } else { - diff_data.right_diff_touue = constrain(diff_data.right_diff_touue, 2*out_torque[1], -2*out_torque[1]); + diff_data.right_diff_touue = constrain(diff_data.right_diff_touue, out_torque[1], -out_torque[1]); } - diff_data.out_torq[0] = (2*out_torque[0] + diff_data.left_diff_touue)/2.0f;//因为每一个电机都是相同的扭矩，所以扭矩和为2倍。 - diff_data.out_torq[2] = (2*out_torque[0] - diff_data.left_diff_touue)/2.0f; - - diff_data.out_torq[1] = (2*out_torque[1] + diff_data.right_diff_touue)/2.0f; - diff_data.out_torq[3] = (2*out_torque[1] - diff_data.right_diff_touue)/2.0f; + diff_data.out_torq[0] = (out_torque[0] + diff_data.left_diff_touue);//因为每一个电机都是相同的扭矩，所以扭矩和为2倍。 + diff_data.out_torq[2] = (out_torque[0] - diff_data.left_diff_touue); + diff_data.out_torq[1] = (out_torque[1] + diff_data.right_diff_touue); + diff_data.out_torq[3] = (out_torque[1] - diff_data.right_diff_touue); + + out_torque[0] = constrain(out_torque[0], -diff_data.max_Torq, diff_data.max_Torq); //限定最大扭矩 + out_torque[1] = constrain(out_torque[1], -diff_data.max_Torq, diff_data.max_Torq); + out_torque[2] = constrain(out_torque[2], -diff_data.max_Torq, diff_data.max_Torq); + out_torque[3] = constrain(out_torque[3], -diff_data.max_Torq, diff_data.max_Torq); // printf("output_speed: %f, output_yaw: %f, integral: %f\n", output_speed, output_yaw_rate,speed_pid.integral);