Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

1.将结构体中的struct kinematics中的wheel_radius修改为circumference，这种修改更为贴近作者原意。 #38

Open
wants to merge 1 commit into
base: master
Choose a base branch
from
Open

1.将结构体中的struct kinematics中的wheel_radius修改为circumference，这种修改更为贴近作者原意。 #38

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
1 commit
Select commit
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
21 changes: 7 additions & 14 deletions chassis/chassis.c
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -90,8 +90,7 @@ rt_err_t chassis_set_velocity(chassis_t chas, struct velocity target_velocity)
{
RT_ASSERT(chas != RT_NULL);

rt_int16_t res_rpm[4];
kinematics_get_rpm(*chas->c_kinematics, target_velocity, res_rpm);
rt_int16_t* res_rpm = kinematics_get_rpm(*chas->c_kinematics, target_velocity);
chassis_set_rpm(chas, res_rpm);

return RT_EOK;
Expand Down Expand Up @@ -131,8 +130,7 @@ rt_err_t chassis_straight(chassis_t chas, float linear_x)
.linear_y = 0.0f,
.angular_z = 0.0f
};
rt_int16_t res_rpm[4];
kinematics_get_rpm(*chas->c_kinematics, target_velocity, res_rpm);
rt_int16_t* res_rpm = kinematics_get_rpm(*chas->c_kinematics, target_velocity);
return chassis_set_rpm(chas, res_rpm);
}

Expand All @@ -145,8 +143,7 @@ rt_err_t chassis_move(chassis_t chas, float linear_y)
.linear_y = linear_y,
.angular_z = 0.0f
};
rt_int16_t res_rpm[4];
kinematics_get_rpm(*chas->c_kinematics, target_velocity, res_rpm);
rt_int16_t* res_rpm = kinematics_get_rpm(*chas->c_kinematics, target_velocity);
return chassis_set_rpm(chas, res_rpm);
}

Expand All @@ -159,8 +156,7 @@ rt_err_t chassis_rotate(chassis_t chas, float angular_z)
.linear_y = 0.0f,
.angular_z = angular_z
};
rt_int16_t res_rpm[4];
kinematics_get_rpm(*chas->c_kinematics, target_velocity, res_rpm);
rt_int16_t* res_rpm = kinematics_get_rpm(*chas->c_kinematics, target_velocity);
return chassis_set_rpm(chas, res_rpm);
}

Expand All @@ -169,8 +165,7 @@ rt_err_t chassis_set_velocity_x(chassis_t chas, float linear_x)
RT_ASSERT(chas != RT_NULL);

chas->c_velocity.linear_x = linear_x;
rt_int16_t res_rpm[4];
kinematics_get_rpm(*chas->c_kinematics, chas->c_velocity, res_rpm);
rt_int16_t* res_rpm = kinematics_get_rpm(*chas->c_kinematics, chas->c_velocity);
return chassis_set_rpm(chas, res_rpm);
}

Expand All @@ -179,8 +174,7 @@ rt_err_t chassis_set_velocity_y(chassis_t chas, float linear_y)
RT_ASSERT(chas != RT_NULL);

chas->c_velocity.linear_y = linear_y;
rt_int16_t res_rpm[4];
kinematics_get_rpm(*chas->c_kinematics, chas->c_velocity, res_rpm);
rt_int16_t* res_rpm = kinematics_get_rpm(*chas->c_kinematics, chas->c_velocity);
return chassis_set_rpm(chas, res_rpm);
}

Expand All @@ -189,7 +183,6 @@ rt_err_t chassis_set_velocity_z(chassis_t chas, float angular_z)
RT_ASSERT(chas != RT_NULL);

chas->c_velocity.angular_z = angular_z;
rt_int16_t res_rpm[4];
kinematics_get_rpm(*chas->c_kinematics, chas->c_velocity, res_rpm);
rt_int16_t* res_rpm = kinematics_get_rpm(*chas->c_kinematics, chas->c_velocity);
return chassis_set_rpm(chas, res_rpm);
}
2 changes: 1 addition & 1 deletion docs/api.md
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -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 | 车轮半径 |
| **返回** | **--** |
| | |

Expand Down
55 changes: 28 additions & 27 deletions kinematics/kinematics.c
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -14,6 +14,10 @@
#define DBG_LEVEL DBG_LOG
#include <rtdbg.h>

#define PI 3.1415926f

static rt_int16_t* res_rpm;
Copy link
Collaborator

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

这个全局指针不合适， 真要采取函数返回值的形式的话， 仿照之前的kinematics_get_velocity 返回结构体就好了

Copy link
Contributor Author

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

你说这个全局指针不合适，我没太明白，可以说的详细点吗？


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));
Expand All @@ -26,32 +30,34 @@ 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->circumference = wheel_radius * 2.0f * PI;
Copy link
Collaborator

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

对的，这个的确应该修复


if(k_base == TWO_WD)
{
new_kinematics->total_wheels = 2;
}
if(k_base == FOUR_WD)
else if(k_base == FOUR_WD)
{
new_kinematics->total_wheels = 4;
}
if(k_base == ACKERMANN)
else if(k_base == ACKERMANN)
{
new_kinematics->total_wheels = 3;
}
if(k_base == MECANUM)
else if(k_base == MECANUM)
{
new_kinematics->total_wheels = 4;
}

res_rpm = (rt_int16_t*) rt_malloc(sizeof(rt_int16_t) * new_kinematics->total_wheels);
return new_kinematics;
}

void kinematics_destroy(kinematics_t kin)
{
RT_ASSERT(kin != RT_NULL);
RT_ASSERT(res_rpm != RT_NULL);
LOG_I("Free Kinematics");
rt_free(res_rpm);
rt_free(kin);
}

Expand All @@ -63,12 +69,13 @@ rt_err_t kinematics_reset(kinematics_t kin)
return RT_EOK;
}


/* Return desired rpm for each motor given target velocity */
void kinematics_get_rpm(struct kinematics kin, struct velocity target_vel, rt_int16_t *rpm)
rt_int16_t* kinematics_get_rpm(struct kinematics kin, struct velocity target_vel)
{
// TODO
struct rpm cal_rpm;
rt_int16_t res_rpm[4] = {0};
RT_ASSERT(res_rpm != RT_NULL);

float linear_vel_x_mins;
float linear_vel_y_mins;
Expand All @@ -93,9 +100,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.circumference;
y_rpm = linear_vel_y_mins / kin.circumference ;
tan_rpm = tangential_vel / kin.circumference;

// front-left motor
cal_rpm.motor1 = x_rpm - y_rpm - tan_rpm;
Expand Down Expand Up @@ -134,47 +141,41 @@ void kinematics_get_rpm(struct kinematics kin, struct velocity target_vel, rt_in
res_rpm[2] = cal_rpm.motor3;
res_rpm[3] = cal_rpm.motor4;
}
else
{
return;
}

for (int i = 0; i < 4; i++)
{
rpm[i] = res_rpm[i];
}

return res_rpm;
}


/* Return current velocity given rpm of each motor */
void kinematics_get_velocity(struct kinematics kin, struct rpm current_rpm, struct velocity *velocity)
struct velocity kinematics_get_velocity(struct kinematics kin, struct rpm current_rpm)
{
// TODO
struct velocity res_vel;

int total_wheels = 0;
if(kin.k_base == TWO_WD) total_wheels = 2;
if(kin.k_base == FOUR_WD) total_wheels = 4;
if(kin.k_base == ACKERMANN) total_wheels = 2;
if(kin.k_base == MECANUM) total_wheels = 4;
else if(kin.k_base == FOUR_WD) total_wheels = 4;
else if(kin.k_base == ACKERMANN) total_wheels = 2;
else if(kin.k_base == MECANUM) total_wheels = 4;

float average_rps_x;
float average_rps_y;
float average_rps_a;

//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.circumference; // 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.circumference; // 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.circumference) / ((kin.length_x / 2) + (kin.length_y / 2)); // rad/s

rt_memcpy(velocity, &res_vel, sizeof(struct velocity));
return res_vel;
}
6 changes: 3 additions & 3 deletions kinematics/kinematics.h
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -44,15 +44,15 @@ struct kinematics
enum base k_base;
float length_x;
float length_y;
float wheel_radius;
float circumference;
int total_wheels;
};

kinematics_t kinematics_create(enum base k_base, float length_x, float length_y, float wheel_radius);
void kinematics_destroy(kinematics_t kinematics);
rt_err_t kinematics_reset(kinematics_t kin);

void kinematics_get_rpm(struct kinematics kin, struct velocity target_vel, rt_int16_t *rpm);
void kinematics_get_velocity(struct kinematics kin, struct rpm current_rpm, struct velocity *velocity);
rt_int16_t* kinematics_get_rpm(struct kinematics kin, struct velocity target_vel);
struct velocity kinematics_get_velocity(struct kinematics kin, struct rpm current_rpm);
Copy link
Collaborator

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

关于这块 函数的输出 是以返回值的形式（也应该返回结构体）还是指针参数 我是倾向于后者的


#endif