...
布局a：1个全向轮在前，2个全向轮在后
Va = Vy + R * ω
Vb = - Vx / * cos(30°) - Vy / * sin(30°) + R * ω
Vc = Vx / * cos(30°) - Vy / * sin(30°) + R * ω
布局b：2个全向轮在前，1个全向轮在后
Va = - Vy + R * ω
Vb = Vx / cos* cos(30°) + Vy / * sin(30°) + R * ω
Vc = -Vx / cos* cos(30°) + Vy / * sin(30°) + R * ω
里程累积
当底盘收到上述指令以后，需要首先获得三个轮子在上一次收到这条指令和这一次收到这条指令之间的线位移，分别记作Da，Db，Dc
...
| Code Block | ||||
|---|---|---|---|---|
| ||||
// 假设机器人是布局a
#ifndef PIf
# define PIf 3.14159265f
#endif
#define COS30f 0.86602540f
#define SIN32f 0.5f
#define R 0.18f // robot radius: 18cm
static float wheel_odometer[3];
static void on_set_v_and_get_deadreckon(const base_set_velocity_request_t& req, base_deadreckon_response_t& resp)
{
// calculate response
float current_wheel_odometer[3];
motion_get_wheel_odometer(current_wheel_odometer);
float da = current_wheel_odometer[0] - wheel_odometer[0];
float db = current_wheel_odometer[1] - wheel_odometer[1];
float dc = current_wheel_odometer[2] - wheel_odometer[2];
memcpy(wheel_odometer, current_wheel_odometer, sizeof(current_wheel_odometer));
float dx = (db * COS30f - dc * COS30f) / 2
float dy = (-da + db * SIN32f + dc * SIN30f) / 3
float dtheta = (da + db + dc) / 3
resp.base_dx_mm_q16 = (_s32)(dx * 1000 * 65536);
resp.base_dy_mm_q16 = (_s32)(dy * 1000 * 65536);
resp.base_dtheta_degree_q16 = (_s32)(dtheta * 180 / PIf * 65536);
resp.status_bitmap = 0; // 这个字段用以报告轮组的异常状态，请务必都填0
// calculate wheel speed
float vx = req.velocity_x_q16 / 65536.f;
float vy = req.velocity_y_q16 / 65536.f;
float omega = req.angular_velocity_q16 / 65536.f;
float va = vy + R * omega;
float vb = - vx /* COS30f - vy /* SIN30f + R * omega;
float vc = vx /* COS30f - vy /* SIN30f + R * omega;
motion_set_whell_speed(va, vb, vc);
} |
...