1.将结构体中的 struct kinematics 中的 wheel_radius 修改为 wheel_cir 。并将kinematics_get_rpm 和 kinematics_get_velocity 函数中的 wheel_radius 替换为 wheel_cir 即可修正计算错误的问题。

3.api.md文档中wheel_radius的值实际作为半径计算的。

docs/api.md

@@ -237,7 +237,7 @@ kinematics_t    kinematics_create(enum base k_base, float length_x, float length
 | k_base  | 类型 |
 | length_x | X轴两轮间距 |
 | length_y | Y轴两轮间距 |
-| wheel_radius | 车轮直径 |
+| wheel_radius | 车轮半径 |
 | **返回** | **--**             |
 |   |  |
 

kinematics/kinematics.c

@@ -14,6 +14,8 @@
 #define DBG_LEVEL         DBG_LOG
 #include <rtdbg.h>
 
+#define PI 3.1415926f
+
 kinematics_t kinematics_create(enum base k_base, float length_x, float length_y, float wheel_radius)
 {
     kinematics_t new_kinematics = (kinematics_t)rt_malloc(sizeof(struct kinematics));
@@ -26,7 +28,7 @@ kinematics_t kinematics_create(enum base k_base, float length_x, float length_y,
     new_kinematics->k_base       = k_base;
     new_kinematics->length_x     = length_x;
     new_kinematics->length_y     = length_y;
-    new_kinematics->wheel_radius = wheel_radius;
+    new_kinematics->wheel_cir = wheel_radius * 2.0f * PI;;
 
     if(k_base == TWO_WD)
     {
@@ -93,9 +95,9 @@ void kinematics_get_rpm(struct kinematics kin, struct velocity target_vel, rt_in
 
     tangential_vel = angular_vel_z_mins * ((kin.length_x / 2) + (kin.length_y / 2));
 
-    x_rpm   = linear_vel_x_mins / kin.wheel_radius;
-    y_rpm   = linear_vel_y_mins / kin.wheel_radius;
-    tan_rpm = tangential_vel / kin.wheel_radius;
+    x_rpm   = linear_vel_x_mins / kin.wheel_cir;
+    y_rpm   = linear_vel_y_mins / kin.wheel_cir;
+    tan_rpm = tangential_vel / kin.wheel_cir;
 
     // front-left motor
     cal_rpm.motor1 = x_rpm - y_rpm - tan_rpm;
@@ -163,18 +165,18 @@ void kinematics_get_velocity(struct kinematics kin, struct rpm current_rpm, stru
 
     //convert average revolutions per minute to revolutions per second
     average_rps_x = ((float)(current_rpm.motor1 + current_rpm.motor2 + current_rpm.motor3 + current_rpm.motor4) / total_wheels) / 60; // RPM
-    res_vel.linear_x = average_rps_x * kin.wheel_radius; // m/s
+    res_vel.linear_x = average_rps_x * kin.wheel_cir; // m/s
 
     //convert average revolutions per minute in y axis to revolutions per second
     average_rps_y = ((float)(-current_rpm.motor1 + current_rpm.motor2 + current_rpm.motor3 - current_rpm.motor4) / total_wheels) / 60; // RPM
     if(kin.k_base == MECANUM)
-        res_vel.linear_y = average_rps_y * kin.wheel_radius; // m/s
+        res_vel.linear_y = average_rps_y * kin.wheel_cir; // m/s
     else
         res_vel.linear_y = 0;
 
     //convert average revolutions per minute to revolutions per second
     average_rps_a = ((float)(-current_rpm.motor1 + current_rpm.motor2 - current_rpm.motor3 + current_rpm.motor4) / total_wheels) / 60;
-    res_vel.angular_z =  (average_rps_a * kin.wheel_radius) / ((kin.length_x / 2) + (kin.length_y / 2)); //  rad/s
+    res_vel.angular_z =  (average_rps_a * kin.wheel_cir) / ((kin.length_x / 2) + (kin.length_y / 2)); //  rad/s
 
     rt_memcpy(velocity, &res_vel, sizeof(struct velocity));
 }

kinematics/kinematics.h

@@ -44,7 +44,7 @@ struct kinematics
     enum base   k_base;
     float       length_x;
     float       length_y;
-    float       wheel_radius;
+    float       wheel_cir;
     int         total_wheels;
 };