54 lines
1.5 KiB
C
54 lines
1.5 KiB
C
|
#include "by_motion.h"
|
||
|
|
|
||
|
|
#include <math.h>
|
||
|
|
#include <string.h>
|
||
|
|
|
||
|
|
#define D_X (0.18f) // 底盘 Y 轴上两轮中心的间距
|
||
|
|
#define D_Y (0.25f) // 底盘 X 轴上两轮中心的间距
|
||
|
|
#define RX_RY ((D_X + D_Y) / 2.f)
|
||
|
|
|
||
|
|
#define FP2U16_SCALE (1000.f) // 浮点转存为整数缩放尺度，所有设备该参数需对应
|
||
|
|
#define FP2U16(x) ((int16_t)(x * FP2U16_SCALE)) // 浮点转存为整数
|
||
|
|
|
||
|
|
/**********************************************
|
||
|
|
* v_1 = v_{ty} + v_{tx} - (r_x + r_y) * \omega
|
||
|
|
* v_2 = v_{ty} - v_{tx} + (r_x + r_y) * \omega
|
||
|
|
* v_3 = v_{ty} + v_{tx} + (r_x + r_y) * \omega
|
||
|
|
* v_4 = v_{ty} - v_{tx} - (r_x + r_y) * \omega
|
||
|
|
**********************************************/
|
||
|
|
|
||
|
|
/*** 各轮转速，左上角为 1 号，顺时针标号 ***/
|
||
|
|
float v_wheel[4] = {0.f};
|
||
|
|
|
||
|
|
/** 目标速度 **/
|
||
|
|
motion_speed_type motion_speed_struct;
|
||
|
|
|
||
|
|
/** 下发数据包 **/
|
||
|
|
int16_t motion_speed_data[4] = {0};
|
||
|
|
|
||
|
|
void by_motion_init(void)
|
||
|
|
{
|
||
|
|
memset(&motion_speed_struct, 0, sizeof(motion_speed_struct));
|
||
|
|
memset(v_wheel, 0, sizeof(v_wheel));
|
||
|
|
}
|
||
|
|
|
||
|
|
void by_motion_calc_speed(motion_speed_type *speed)
|
||
|
|
{
|
||
|
|
motion_speed_struct = *speed;
|
||
|
|
|
||
|
|
v_wheel[0] = speed->v_x + speed->v_y - RX_RY * speed->v_w;
|
||
|
|
v_wheel[1] = speed->v_x - speed->v_y + RX_RY * speed->v_w;
|
||
|
|
v_wheel[2] = speed->v_x + speed->v_y + RX_RY * speed->v_w;
|
||
|
|
v_wheel[3] = speed->v_x - speed->v_y - RX_RY * speed->v_w;
|
||
|
|
}
|
||
|
|
|
||
|
|
/**
|
||
|
|
* @brief
|
||
|
|
*
|
||
|
|
*/
|
||
|
|
void by_motion_pack_speed(void)
|
||
|
|
{
|
||
|
|
for (uint8_t i = 0; i < 4; i++) {
|
||
|
|
motion_speed_data[i] = FP2U16(v_wheel[i]);
|
||
|
|
}
|
||
|
|
}
|