liumin/test
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases 1 Wiki Activity

扭矩模式修改限制

Browse Source
This commit is contained in:
liumin
2025-07-25 21:35:22 +08:00
parent cdc62d856d
commit 242dcb6feb
1 changed files with 23 additions and 18 deletions
Download Patch File Download Diff File Expand all files Collapse all files

41
app/app_differential_drive.c
View File

@@ -427,10 +427,9 @@ void computeInverseKinematics(float linear_velocity_x, float yaw_rate, float max
float left_speed_mps = 0.0f; float left_speed_mps = 0.0f;
float right_speed_mps = 0.0f; float right_speed_mps = 0.0f;
float max_torque = diff_data.max_Torq; float max_torque = diff_data.max_Torq;//不需要限制，PID输出已经限制了
// linear_velocity_x = constrain(linear_velocity_x, -max_torque, max_torque);
linear_velocity_x = constrain(linear_velocity_x, -max_torque, max_torque); // yaw_rate = constrain(yaw_rate, -2*max_torque, 2*max_torque);
yaw_rate = constrain(yaw_rate, -2*max_torque, 2*max_torque);
left_speed_mps = linear_velocity_x + yaw_rate; left_speed_mps = linear_velocity_x + yaw_rate;
right_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[2] = left_speed_mps;
motor_speed[1] = right_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[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[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[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.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); // 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); 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_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 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_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 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; 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 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) 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 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[0] = (out_torque[0] + diff_data.left_diff_touue);//因为每一个电机都是相同的扭矩，所以扭矩和为2倍。
diff_data.out_torq[2] = (2*out_torque[0] - diff_data.left_diff_touue)/2.0f; diff_data.out_torq[2] = (out_torque[0] - diff_data.left_diff_touue);
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[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); // printf("output_speed: %f, output_yaw: %f, integral: %f\n", output_speed, output_yaw_rate,speed_pid.integral);