...
机器人往前方移动的速度为Vx，往左方平移的速度为Vy，逆时针旋转的角速度为ω。
同理，如果需要机器人向后移动，那么Vx为负值，顺时针旋转，则ω为负值。同理，如果需要机器人向后移动，那么Vx为负值；向右移动，Vy为负值；顺时针旋转，则ω为负值。
这些速度和角速度的计量单位均为公制单位，即分别为米每秒，和弧度每秒。
...
| Code Block | ||||
|---|---|---|---|---|
| ||||
base_set_velocity_request_t req; float vx = req.velocity_x_q16 / 65536.f; float vy = req.velocity_y_q16 / 65536.f; float omega = req.angular_velocity_q16 / 65536.f; |
而这个指令的响应报文如下：(由于历史原因，响应报文中dx ,dy的单位是mm, dtheta的单位是度)
| Code Block | ||||||||
|---|---|---|---|---|---|---|---|---|
| ||||||||
typedef struct _base_deadreckon_response
{
_s32 base_dx_mm_q16;
_s32 base_dy_mm_q16;
_s32 base_dtheta_degree_q16;
_u8 status_bitmap;
} __attribute__((packed)) base_deadreckon_response_t; |
...
| Code Block | ||||
|---|---|---|---|---|
| ||||
// 假设机器人是布局a #ifndef PIf # define PIf 3.14159265f #endif #define COS30f 0.86602540f #define SIN32fSIN30f 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 / SIN32fSIN30f + 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_whellwheel_speed(va, vb, vc); } |